| 1 | #include "MCosy.h"
|
|---|
| 2 | #include "MCosy.h"
|
|---|
| 3 |
|
|---|
| 4 | #include <iomanip.h>
|
|---|
| 5 | #include <fstream.h>
|
|---|
| 6 | #include <iostream.h>
|
|---|
| 7 |
|
|---|
| 8 | #include <TROOT.h>
|
|---|
| 9 | #include <TEnv.h>
|
|---|
| 10 | #include <TSystem.h>
|
|---|
| 11 | #include <TApplication.h>
|
|---|
| 12 | #include <TTimer.h>
|
|---|
| 13 |
|
|---|
| 14 | #include <TH2.h>
|
|---|
| 15 | #include <TH3.h>
|
|---|
| 16 | #include <TProfile.h>
|
|---|
| 17 | #include <TCanvas.h>
|
|---|
| 18 |
|
|---|
| 19 | #include "MGCosy.h"
|
|---|
| 20 | #include "SlaStars.h"
|
|---|
| 21 |
|
|---|
| 22 | #include "slalib/slalib.h" // FIXME: REMOVE
|
|---|
| 23 |
|
|---|
| 24 | #include "macs.h"
|
|---|
| 25 | #include "base/timer.h"
|
|---|
| 26 | #include "shaftencoder.h"
|
|---|
| 27 |
|
|---|
| 28 | ClassImp(MCosy);
|
|---|
| 29 |
|
|---|
| 30 | typedef struct tm tm_t;
|
|---|
| 31 |
|
|---|
| 32 | /*
|
|---|
| 33 | #define GEAR_RATIO_ALT 2475.6 // [U_mot/U_tel(360deg)]
|
|---|
| 34 | #define GEAR_RATIO_AZ 5891.7 // [U_mot/U_tel(360deg)]
|
|---|
| 35 |
|
|---|
| 36 | #define RES_RE 500 // [re/U_mot]
|
|---|
| 37 | #define RES_SE 16384 // [se/U_tel(360deg)]
|
|---|
| 38 | */
|
|---|
| 39 | /*
|
|---|
| 40 | #define GEAR_RATIO_ALT (75.55*16384/1500) // 75.25 VERY IMPORTANT! unit=U_mot/U_tel
|
|---|
| 41 | #define GEAR_RATIO_AZ (179.8*16384/1500) // VERY IMPORTANT! unit=U_mot/U_tel
|
|---|
| 42 | */
|
|---|
| 43 |
|
|---|
| 44 | //const XY kGearRatio (GEAR_RATIO_ALT*RES_RE/RES_SE, GEAR_RATIO_AZ*RES_RE/RES_SE); //[re/se]
|
|---|
| 45 | //const XY kGearRatio2(GEAR_RATIO_ALT*RES_RE/360.0, GEAR_RATIO_AZ*RES_RE/360.0); //[re/deg]
|
|---|
| 46 |
|
|---|
| 47 | /* +===================================+
|
|---|
| 48 | FIXME: What if fMac3 (Sync) died?
|
|---|
| 49 | +===================================+
|
|---|
| 50 | */
|
|---|
| 51 |
|
|---|
| 52 | #define EXPERT
|
|---|
| 53 |
|
|---|
| 54 | double MCosy::Rad2SE(double rad) const
|
|---|
| 55 | {
|
|---|
| 56 | return 16384.0/k2Pi*rad;
|
|---|
| 57 | }
|
|---|
| 58 |
|
|---|
| 59 | double MCosy::Rad2ZdRE(double rad) const
|
|---|
| 60 | {
|
|---|
| 61 | return 16384.0/k2Pi*rad*kGearRatio.X();
|
|---|
| 62 | }
|
|---|
| 63 |
|
|---|
| 64 | double MCosy::Rad2AzRE(double rad) const
|
|---|
| 65 | {
|
|---|
| 66 | return 16384.0/k2Pi*rad*kGearRatio.Y();
|
|---|
| 67 | }
|
|---|
| 68 |
|
|---|
| 69 | double MCosy::Deg2ZdRE(double rad) const
|
|---|
| 70 | {
|
|---|
| 71 | return rad*kGearRatio2.X();
|
|---|
| 72 | }
|
|---|
| 73 |
|
|---|
| 74 | double MCosy::Deg2AzRE(double rad) const
|
|---|
| 75 | {
|
|---|
| 76 | return rad*kGearRatio2.Y();
|
|---|
| 77 | }
|
|---|
| 78 |
|
|---|
| 79 | /*
|
|---|
| 80 | ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
|
|---|
| 81 | {
|
|---|
| 82 | // CorrectTarget [se]
|
|---|
| 83 |
|
|---|
| 84 | // src [se]
|
|---|
| 85 | // dst [rad]
|
|---|
| 86 |
|
|---|
| 87 | // fAltMax = 70
|
|---|
| 88 | // fAltMin = -105/110
|
|---|
| 89 | // fAzMin = -355
|
|---|
| 90 | // fAzMax = 355
|
|---|
| 91 |
|
|---|
| 92 | ZdAz source = src * 360.0/16384.0;
|
|---|
| 93 | ZdAz dest = dst * kRad2Deg;
|
|---|
| 94 |
|
|---|
| 95 | if (dest.Zd()>-3 && dest.Zd()<3)
|
|---|
| 96 | dest.Zd(dest.Zd()<0?-3:3);
|
|---|
| 97 |
|
|---|
| 98 | if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
|
|---|
| 99 | return dst*(16384.0/k2Pi);
|
|---|
| 100 |
|
|---|
| 101 | const float fZdMin = -67;
|
|---|
| 102 | const float fZdMax = 67;
|
|---|
| 103 | const float fAzMin = -29;
|
|---|
| 104 | const float fAzMax = 423;
|
|---|
| 105 |
|
|---|
| 106 | //
|
|---|
| 107 | // This corrects to target for the shortest distance, not for the fastest move!
|
|---|
| 108 | //
|
|---|
| 109 | ZdAz s = source-dest;
|
|---|
| 110 |
|
|---|
| 111 | float min = s.Sqr();
|
|---|
| 112 |
|
|---|
| 113 | //
|
|---|
| 114 | // Is it enought to search inside one revolution?
|
|---|
| 115 | //
|
|---|
| 116 | ZdAz ret = dest;
|
|---|
| 117 |
|
|---|
| 118 | for (int i=-5; i<5+1; i++)
|
|---|
| 119 | {
|
|---|
| 120 | const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);
|
|---|
| 121 |
|
|---|
| 122 | //
|
|---|
| 123 | // Range Check
|
|---|
| 124 | //
|
|---|
| 125 | if (p.Zd()<fZdMin || p.Zd()>fZdMax)
|
|---|
| 126 | continue;
|
|---|
| 127 |
|
|---|
| 128 | if (p.Az()<fAzMin || p.Az()>fAzMax)
|
|---|
| 129 | continue;
|
|---|
| 130 |
|
|---|
| 131 | //
|
|---|
| 132 | // Calculate distance
|
|---|
| 133 | //
|
|---|
| 134 | s = source-p;
|
|---|
| 135 |
|
|---|
| 136 | const float dist = s.Sqr();
|
|---|
| 137 |
|
|---|
| 138 | if (dist > min)
|
|---|
| 139 | continue;
|
|---|
| 140 |
|
|---|
| 141 | //
|
|---|
| 142 | // New shortest distance
|
|---|
| 143 | //
|
|---|
| 144 | ret = p;
|
|---|
| 145 | min = dist;
|
|---|
| 146 | }
|
|---|
| 147 | return ret*(16384.0/360.0);
|
|---|
| 148 | }
|
|---|
| 149 | */
|
|---|
| 150 |
|
|---|
| 151 | // --------------------------------------------------------------------------
|
|---|
| 152 | //
|
|---|
| 153 | // GetSePos, reads the Shaftencoder positions from the Can-drivers
|
|---|
| 154 | // for the shaftencoders. The two shaft encoders at the elevation axis
|
|---|
| 155 | // are avaraged. The values are returned as a ZdAz object.
|
|---|
| 156 | //
|
|---|
| 157 | // If one of the two shaftencoders on the elevation axis is missing
|
|---|
| 158 | // the other one's position is returned.
|
|---|
| 159 | //
|
|---|
| 160 | // The positions are alway up-to-date because the shaftencoders are
|
|---|
| 161 | // sending all changes immediatly.
|
|---|
| 162 | //
|
|---|
| 163 | ZdAz MCosy::GetSePos() const
|
|---|
| 164 | {
|
|---|
| 165 | const int pa = fAz->GetPos();
|
|---|
| 166 | if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
|
|---|
| 167 | return ZdAz(0, pa);
|
|---|
| 168 |
|
|---|
| 169 | //
|
|---|
| 170 | // Get the values
|
|---|
| 171 | //
|
|---|
| 172 | int p1 = (fZd1->GetPos()+8192)%16384;
|
|---|
| 173 | int p2 = -(fZd2->GetPos()+8192)%16384;
|
|---|
| 174 |
|
|---|
| 175 | if (fZd1->IsZombieNode())
|
|---|
| 176 | return ZdAz(p2, pa);
|
|---|
| 177 | if (fZd2->IsZombieNode())
|
|---|
| 178 | return ZdAz(p1, pa);
|
|---|
| 179 |
|
|---|
| 180 | //
|
|---|
| 181 | // interpolate shaft encoder positions
|
|---|
| 182 | //
|
|---|
| 183 | float p = (float)(p1+p2)/2;
|
|---|
| 184 |
|
|---|
| 185 | return ZdAz(p, pa);
|
|---|
| 186 | }
|
|---|
| 187 |
|
|---|
| 188 | // --------------------------------------------------------------------------
|
|---|
| 189 | //
|
|---|
| 190 | // request the current positions from the rotary encoders.
|
|---|
| 191 | // use GetRePos to get the psotions. If the request fails the function
|
|---|
| 192 | // returns kFALSE, otherwise kTRUE
|
|---|
| 193 | //
|
|---|
| 194 | Bool_t MCosy::RequestRePos()
|
|---|
| 195 | {
|
|---|
| 196 | //
|
|---|
| 197 | // Send request
|
|---|
| 198 | //
|
|---|
| 199 | fMac2->RequestSDO(0x6004);
|
|---|
| 200 | fMac1->RequestSDO(0x6004);
|
|---|
| 201 |
|
|---|
| 202 | //
|
|---|
| 203 | // Wait until the objects are received.
|
|---|
| 204 | //
|
|---|
| 205 | fMac2->WaitForSdo(0x6004);
|
|---|
| 206 | fMac1->WaitForSdo(0x6004);
|
|---|
| 207 |
|
|---|
| 208 | //
|
|---|
| 209 | // If waiting was not interrupted everything is ok. return.
|
|---|
| 210 | //
|
|---|
| 211 | if (!(Break() || HasError() || HasZombie()))
|
|---|
| 212 | return kTRUE;
|
|---|
| 213 |
|
|---|
| 214 | //
|
|---|
| 215 | // If the waiting was interrupted due to a network error,
|
|---|
| 216 | // print some logging message.
|
|---|
| 217 | //
|
|---|
| 218 | if (HasError())
|
|---|
| 219 | lout << "Error while requesting re pos from Macs (SDO #6004)" << endl;
|
|---|
| 220 |
|
|---|
| 221 | return kFALSE;
|
|---|
| 222 | }
|
|---|
| 223 |
|
|---|
| 224 | // --------------------------------------------------------------------------
|
|---|
| 225 | //
|
|---|
| 226 | // reads the Rotary encoder positions from the last request of the Macs.
|
|---|
| 227 | //
|
|---|
| 228 | // The positions are returned as a ZdAz object. Use RequestRePos to request
|
|---|
| 229 | // the current positions first.
|
|---|
| 230 | //
|
|---|
| 231 | ZdAz MCosy::GetRePos()
|
|---|
| 232 | {
|
|---|
| 233 | return ZdAz(fMac2->GetPos(), fMac1->GetPos());
|
|---|
| 234 | }
|
|---|
| 235 |
|
|---|
| 236 | // --------------------------------------------------------------------------
|
|---|
| 237 | //
|
|---|
| 238 | // reads the Rotary encoder positions from the Macs.
|
|---|
| 239 | //
|
|---|
| 240 | // The positions are returned as a ZdAz object. The positions are the ones
|
|---|
| 241 | // which are send as PDOs to the computer. This is done at a given
|
|---|
| 242 | // frequency. Which means, that this positions are not ought to be
|
|---|
| 243 | // up-to-date.
|
|---|
| 244 | //
|
|---|
| 245 | ZdAz MCosy::GetRePosPdo()
|
|---|
| 246 | {
|
|---|
| 247 | return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
|
|---|
| 248 | }
|
|---|
| 249 |
|
|---|
| 250 |
|
|---|
| 251 |
|
|---|
| 252 | // --------------------------------------------------------------------------
|
|---|
| 253 | //
|
|---|
| 254 | // set the velocity and accelerations for position maneuvers.
|
|---|
| 255 | //
|
|---|
| 256 | // The acceleratin is set as given (in percent of maximum).
|
|---|
| 257 | // The velocity is given in percent, depending on the ratio (<1 or >1)
|
|---|
| 258 | // one of the axis becomes a slower velocity. This is used for maneuvers
|
|---|
| 259 | // in which both axis are moved synchromously and should reach their
|
|---|
| 260 | // target position at the same time.
|
|---|
| 261 | //
|
|---|
| 262 | void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel)
|
|---|
| 263 | {
|
|---|
| 264 | //
|
|---|
| 265 | // Set velocities
|
|---|
| 266 | //
|
|---|
| 267 | const int vr = fMac1->GetVelRes();
|
|---|
| 268 |
|
|---|
| 269 | vel *= vr;
|
|---|
| 270 |
|
|---|
| 271 | if (ratio <1)
|
|---|
| 272 | {
|
|---|
| 273 | fMac1->SetVelocity(vel);
|
|---|
| 274 | fMac2->SetVelocity(vel*ratio);
|
|---|
| 275 | }
|
|---|
| 276 | else
|
|---|
| 277 | {
|
|---|
| 278 | fMac1->SetVelocity(vel/ratio);
|
|---|
| 279 | fMac2->SetVelocity(vel);
|
|---|
| 280 | }
|
|---|
| 281 | }
|
|---|
| 282 |
|
|---|
| 283 | // --------------------------------------------------------------------------
|
|---|
| 284 | //
|
|---|
| 285 | // Does a relative positioning.
|
|---|
| 286 | //
|
|---|
| 287 | // The steps to move are given in a ZdAz object relative to the current
|
|---|
| 288 | // position. The coordinates are given in Roteryencoder steps.
|
|---|
| 289 | // Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only
|
|---|
| 290 | // if Axis 2==kTRUE. The function waits for the movement to be finished.
|
|---|
| 291 | //
|
|---|
| 292 | void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
|
|---|
| 293 | {
|
|---|
| 294 | if (HasZombie())
|
|---|
| 295 | return;
|
|---|
| 296 |
|
|---|
| 297 | SetStatus(MCosy::kMoving);
|
|---|
| 298 |
|
|---|
| 299 | if (axe1) fMac2->StartRelPos(rd.Zd());
|
|---|
| 300 | if (axe2) fMac1->StartRelPos(rd.Az());
|
|---|
| 301 | #ifdef EXPERT
|
|---|
| 302 | cout << "Waiting for positioning..." << flush;
|
|---|
| 303 | #endif
|
|---|
| 304 | if (axe1) fMac2->WaitForSdo(0x6004, 1);
|
|---|
| 305 | if (axe2) fMac1->WaitForSdo(0x6004, 1);
|
|---|
| 306 |
|
|---|
| 307 | WaitForEndMovement();
|
|---|
| 308 | #ifdef EXPERT
|
|---|
| 309 | cout << "done." << endl;
|
|---|
| 310 | #endif
|
|---|
| 311 | }
|
|---|
| 312 |
|
|---|
| 313 | // --------------------------------------------------------------------------
|
|---|
| 314 | //
|
|---|
| 315 | // check for a break-signal (from the msgqueue) and errors.
|
|---|
| 316 | //
|
|---|
| 317 | int MCosy::StopWaitingForSDO() const
|
|---|
| 318 | {
|
|---|
| 319 | return 0/*Break() || HasError()*/;
|
|---|
| 320 | }
|
|---|
| 321 |
|
|---|
| 322 | // --------------------------------------------------------------------------
|
|---|
| 323 | //
|
|---|
| 324 | // Waits for a movement to become finished.
|
|---|
| 325 | //
|
|---|
| 326 | // First waits for all peding Sdos, then waits until both motors are stopped
|
|---|
| 327 | // or waiting for SDOs was stopped (either by an error or by Break)
|
|---|
| 328 | //
|
|---|
| 329 | void MCosy::WaitForEndMovement()
|
|---|
| 330 | {
|
|---|
| 331 | // FIXME, what when waiting times out (Zombie)
|
|---|
| 332 |
|
|---|
| 333 | while ((fMac1->IsPositioning() || fMac2->IsPositioning()) &&
|
|---|
| 334 | !(Break() || HasError() || HasZombie()))
|
|---|
| 335 | usleep(1);
|
|---|
| 336 |
|
|---|
| 337 | if (Break() || HasError() || HasZombie())
|
|---|
| 338 | {
|
|---|
| 339 | lout << "WaitForEndMovement aborted... ";
|
|---|
| 340 | if (Break())
|
|---|
| 341 | lout << "Break signal." << endl;
|
|---|
| 342 | if (HasError())
|
|---|
| 343 | lout << "Network has error." << endl;
|
|---|
| 344 | if (HasZombie())
|
|---|
| 345 | lout << "Network has zombie." << endl;
|
|---|
| 346 | }
|
|---|
| 347 | }
|
|---|
| 348 |
|
|---|
| 349 | // --------------------------------------------------------------------------
|
|---|
| 350 | //
|
|---|
| 351 | // Check for an error...
|
|---|
| 352 | //
|
|---|
| 353 | // This is ment for usage after the Action: All Motors Stop.
|
|---|
| 354 | //
|
|---|
| 355 | void MCosy::CheckForError()
|
|---|
| 356 | {
|
|---|
| 357 | //
|
|---|
| 358 | // Check all Can-Nodes for an Error. If there is no error the motor
|
|---|
| 359 | // status is set to stopped.
|
|---|
| 360 | //
|
|---|
| 361 | if (!HasError())
|
|---|
| 362 | {
|
|---|
| 363 | SetStatus(MCosy::kStopped);
|
|---|
| 364 | return;
|
|---|
| 365 | }
|
|---|
| 366 |
|
|---|
| 367 | //
|
|---|
| 368 | // If there is an error, the error status is set to Error.
|
|---|
| 369 | //
|
|---|
| 370 | SetStatus(MCosy::kError);
|
|---|
| 371 |
|
|---|
| 372 | /*
|
|---|
| 373 | FIXME: HANDLINGE ERROR
|
|---|
| 374 |
|
|---|
| 375 | //
|
|---|
| 376 | // Now try to handle the error.
|
|---|
| 377 | //
|
|---|
| 378 | fMac1->HandleError();
|
|---|
| 379 | fMac2->HandleError();
|
|---|
| 380 |
|
|---|
| 381 | //
|
|---|
| 382 | // If the error couldn't get solved return
|
|---|
| 383 | //
|
|---|
| 384 | if (HasError())
|
|---|
| 385 | return;
|
|---|
| 386 |
|
|---|
| 387 | //
|
|---|
| 388 | // Set motor status to stopped
|
|---|
| 389 | //
|
|---|
| 390 | SetStatus(MCosy::kStopped);
|
|---|
| 391 | */
|
|---|
| 392 | }
|
|---|
| 393 |
|
|---|
| 394 | Bool_t MCosy::CheckRange(const ZdAz &d) const
|
|---|
| 395 | {
|
|---|
| 396 | // d [deg]
|
|---|
| 397 |
|
|---|
| 398 | if (d.Zd()<fMin.Zd() || d.Zd()>fMax.Zd())
|
|---|
| 399 | {
|
|---|
| 400 | lout << "ERROR: Requested Zenith Angle (" << d.Zd() << "deg, Az=";
|
|---|
| 401 | lout << d.Az() << ") not inside allowed range." << endl;
|
|---|
| 402 | return kFALSE;
|
|---|
| 403 | }
|
|---|
| 404 |
|
|---|
| 405 | if (d.Az()<fMin.Az() || d.Az()>fMax.Az())
|
|---|
| 406 | {
|
|---|
| 407 | lout << "ERROR: Requested Azimuth Angle (" << d.Az() << "deg, Zd=";
|
|---|
| 408 | lout << d.Zd() << ") not inside allowed range." << endl;
|
|---|
| 409 | return kFALSE;
|
|---|
| 410 | }
|
|---|
| 411 |
|
|---|
| 412 | return kTRUE;
|
|---|
| 413 | }
|
|---|
| 414 |
|
|---|
| 415 | // --------------------------------------------------------------------------
|
|---|
| 416 | //
|
|---|
| 417 | // Move the telescope to the given position. The position must be given in
|
|---|
| 418 | // a ZdAz object in rad.
|
|---|
| 419 | //
|
|---|
| 420 | // The first positioning is done absolutely. If we didn't reach the
|
|---|
| 421 | // correct psotion we try to correct for this by 10 relative position
|
|---|
| 422 | // maneuvers. If this doesn't help positioning failed.
|
|---|
| 423 | //
|
|---|
| 424 | // As a reference the shaftencoder values are used.
|
|---|
| 425 | //
|
|---|
| 426 | int MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
|
|---|
| 427 | {
|
|---|
| 428 | const ZdAz d = dst*kRad2Deg;
|
|---|
| 429 |
|
|---|
| 430 | lout << "Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
|
|---|
| 431 |
|
|---|
| 432 | if (!CheckRange(d))
|
|---|
| 433 | return kFALSE;
|
|---|
| 434 |
|
|---|
| 435 | //
|
|---|
| 436 | // Calculate new target position (shortest distance to go)
|
|---|
| 437 | //
|
|---|
| 438 | const ZdAz src = GetSePos(); // [se]
|
|---|
| 439 |
|
|---|
| 440 | //
|
|---|
| 441 | // Make sure that the motors are in sync mode (necessary if the
|
|---|
| 442 | // MACS has been rebooted from a Zombie state.
|
|---|
| 443 | //
|
|---|
| 444 | //InitSync();
|
|---|
| 445 | //if (fMac3->IsZombieNode())
|
|---|
| 446 | // return false;
|
|---|
| 447 |
|
|---|
| 448 | //
|
|---|
| 449 | // Because we agreed on I don't search for the shortest move
|
|---|
| 450 | // anymore
|
|---|
| 451 | //
|
|---|
| 452 | // const ZdAz dest = CorrectTarget(src, dst);
|
|---|
| 453 | //
|
|---|
| 454 | const ZdAz dest = fBending(dst)*16384/2/TMath::Pi(); // [se]
|
|---|
| 455 | fZdAzSoll = dst;
|
|---|
| 456 |
|
|---|
| 457 | cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl;
|
|---|
| 458 | cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl;
|
|---|
| 459 | cout << "Bend'd Dest Zd: " << Rad2SE(fZdAzSoll.Zd()) << "se Az:" << Rad2SE(fZdAzSoll.Az()) << "se" << endl;
|
|---|
| 460 | cout << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl;
|
|---|
| 461 |
|
|---|
| 462 | //
|
|---|
| 463 | // Set velocities
|
|---|
| 464 | //
|
|---|
| 465 | const int vr = fMac1->GetVelRes();
|
|---|
| 466 |
|
|---|
| 467 | int i;
|
|---|
| 468 | for (i=0; i<10 && !(Break() || HasError() || HasZombie()); i++)
|
|---|
| 469 | {
|
|---|
| 470 |
|
|---|
| 471 | lout << "Step #" << i << endl;
|
|---|
| 472 | //
|
|---|
| 473 | // Get Shaft Encoder Positions
|
|---|
| 474 | //
|
|---|
| 475 | const ZdAz p=GetSePos();
|
|---|
| 476 |
|
|---|
| 477 | //
|
|---|
| 478 | // calculate control deviation and rounded cd
|
|---|
| 479 | //
|
|---|
| 480 | ZdAz rd = dest-p; // [se]
|
|---|
| 481 |
|
|---|
| 482 | // ===========================================
|
|---|
| 483 | const ZdAz ist = dst-rd*TMath::Pi()/8192;
|
|---|
| 484 |
|
|---|
| 485 | const double p1 = ist.Zd()-19.0605/kRad2Deg;
|
|---|
| 486 | const double p2 = dst.Zd()-19.0605/kRad2Deg;
|
|---|
| 487 |
|
|---|
| 488 | const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*8192/TMath::Pi();
|
|---|
| 489 | const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi();
|
|---|
| 490 | // ===========================================
|
|---|
| 491 |
|
|---|
| 492 | ZdAz cd = rd; // [se]
|
|---|
| 493 | cd.Round();
|
|---|
| 494 |
|
|---|
| 495 | //
|
|---|
| 496 | // Check if there is a control deviation on the axis
|
|---|
| 497 | //
|
|---|
| 498 | const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
|
|---|
| 499 | const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
|
|---|
| 500 |
|
|---|
| 501 | //
|
|---|
| 502 | // check if we reached the correct position already
|
|---|
| 503 | //
|
|---|
| 504 | if (!cdzd && !cdaz)
|
|---|
| 505 | {
|
|---|
| 506 | lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl;
|
|---|
| 507 | SetStatus(MCosy::kStopped);
|
|---|
| 508 | return TRUE;
|
|---|
| 509 | }
|
|---|
| 510 |
|
|---|
| 511 | //
|
|---|
| 512 | // change units from se to re
|
|---|
| 513 | //
|
|---|
| 514 | rd *= kGearRatio; // [re]
|
|---|
| 515 | rd.Zd(f2-f1);
|
|---|
| 516 |
|
|---|
| 517 | //
|
|---|
| 518 | // Initialize Velocities so that we reach both positions
|
|---|
| 519 | // at the same time
|
|---|
| 520 | //
|
|---|
| 521 | if (i)
|
|---|
| 522 | {
|
|---|
| 523 | fMac1->SetAcceleration(0.1*vr);
|
|---|
| 524 | fMac2->SetAcceleration(0.1*vr);
|
|---|
| 525 |
|
|---|
| 526 | fMac1->SetDeceleration(0.1*vr);
|
|---|
| 527 | fMac2->SetDeceleration(0.1*vr);
|
|---|
| 528 |
|
|---|
| 529 | SetPosVelocity(1.0, 0.05);
|
|---|
| 530 | }
|
|---|
| 531 | else
|
|---|
| 532 | {
|
|---|
| 533 | if (rd.Az()>-15*kGearRatio.Y() && rd.Az()<15*kGearRatio.Y())
|
|---|
| 534 | {
|
|---|
| 535 | #ifdef EXPERT
|
|---|
| 536 | cout << " -------------- LO ---------------- " << endl;
|
|---|
| 537 | #endif
|
|---|
| 538 | fMac1->SetAcceleration(0.05*vr);
|
|---|
| 539 | fMac1->SetDeceleration(0.05*vr);
|
|---|
| 540 | }
|
|---|
| 541 | else
|
|---|
| 542 | {
|
|---|
| 543 | #ifdef EXPERT
|
|---|
| 544 | cout << " -------------- HI ---------------- " << endl;
|
|---|
| 545 | fMac1->SetAcceleration(0.4*vr);// 0.4
|
|---|
| 546 | fMac1->SetDeceleration(0.4*vr);// 0.4
|
|---|
| 547 | #else
|
|---|
| 548 | fMac1->SetAcceleration(0.2*vr);
|
|---|
| 549 | fMac1->SetDeceleration(0.1*vr);
|
|---|
| 550 | #endif
|
|---|
| 551 | }
|
|---|
| 552 |
|
|---|
| 553 | #ifdef EXPERT
|
|---|
| 554 | fMac2->SetAcceleration(0.4*vr);// 0.4
|
|---|
| 555 | fMac2->SetDeceleration(0.4*vr);// 0.4
|
|---|
| 556 | SetPosVelocity(fabs(rd.Ratio()), 0.2); // 0.175
|
|---|
| 557 | #else
|
|---|
| 558 | fMac2->SetAcceleration(0.2*vr);
|
|---|
| 559 | fMac2->SetDeceleration(0.1*vr);
|
|---|
| 560 | SetPosVelocity(fabs(rd.Ratio()), 0.1);
|
|---|
| 561 | #endif
|
|---|
| 562 | }
|
|---|
| 563 |
|
|---|
| 564 | rd.Round();
|
|---|
| 565 |
|
|---|
| 566 | // FIXME? Check for Error or Zombie?
|
|---|
| 567 |
|
|---|
| 568 | /*
|
|---|
| 569 | cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
|
|---|
| 570 | cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
|
|---|
| 571 | cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
|
|---|
| 572 | cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
|
|---|
| 573 | cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
|
|---|
| 574 | */
|
|---|
| 575 |
|
|---|
| 576 | //
|
|---|
| 577 | // repositioning (relative)
|
|---|
| 578 | //
|
|---|
| 579 | lout << "Do Relative Positioning Done" << endl;
|
|---|
| 580 | DoRelPos(rd, cdzd, cdaz);
|
|---|
| 581 |
|
|---|
| 582 | lout << "Relative Positioning Done" << endl;
|
|---|
| 583 | }
|
|---|
| 584 |
|
|---|
| 585 | if (i<10)
|
|---|
| 586 | StopMovement();
|
|---|
| 587 | else
|
|---|
| 588 | SetStatus(MCosy::kStopped);
|
|---|
| 589 |
|
|---|
| 590 | lout << "Warning: Requested position not reached (i=" << i << ")" << endl;
|
|---|
| 591 | return FALSE;
|
|---|
| 592 | }
|
|---|
| 593 |
|
|---|
| 594 | // --------------------------------------------------------------------------
|
|---|
| 595 | //
|
|---|
| 596 | // Sets the tracking velocity
|
|---|
| 597 | //
|
|---|
| 598 | // The velocities are given in a ZdAz object in re/min. Return kTRUE
|
|---|
| 599 | // in case of success, kFALSE in case of failure.
|
|---|
| 600 | //
|
|---|
| 601 | Bool_t MCosy::SetVelocity(const ZdAz &v)
|
|---|
| 602 | {
|
|---|
| 603 | //
|
|---|
| 604 | // Send the new velocities for both axes.
|
|---|
| 605 | //
|
|---|
| 606 | fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min]
|
|---|
| 607 | fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min]
|
|---|
| 608 |
|
|---|
| 609 | //
|
|---|
| 610 | // Wait for the objects to be acknoledged.
|
|---|
| 611 | //
|
|---|
| 612 | fMac2->WaitForSdo(0x3006, 1);
|
|---|
| 613 | fMac1->WaitForSdo(0x3006, 1);
|
|---|
| 614 |
|
|---|
| 615 | //
|
|---|
| 616 | // If the waiting for the objects wasn't interrupted return kTRUE
|
|---|
| 617 | //
|
|---|
| 618 | if (!(Break() || HasError() || HasZombie()))
|
|---|
| 619 | return kTRUE;
|
|---|
| 620 |
|
|---|
| 621 | //
|
|---|
| 622 | // print a message if the interruption was due to a Can-node Error
|
|---|
| 623 | //
|
|---|
| 624 | if (HasError())
|
|---|
| 625 | lout << "Error while setting velocity (SDO #3006)" << endl;
|
|---|
| 626 |
|
|---|
| 627 | return kFALSE;
|
|---|
| 628 | }
|
|---|
| 629 |
|
|---|
| 630 | // --------------------------------------------------------------------------
|
|---|
| 631 | //
|
|---|
| 632 | // Initializes Tracking mode
|
|---|
| 633 | //
|
|---|
| 634 | // Initializes the accelerations of both axes with 90% of the maximum
|
|---|
| 635 | // acceleration. Set the status for moving and tracking and starts thr
|
|---|
| 636 | // revolution mode.
|
|---|
| 637 | //
|
|---|
| 638 | bool MCosy::InitTracking()
|
|---|
| 639 | {
|
|---|
| 640 | // FIXME? Handling of Zombie OK?
|
|---|
| 641 | if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
|
|---|
| 642 | return false;
|
|---|
| 643 |
|
|---|
| 644 | //
|
|---|
| 645 | // Start revolution mode
|
|---|
| 646 | //
|
|---|
| 647 | fMac2->SetAcceleration(0.1*fMac2->GetVelRes());
|
|---|
| 648 | fMac2->SetDeceleration(0.1*fMac2->GetVelRes());
|
|---|
| 649 | if (fMac2->IsZombieNode())
|
|---|
| 650 | return false;
|
|---|
| 651 |
|
|---|
| 652 | fMac1->SetAcceleration(0.1*fMac1->GetVelRes());
|
|---|
| 653 | fMac1->SetDeceleration(0.1*fMac1->GetVelRes());
|
|---|
| 654 | if (fMac1->IsZombieNode())
|
|---|
| 655 | return false;
|
|---|
| 656 |
|
|---|
| 657 | SetStatus(MCosy::kMoving | MCosy::kTracking);
|
|---|
| 658 |
|
|---|
| 659 | fMac2->SetRpmMode(TRUE);
|
|---|
| 660 | if (fMac2->IsZombieNode())
|
|---|
| 661 | return false;
|
|---|
| 662 |
|
|---|
| 663 | fMac1->SetRpmMode(TRUE);
|
|---|
| 664 | if (fMac1->IsZombieNode())
|
|---|
| 665 | return false;
|
|---|
| 666 |
|
|---|
| 667 | return true;
|
|---|
| 668 | }
|
|---|
| 669 |
|
|---|
| 670 | // --------------------------------------------------------------------------
|
|---|
| 671 | //
|
|---|
| 672 | // Limits the speed.
|
|---|
| 673 | //
|
|---|
| 674 | // This function should work as a limiter. If a tracking error is too large
|
|---|
| 675 | // to be corrected fast enough we would get enormous velocities. These
|
|---|
| 676 | // velocities are limited to the maximum velocity.
|
|---|
| 677 | //
|
|---|
| 678 | void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
|
|---|
| 679 | {
|
|---|
| 680 | //
|
|---|
| 681 | // How to limit the speed. If the wind comes and blowes
|
|---|
| 682 | // we cannot forbid changing of the sign. But on the other hand
|
|---|
| 683 | // we don't want fast changes!
|
|---|
| 684 | //
|
|---|
| 685 | ULong_t vrzd = fMac1->GetVelRes();
|
|---|
| 686 | ULong_t vraz = fMac2->GetVelRes();
|
|---|
| 687 |
|
|---|
| 688 | #define sgn(x) (x<0?-1:1)
|
|---|
| 689 |
|
|---|
| 690 | //
|
|---|
| 691 | // When speed changes sign, the maximum allowed speed
|
|---|
| 692 | // is 25% of the |v|
|
|---|
| 693 | //
|
|---|
| 694 | const Float_t limit = 0.25;
|
|---|
| 695 |
|
|---|
| 696 | //
|
|---|
| 697 | // The maximum allowed speed while tracking is 10%
|
|---|
| 698 | //
|
|---|
| 699 | const Float_t maxtrack = 0.1;
|
|---|
| 700 | /*
|
|---|
| 701 | if (sgn(vt->Az()) != sgn(vcalc.Az()))
|
|---|
| 702 | vt->Az(0);
|
|---|
| 703 | // else
|
|---|
| 704 | {
|
|---|
| 705 | if (fabs(vt->Az()) < fabs(vcalc.Az()) *0.5)
|
|---|
| 706 | vt->Az(0.5*vcalc.Az());
|
|---|
| 707 |
|
|---|
| 708 | if (fabs(vt->Az()) > fabs(vcalc.Az()) *1.5)
|
|---|
| 709 | vt->Az(1.5*vcalc.Az());
|
|---|
| 710 | }
|
|---|
| 711 |
|
|---|
| 712 | if (sgn(vt->Zd()) != sgn(vcalc.Zd()))
|
|---|
| 713 | vt->Zd(0);
|
|---|
| 714 | // else
|
|---|
| 715 | {
|
|---|
| 716 | if (fabs(vt->Zd()) > fabs(vcalc.Az()) *1.5)
|
|---|
| 717 | vt->Zd(1.5*vcalc.Zd());
|
|---|
| 718 |
|
|---|
| 719 | if (fabs(vt->Zd()) < fabs(vcalc.Az()) *0.5)
|
|---|
| 720 | vt->Zd(0.5*vcalc.Zd());
|
|---|
| 721 | }
|
|---|
| 722 | */
|
|---|
| 723 |
|
|---|
| 724 | if (sgn(vt->Az()) != sgn(vcalc.Az())
|
|---|
| 725 | && fabs(vt->Az()) < limit*fabs(vcalc.Az())
|
|---|
| 726 | )
|
|---|
| 727 | {
|
|---|
| 728 | lout << "Warning: Negative Azimuth speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
|
|---|
| 729 | vt->Az(0);
|
|---|
| 730 | }
|
|---|
| 731 | else
|
|---|
| 732 | if (fabs(vt->Az()) > maxtrack*vraz)
|
|---|
| 733 | {
|
|---|
| 734 | lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded... limited." << endl;
|
|---|
| 735 | vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
|
|---|
| 736 | }
|
|---|
| 737 |
|
|---|
| 738 | if (sgn(vt->Zd()) != sgn(vcalc.Zd())
|
|---|
| 739 | && fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
|
|---|
| 740 | )
|
|---|
| 741 | {
|
|---|
| 742 | lout << "Warning: Negative Altitude speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
|
|---|
| 743 | vt->Zd(0);
|
|---|
| 744 | }
|
|---|
| 745 | else
|
|---|
| 746 | if (fabs(vt->Zd()) > maxtrack*vrzd)
|
|---|
| 747 | {
|
|---|
| 748 | lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded... limited." << endl;
|
|---|
| 749 | vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
|
|---|
| 750 | }
|
|---|
| 751 | }
|
|---|
| 752 |
|
|---|
| 753 | Bool_t MCosy::AlignTrackingPos(ZdAz pointing, ZdAz &za) const
|
|---|
| 754 | {
|
|---|
| 755 | // pointing [deg]
|
|---|
| 756 | if (pointing.Zd()<0)
|
|---|
| 757 | {
|
|---|
| 758 | pointing.Zd(-pointing.Zd());
|
|---|
| 759 | pointing.Az(pointing.Az()+180);
|
|---|
| 760 | }
|
|---|
| 761 |
|
|---|
| 762 | const ZdAz se = GetSePos()*2*TMath::Pi()/16384; // [rad]
|
|---|
| 763 | const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing
|
|---|
| 764 |
|
|---|
| 765 | do
|
|---|
| 766 | {
|
|---|
| 767 | const Double_t d = unbendedse.Az() - pointing.Az();
|
|---|
| 768 | if (d>-180 && d<=180)
|
|---|
| 769 | break;
|
|---|
| 770 |
|
|---|
| 771 | pointing.Az(pointing.Az()+TMath::Sign(360., d));
|
|---|
| 772 | } while (1);
|
|---|
| 773 |
|
|---|
| 774 | const Bool_t rc = CheckRange(pointing);
|
|---|
| 775 | za = pointing/kRad2Deg; // [rad]
|
|---|
| 776 |
|
|---|
| 777 | if (!rc)
|
|---|
| 778 | lout << "Error: Aligned position out of Range." << endl;
|
|---|
| 779 |
|
|---|
| 780 | return rc;
|
|---|
| 781 | }
|
|---|
| 782 |
|
|---|
| 783 | Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
|
|---|
| 784 | {
|
|---|
| 785 | ifstream fin("pointingpos.txt");
|
|---|
| 786 | if (!fin)
|
|---|
| 787 | return -1;
|
|---|
| 788 |
|
|---|
| 789 | Double_t mjd0, zd, az;
|
|---|
| 790 | fin >> mjd0 >> zd >> az;
|
|---|
| 791 |
|
|---|
| 792 | mjd0 += 52000;
|
|---|
| 793 |
|
|---|
| 794 | if (mjd0+1./24/60 <mjd)
|
|---|
| 795 | return -1;
|
|---|
| 796 |
|
|---|
| 797 | ZdAz point;
|
|---|
| 798 | if (!AlignTrackingPos(ZdAz(zd, az), point))
|
|---|
| 799 | {
|
|---|
| 800 | cout << "Starguider position couldn't be aligned..." << endl;
|
|---|
| 801 | return -1;
|
|---|
| 802 | }
|
|---|
| 803 |
|
|---|
| 804 | const ZdAz diff = (dest-point)*kRad2Deg;
|
|---|
| 805 |
|
|---|
| 806 | if (diff.Zd()>5 || diff.Az()>5)
|
|---|
| 807 | {
|
|---|
| 808 | cout << "Starguider deviation too large... dZd=" << diff.Zd() <<" dAz="<<diff.Az() << endl;
|
|---|
| 809 | return -1;
|
|---|
| 810 | }
|
|---|
| 811 |
|
|---|
| 812 | dest -= point;
|
|---|
| 813 | dest *= 16384/TMath::Pi()/2; // [se]
|
|---|
| 814 | dest *= -kGearRatio; // [re]
|
|---|
| 815 |
|
|---|
| 816 | cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl;
|
|---|
| 817 |
|
|---|
| 818 | return (mjd-mjd0) * (24*60*60); // [s]
|
|---|
| 819 | }
|
|---|
| 820 |
|
|---|
| 821 | void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
|
|---|
| 822 | {
|
|---|
| 823 | lout << "Track Position: " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg << "deg (Ra/Dec)" << endl;
|
|---|
| 824 |
|
|---|
| 825 | SlaStars sla(fObservatory);
|
|---|
| 826 |
|
|---|
| 827 | //
|
|---|
| 828 | // Position to actual position
|
|---|
| 829 | //
|
|---|
| 830 | sla.Now();
|
|---|
| 831 | ZdAz dest = sla.CalcZdAz(dst);
|
|---|
| 832 |
|
|---|
| 833 | // FIXME: Determin tracking start point by star culmination
|
|---|
| 834 | if (dest.Az()<-TMath::Pi()/2)
|
|---|
| 835 | {
|
|---|
| 836 | lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
|
|---|
| 837 | dest.Az(dest.Az() + TMath::Pi()*2);
|
|---|
| 838 | }
|
|---|
| 839 |
|
|---|
| 840 | if (dest.Az()>3*TMath::Pi()/2)
|
|---|
| 841 | {
|
|---|
| 842 | lout << "Substracting 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
|
|---|
| 843 | dest.Az(dest.Az() -TMath::Pi()*2);
|
|---|
| 844 | }
|
|---|
| 845 |
|
|---|
| 846 | if (!SetPosition(dest, kTRUE))
|
|---|
| 847 | {
|
|---|
| 848 | lout << "Error: Cannot start tracking, positioning failed." << endl;
|
|---|
| 849 | return;
|
|---|
| 850 | }
|
|---|
| 851 |
|
|---|
| 852 | //
|
|---|
| 853 | // calculate offset from present se position
|
|---|
| 854 | //
|
|---|
| 855 | const ZdAz sepos = GetSePos()*kGearRatio;
|
|---|
| 856 |
|
|---|
| 857 | if (!RequestRePos())
|
|---|
| 858 | return;
|
|---|
| 859 |
|
|---|
| 860 | //
|
|---|
| 861 | // Estimate Offset before starting to track
|
|---|
| 862 | //
|
|---|
| 863 | fOffset = sepos-GetRePos();
|
|---|
| 864 |
|
|---|
| 865 | /*
|
|---|
| 866 | cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
|
|---|
| 867 | cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
|
|---|
| 868 | cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
|
|---|
| 869 | */
|
|---|
| 870 |
|
|---|
| 871 | //
|
|---|
| 872 | // Init accelerations and Rpm Mode
|
|---|
| 873 | //
|
|---|
| 874 | if (!InitTracking())
|
|---|
| 875 | {
|
|---|
| 876 | StopMovement();
|
|---|
| 877 | return;
|
|---|
| 878 | }
|
|---|
| 879 |
|
|---|
| 880 | XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
|
|---|
| 881 |
|
|---|
| 882 | lout << "Start tracking:";
|
|---|
| 883 | lout << " Ra: " << xy.X() << "h " << "Dec: " << xy.Y() << "\xb0" << endl;
|
|---|
| 884 | #ifdef EXPERT
|
|---|
| 885 | ofstream fout("coordinates.txt");
|
|---|
| 886 | fout << xy;
|
|---|
| 887 | fout.close();
|
|---|
| 888 | #endif
|
|---|
| 889 | //
|
|---|
| 890 | // Initialize Tracker (slalib or starguider)
|
|---|
| 891 | //
|
|---|
| 892 | fRaDec = dst;
|
|---|
| 893 | fBackground = kBgdTracking;
|
|---|
| 894 |
|
|---|
| 895 | //--- ofstream fout("log/cosy.pos");
|
|---|
| 896 | //--- fout << "Tracking:";
|
|---|
| 897 | //--- fout << " Ra: " << Rad2Deg(dst.Ra()) << "\x9c ";
|
|---|
| 898 | //--- fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
|
|---|
| 899 | //--- fout << " Mjd/10ms V/re/min/4" << endl;
|
|---|
| 900 |
|
|---|
| 901 | //
|
|---|
| 902 | // We want to reach the theoretical position exactly in about 0.5s
|
|---|
| 903 | //
|
|---|
| 904 | // *OLD*const float dt = 1; // 1 second
|
|---|
| 905 | const float dt = 5;//3; // 2 second
|
|---|
| 906 | while (!(Break() || HasError() || HasZombie()))
|
|---|
| 907 | {
|
|---|
| 908 | //
|
|---|
| 909 | // Request Target position for this moment
|
|---|
| 910 | //
|
|---|
| 911 | sla.Now();
|
|---|
| 912 |
|
|---|
| 913 | //
|
|---|
| 914 | // Request theoretical Position for a time in the future (To+dt) from CPU
|
|---|
| 915 | //
|
|---|
| 916 | const Double_t mjd = sla.GetMjd()+dt/(60*60*24);
|
|---|
| 917 | const ZdAz pointing = sla.CalcZdAz(fRaDec, mjd)*kRad2Deg; // soll pointing [deg]
|
|---|
| 918 |
|
|---|
| 919 | ZdAz dest;
|
|---|
| 920 | if (!AlignTrackingPos(pointing, dest))
|
|---|
| 921 | break;
|
|---|
| 922 |
|
|---|
| 923 | ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min]
|
|---|
| 924 |
|
|---|
| 925 | float dtime = -1;
|
|---|
| 926 | if (kFALSE /*fUseStarguider*/)
|
|---|
| 927 | dtime = Starguider(mjd, dest);
|
|---|
| 928 |
|
|---|
| 929 | if (dtime<0)
|
|---|
| 930 | {
|
|---|
| 931 | dest = fBending(dest); // [rad]
|
|---|
| 932 | dest *= 16384/TMath::Pi()/2; // [se]
|
|---|
| 933 | dest *= kGearRatio; // [re]
|
|---|
| 934 |
|
|---|
| 935 | //
|
|---|
| 936 | // Request absolute position of rotary encoder from Macs
|
|---|
| 937 | //
|
|---|
| 938 | if (!RequestRePos())
|
|---|
| 939 | break;
|
|---|
| 940 |
|
|---|
| 941 | //
|
|---|
| 942 | // distance between (To+dt) and To [re]
|
|---|
| 943 | // position time difference < 5usec
|
|---|
| 944 | // fOffset does the synchronization between the
|
|---|
| 945 | // Shaft- and the rotary encoders
|
|---|
| 946 | dest -= GetRePos() + fOffset;
|
|---|
| 947 |
|
|---|
| 948 | dtime = dt;
|
|---|
| 949 | }
|
|---|
| 950 |
|
|---|
| 951 | //
|
|---|
| 952 | // Velocity to go [re/min] to reach the right position at time t+dt
|
|---|
| 953 | // correct for the duration of RaDec2AltAz
|
|---|
| 954 | //
|
|---|
| 955 | const ZdAz v = dest*60.0/(dtime/*-(fMac2->GetTime()-sla)*/);
|
|---|
| 956 |
|
|---|
| 957 | //
|
|---|
| 958 | // calculate real velocity of future [re/min]
|
|---|
| 959 | // believing the Macs manual '/4' shouldn't be necessary, but it is.
|
|---|
| 960 | //
|
|---|
| 961 | ZdAz vt = v/4;
|
|---|
| 962 | LimitSpeed(&vt, vcalc);
|
|---|
| 963 | vt.Round();
|
|---|
| 964 |
|
|---|
| 965 | //
|
|---|
| 966 | // check if the drive is fast enough to follow the star
|
|---|
| 967 | //
|
|---|
| 968 | if (vt.Zd()>.9*fMac1->GetVelRes() || vt.Az()>.9*fMac2->GetVelRes())
|
|---|
| 969 | {
|
|---|
| 970 | lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
|
|---|
| 971 | break;
|
|---|
| 972 | }
|
|---|
| 973 |
|
|---|
| 974 | //
|
|---|
| 975 | // Set theoretical velocity (as early after calculation as possible)
|
|---|
| 976 | // Maybe we should attenuate the changes
|
|---|
| 977 | //
|
|---|
| 978 | if (!SetVelocity(vt))
|
|---|
| 979 | break;
|
|---|
| 980 |
|
|---|
| 981 | //
|
|---|
| 982 | // Now do 'unnecessary' things
|
|---|
| 983 | //
|
|---|
| 984 | fVelocity = vt/kGearRatio2*4;
|
|---|
| 985 |
|
|---|
| 986 | //--- const double mjd = fMac2->GetMjd();
|
|---|
| 987 | //--- fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " ";
|
|---|
| 988 | //--- fout << setw(4) << vt.Zd() << " ";
|
|---|
| 989 | //--- fout << setw(4) << vt.Az() << endl;
|
|---|
| 990 | //
|
|---|
| 991 | // FIXME? Calculate an accuracy for the tracking system?
|
|---|
| 992 | // How good do we reach the calculated position in 'real'
|
|---|
| 993 | // re valus?
|
|---|
| 994 | //
|
|---|
| 995 |
|
|---|
| 996 |
|
|---|
| 997 | //
|
|---|
| 998 | // Update speed as often as possible.
|
|---|
| 999 | // make sure, that dt is around 10 times larger than the
|
|---|
| 1000 | // update time
|
|---|
| 1001 | //
|
|---|
| 1002 | //
|
|---|
| 1003 | // The loop should not be executed faster than the ramp of
|
|---|
| 1004 | // a change in the velocity can be followed.
|
|---|
| 1005 | // (This is important on fast machines >500MHz)
|
|---|
| 1006 | //
|
|---|
| 1007 | /*
|
|---|
| 1008 | MTimeout t(1000);
|
|---|
| 1009 | while (!t.HasTimedOut())
|
|---|
| 1010 | usleep(1);
|
|---|
| 1011 | */
|
|---|
| 1012 | usleep(1000000); // 1s
|
|---|
| 1013 | //usleep(50000); // 0.05s
|
|---|
| 1014 | }
|
|---|
| 1015 |
|
|---|
| 1016 | fBackground = kBgdNone;
|
|---|
| 1017 | StopMovement();
|
|---|
| 1018 | lout << "Tracking stopped." << endl;
|
|---|
| 1019 | }
|
|---|
| 1020 |
|
|---|
| 1021 | // --------------------------------------------------------------------------
|
|---|
| 1022 | //
|
|---|
| 1023 | // Stops the movement of both motors.
|
|---|
| 1024 | //
|
|---|
| 1025 | // Sets the status to stopping. Sets the deceleration to 50% of the maximum.
|
|---|
| 1026 | // stops. Quits the revolution mode and wait for the end of the movement.
|
|---|
| 1027 | //
|
|---|
| 1028 | void MCosy::StopMovement()
|
|---|
| 1029 | {
|
|---|
| 1030 | //
|
|---|
| 1031 | // Set status to Stopping
|
|---|
| 1032 | //
|
|---|
| 1033 | SetStatus(MCosy::kStopping);
|
|---|
| 1034 |
|
|---|
| 1035 | //
|
|---|
| 1036 | // set deceleration to 50%
|
|---|
| 1037 | //
|
|---|
| 1038 | cout << "Stopping movement (dec=30%)..." << endl;
|
|---|
| 1039 | #ifdef EXPERT
|
|---|
| 1040 | fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
|
|---|
| 1041 | fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
|
|---|
| 1042 | #else
|
|---|
| 1043 | fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
|
|---|
| 1044 | fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
|
|---|
| 1045 | #endif
|
|---|
| 1046 | fMac1->SetRpmMode(FALSE);
|
|---|
| 1047 | fMac2->SetRpmMode(FALSE);
|
|---|
| 1048 |
|
|---|
| 1049 | /*
|
|---|
| 1050 | fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
|
|---|
| 1051 | fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
|
|---|
| 1052 |
|
|---|
| 1053 | fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
|
|---|
| 1054 | fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
|
|---|
| 1055 | fMac2->WaitForSdo(0x3000, 0);
|
|---|
| 1056 | fMac1->WaitForSdo(0x3000, 0);
|
|---|
| 1057 | fMac1->SetRpmMode(FALSE);
|
|---|
| 1058 | fMac2->SetRpmMode(FALSE);
|
|---|
| 1059 | */
|
|---|
| 1060 |
|
|---|
| 1061 | //
|
|---|
| 1062 | // Wait for the movement to really be finished.
|
|---|
| 1063 | //
|
|---|
| 1064 | #ifdef EXPERT
|
|---|
| 1065 | cout << "Waiting for end of movement..." << endl;
|
|---|
| 1066 | #endif
|
|---|
| 1067 | WaitForEndMovement();
|
|---|
| 1068 |
|
|---|
| 1069 | //
|
|---|
| 1070 | // Check whether everything works fine.
|
|---|
| 1071 | //
|
|---|
| 1072 | CheckForError();
|
|---|
| 1073 | #ifdef EXPERT
|
|---|
| 1074 | cout << "Movement stopped." << endl;
|
|---|
| 1075 | #endif
|
|---|
| 1076 | }
|
|---|
| 1077 |
|
|---|
| 1078 | void MCosy::StopTracking()
|
|---|
| 1079 | {
|
|---|
| 1080 | //
|
|---|
| 1081 | // Set status to Stopping
|
|---|
| 1082 | //
|
|---|
| 1083 | SetStatus(MCosy::kStopping);
|
|---|
| 1084 |
|
|---|
| 1085 | //
|
|---|
| 1086 | // set deceleration to 50%
|
|---|
| 1087 | //
|
|---|
| 1088 | cout << "Stopping tracking (dec=20%)..." << endl;
|
|---|
| 1089 | fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
|
|---|
| 1090 | fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
|
|---|
| 1091 |
|
|---|
| 1092 | fMac2->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
|
|---|
| 1093 | fMac1->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
|
|---|
| 1094 | fMac2->WaitForSdo(0x3006, 1);
|
|---|
| 1095 | fMac1->WaitForSdo(0x3006, 1);
|
|---|
| 1096 |
|
|---|
| 1097 | cout << "Waiting for end of movement..." << endl;
|
|---|
| 1098 | WaitForEndMovement();
|
|---|
| 1099 |
|
|---|
| 1100 | //
|
|---|
| 1101 | // Wait for the objects to be OKed.
|
|---|
| 1102 | //
|
|---|
| 1103 | fMac1->SetRpmMode(FALSE);
|
|---|
| 1104 | fMac2->SetRpmMode(FALSE);
|
|---|
| 1105 |
|
|---|
| 1106 | //
|
|---|
| 1107 | // Wait for the movement to really be finished.
|
|---|
| 1108 | //
|
|---|
| 1109 | //cout << "Waiting for end of movement..." << endl;
|
|---|
| 1110 | //WaitForEndMovement();
|
|---|
| 1111 |
|
|---|
| 1112 | //
|
|---|
| 1113 | // Check whether everything works fine.
|
|---|
| 1114 | //
|
|---|
| 1115 | CheckForError();
|
|---|
| 1116 | cout << "Movement stopped." << endl;
|
|---|
| 1117 | }
|
|---|
| 1118 |
|
|---|
| 1119 | bool MCosy::CheckNetwork()
|
|---|
| 1120 | {
|
|---|
| 1121 | //return kTRUE;
|
|---|
| 1122 | //CheckConnections();
|
|---|
| 1123 |
|
|---|
| 1124 | if (HasZombie())
|
|---|
| 1125 | {
|
|---|
| 1126 | lout << "- Found Zombies in Network..." << endl;
|
|---|
| 1127 | if (!RebootZombies())
|
|---|
| 1128 | return false;
|
|---|
| 1129 | }
|
|---|
| 1130 |
|
|---|
| 1131 | /*
|
|---|
| 1132 | FIXME HANDLING ERROR
|
|---|
| 1133 | */
|
|---|
| 1134 | if (HasError())
|
|---|
| 1135 | {
|
|---|
| 1136 | fMac1->HandleError();
|
|---|
| 1137 | fMac2->HandleError();
|
|---|
| 1138 | fMac3->HandleError();
|
|---|
| 1139 | if (HasError() || HasZombie())
|
|---|
| 1140 | return false;
|
|---|
| 1141 | }
|
|---|
| 1142 |
|
|---|
| 1143 | return true;
|
|---|
| 1144 | }
|
|---|
| 1145 |
|
|---|
| 1146 | void *MCosy::Proc(int msg, void *mp)
|
|---|
| 1147 | {
|
|---|
| 1148 | switch (msg)
|
|---|
| 1149 | {
|
|---|
| 1150 | case WM_WAIT:
|
|---|
| 1151 | cout << "Wait for execution of Proc(WM_*, ): done." << endl;
|
|---|
| 1152 | return NULL;
|
|---|
| 1153 |
|
|---|
| 1154 | case WM_STOP:
|
|---|
| 1155 | cout << "MCosy::Proc: Stop." << endl;
|
|---|
| 1156 | if (!CheckNetwork())
|
|---|
| 1157 | return (void*)0xebb0;
|
|---|
| 1158 | StopMovement();
|
|---|
| 1159 | return NULL;
|
|---|
| 1160 | /*
|
|---|
| 1161 | case WM_PRESET:
|
|---|
| 1162 | cout << "WM_Preset: start." << endl;
|
|---|
| 1163 | if (!CheckNetwork())
|
|---|
| 1164 | return (void*)0xebb0;
|
|---|
| 1165 | fZd1->SetPreset();
|
|---|
| 1166 | fZd2->SetPreset();
|
|---|
| 1167 | fAz->SetPreset();
|
|---|
| 1168 | cout << "WM_Preset: done. (return 0xaffe)" << endl;
|
|---|
| 1169 | return (void*)0xaffe;
|
|---|
| 1170 | */
|
|---|
| 1171 | /*
|
|---|
| 1172 | case WM_CALIB:
|
|---|
| 1173 | {
|
|---|
| 1174 | cout << "WM_Calib: start." << endl;
|
|---|
| 1175 | if (!CheckNetwork())
|
|---|
| 1176 | return (void*)0xebb0;
|
|---|
| 1177 |
|
|---|
| 1178 | SlaStars sla(fObservatory);
|
|---|
| 1179 | sla.Now();
|
|---|
| 1180 |
|
|---|
| 1181 | RaDec rd = *((RaDec*)mp);
|
|---|
| 1182 |
|
|---|
| 1183 | //RaDec rd(37.94, 89.2644); // POLARIS
|
|---|
| 1184 | //RaDec rd(213.915417, 19.1825); // ARCTURUS
|
|---|
| 1185 |
|
|---|
| 1186 | cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
|
|---|
| 1187 |
|
|---|
| 1188 | ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;
|
|---|
| 1189 |
|
|---|
| 1190 | cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
|
|---|
| 1191 |
|
|---|
| 1192 | ZdAz sepos = GetSePos();
|
|---|
| 1193 | cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
|
|---|
| 1194 |
|
|---|
| 1195 | fZd1->SetPreset(za.Zd());
|
|---|
| 1196 | fZd2->SetPreset(-za.Zd());
|
|---|
| 1197 | fAz->SetPreset(za.Az());
|
|---|
| 1198 |
|
|---|
| 1199 | cout << "WM_Calib: done. (return 0xaffe)" << endl;
|
|---|
| 1200 | }
|
|---|
| 1201 | return (void*)0xaffe;
|
|---|
| 1202 | */
|
|---|
| 1203 | #ifdef EXPERT
|
|---|
| 1204 | case WM_TPOINT:
|
|---|
| 1205 | {
|
|---|
| 1206 | cout << "WM_TPoint: start." << endl;
|
|---|
| 1207 | SlaStars sla(fObservatory);
|
|---|
| 1208 | sla.Now();
|
|---|
| 1209 |
|
|---|
| 1210 | RaDec rd = *((RaDec*)mp);
|
|---|
| 1211 | cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
|
|---|
| 1212 |
|
|---|
| 1213 | AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;
|
|---|
| 1214 |
|
|---|
| 1215 | cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
|---|
| 1216 | *tpout << setprecision(7) << za.Az() << " " << za.Alt() << " ";
|
|---|
| 1217 |
|
|---|
| 1218 | ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;;
|
|---|
| 1219 | za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
|
|---|
| 1220 | za *= kRad2Deg;
|
|---|
| 1221 |
|
|---|
| 1222 | cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
|---|
| 1223 | *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";
|
|---|
| 1224 | *tpout << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;
|
|---|
| 1225 |
|
|---|
| 1226 | cout << "WM_TPoint: done. (return 0xaffe)" << endl;
|
|---|
| 1227 | }
|
|---|
| 1228 | return (void*)0xca1b;
|
|---|
| 1229 | #endif
|
|---|
| 1230 |
|
|---|
| 1231 | case WM_TRACKPOS:
|
|---|
| 1232 | cout << "WM_TrackPosition: start." << endl;
|
|---|
| 1233 | {
|
|---|
| 1234 | if (!CheckNetwork())
|
|---|
| 1235 | return (void*)0xebb0;
|
|---|
| 1236 |
|
|---|
| 1237 | ZdAz dest = *((ZdAz*)mp) * kDeg2Rad;
|
|---|
| 1238 | //if (!SetPosition(dest))
|
|---|
| 1239 | // return (void*)0x1234;
|
|---|
| 1240 |
|
|---|
| 1241 | SlaStars sla(fObservatory);
|
|---|
| 1242 | sla.Now();
|
|---|
| 1243 |
|
|---|
| 1244 | RaDec rd = sla.CalcRaDec(dest);
|
|---|
| 1245 | cout << dest.Zd()*180/3.1415 << " " << dest.Az()*180/3.1415 << endl;
|
|---|
| 1246 | cout << rd.Ra()*12/3.1415 << " " << rd.Dec()*180/3.1415 << endl;
|
|---|
| 1247 | TrackPosition(rd);
|
|---|
| 1248 | }
|
|---|
| 1249 | cout << "WM_TrackPosition: done. (return 0xabcd)" << endl;
|
|---|
| 1250 | return (void*)0xabcd;
|
|---|
| 1251 |
|
|---|
| 1252 | case WM_POSITION:
|
|---|
| 1253 | cout << "WM_Position: start." << endl;
|
|---|
| 1254 | {
|
|---|
| 1255 | if (!CheckNetwork())
|
|---|
| 1256 | return (void*)0xebb0;
|
|---|
| 1257 |
|
|---|
| 1258 | ZdAz dest = *((ZdAz*)mp);
|
|---|
| 1259 | SetPosition(dest*kDeg2Rad);
|
|---|
| 1260 | }
|
|---|
| 1261 | cout << "WM_Position: done. (return 0x7777)" << endl;
|
|---|
| 1262 | return (void*)0x7777;
|
|---|
| 1263 |
|
|---|
| 1264 | case WM_TESTSE:
|
|---|
| 1265 | cout << "WM_TestSe: start." << endl;
|
|---|
| 1266 | fBackground = mp ? kBgdSeTest : kBgdNone;
|
|---|
| 1267 | cout << "WM_TestSe: done. (return 0x1e51)" << endl;
|
|---|
| 1268 | return (void*)0x1e51;
|
|---|
| 1269 |
|
|---|
| 1270 | case WM_GEAR:
|
|---|
| 1271 | cout << "WM_Gear: start." << endl;
|
|---|
| 1272 | fBackground = mp ? kBgdGear : kBgdNone;
|
|---|
| 1273 | cout << "WM_Gear: done. (return 0xfeaf)" << endl;
|
|---|
| 1274 | return (void*)0xfeaf;
|
|---|
| 1275 |
|
|---|
| 1276 | case WM_DISPLAY:
|
|---|
| 1277 | cout << "WM_Display: start." << endl;
|
|---|
| 1278 | fTriggerDisplay = kTRUE;
|
|---|
| 1279 | cout << "WM_Disply: done. (return 0xd1e1)" << endl;
|
|---|
| 1280 | return (void*)0xd1e1;
|
|---|
| 1281 |
|
|---|
| 1282 | case WM_TRACK:
|
|---|
| 1283 | cout << "WM_Track: START" << endl;
|
|---|
| 1284 | {
|
|---|
| 1285 | if (!CheckNetwork())
|
|---|
| 1286 | return (void*)0xebb0;
|
|---|
| 1287 |
|
|---|
| 1288 | RaDec dest = *((RaDec*)mp);
|
|---|
| 1289 | TrackPosition(dest*kDeg2Rad);
|
|---|
| 1290 | }
|
|---|
| 1291 | cout << "WM_Track: done. (return 0x8888)" << endl;
|
|---|
| 1292 | return (void*)0x8888;
|
|---|
| 1293 |
|
|---|
| 1294 | case WM_NEWTRACK:
|
|---|
| 1295 | cout << "WM_NewTrack: START" << endl;
|
|---|
| 1296 | fRaDec = *((RaDec*)mp);
|
|---|
| 1297 | cout << "WM_NewTrack: done. (return 0x9999)" << endl;
|
|---|
| 1298 | return (void*)0x9999;
|
|---|
| 1299 |
|
|---|
| 1300 | case WM_LOADBENDING:
|
|---|
| 1301 | cout << "WM_LoadBending: START" << endl;
|
|---|
| 1302 | fBending.Load("bending.txt");
|
|---|
| 1303 | cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
|
|---|
| 1304 | return (void*)0xbe0d;
|
|---|
| 1305 |
|
|---|
| 1306 | case WM_RESETBENDING:
|
|---|
| 1307 | cout << "WM_ResetBending: START" << endl;
|
|---|
| 1308 | fBending.Reset();
|
|---|
| 1309 | cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
|
|---|
| 1310 | return (void*)0xbe0e;
|
|---|
| 1311 |
|
|---|
| 1312 | case WM_HOME:
|
|---|
| 1313 | cout << "WM_Home: START" << endl;
|
|---|
| 1314 | if (!CheckNetwork())
|
|---|
| 1315 | return (void*)0xebb0;
|
|---|
| 1316 | else
|
|---|
| 1317 | {
|
|---|
| 1318 | cout << "HOME NOT ALLOWED... for Magic." << endl;
|
|---|
| 1319 | /*
|
|---|
| 1320 | cout << "Going Home..." << endl;
|
|---|
| 1321 | TEnv env(".cosyrc");
|
|---|
| 1322 |
|
|---|
| 1323 | SetStatus(MCosy::kMoving);
|
|---|
| 1324 |
|
|---|
| 1325 | fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
|
|---|
| 1326 | fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
|
|---|
| 1327 |
|
|---|
| 1328 | lout << "SETHOME DONE" << endl;
|
|---|
| 1329 |
|
|---|
| 1330 | SetStatus(HasError() ? MCosy::kError : MCosy::kStopped);
|
|---|
| 1331 |
|
|---|
| 1332 | fAz->SetPreset();
|
|---|
| 1333 | fZd1->SetPreset();
|
|---|
| 1334 | fZd2->SetPreset();
|
|---|
| 1335 |
|
|---|
| 1336 | fMac1->ReqPos();
|
|---|
| 1337 | fMac2->ReqPos();
|
|---|
| 1338 | fMac3->StopMotor();
|
|---|
| 1339 | */
|
|---|
| 1340 | }
|
|---|
| 1341 | cout << "WM_Home: done. (return 0x403e)" << endl;
|
|---|
| 1342 | return (void*)0x403e;
|
|---|
| 1343 |
|
|---|
| 1344 | case WM_CALCALTAZ:
|
|---|
| 1345 | {
|
|---|
| 1346 | cout << endl;
|
|---|
| 1347 |
|
|---|
| 1348 | SlaStars sla(fObservatory);
|
|---|
| 1349 | sla.Now();
|
|---|
| 1350 |
|
|---|
| 1351 | XY xy = *((XY*)mp);
|
|---|
| 1352 | RaDec rd(xy.X()*15., xy.Y()); // [deg]
|
|---|
| 1353 |
|
|---|
| 1354 | const ZdAz a0 = sla.CalcZdAz(rd*kDeg2Rad);
|
|---|
| 1355 |
|
|---|
| 1356 | ZdAz a1;
|
|---|
| 1357 | AlignTrackingPos(a0, a1);
|
|---|
| 1358 | a1 *= 180/TMath::Pi();
|
|---|
| 1359 |
|
|---|
| 1360 | const ZdAz a2 = a1*16384/360;
|
|---|
| 1361 | const ZdAz se = a0*16384/360;
|
|---|
| 1362 |
|
|---|
| 1363 | cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
|
|---|
| 1364 | cout << "Zd/Az target: " << a0.Zd() << "° " << a0.Az() << "°" << endl;
|
|---|
| 1365 | cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
|
|---|
| 1366 | cout << "SE target: " << se.Zd() << " " << se.Az() << endl;
|
|---|
| 1367 | cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
|
|---|
| 1368 | }
|
|---|
| 1369 | return (void*)0xa17a;
|
|---|
| 1370 |
|
|---|
| 1371 | case WM_QUIT:
|
|---|
| 1372 | cout << "WM_Quit: now." << endl;
|
|---|
| 1373 | if (!CheckNetwork())
|
|---|
| 1374 | {
|
|---|
| 1375 | lout << "ERROR: Cannot shutdown CANbus network." << endl;
|
|---|
| 1376 | return (void*)0xebb0;
|
|---|
| 1377 | }
|
|---|
| 1378 | TerminateApp();
|
|---|
| 1379 | cout << "WM_Quit: done." << endl;
|
|---|
| 1380 | return (void*)0xaaaa;
|
|---|
| 1381 | }
|
|---|
| 1382 | cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
|
|---|
| 1383 | return (void*)0xffffffff;
|
|---|
| 1384 | }
|
|---|
| 1385 |
|
|---|
| 1386 | void *MTTalk::Thread()
|
|---|
| 1387 | {
|
|---|
| 1388 | fCosy->TalkThread();
|
|---|
| 1389 | return NULL;
|
|---|
| 1390 | }
|
|---|
| 1391 |
|
|---|
| 1392 | void MCosy::ReadConfig()
|
|---|
| 1393 | {
|
|---|
| 1394 | cout << "Reading configuration file..." << flush;
|
|---|
| 1395 | TEnv env(".cosyrc");
|
|---|
| 1396 | cout << "done." << endl;
|
|---|
| 1397 |
|
|---|
| 1398 | cout << "Reading telescope range..." << flush;
|
|---|
| 1399 | const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
|
|---|
| 1400 | const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
|
|---|
| 1401 | fMin.Set(zmin, amin);
|
|---|
| 1402 |
|
|---|
| 1403 | cout << " Min: " << zmin << "deg " << amin << "deg" << endl;
|
|---|
| 1404 |
|
|---|
| 1405 | const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
|
|---|
| 1406 | const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
|
|---|
| 1407 | fMax.Set(zmax, amax);
|
|---|
| 1408 |
|
|---|
| 1409 | cout << " Max: " << zmax << "deg " << amax << "deg" << endl;
|
|---|
| 1410 |
|
|---|
| 1411 | cout << "Reading gear ratios..." << flush;
|
|---|
| 1412 | const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0);
|
|---|
| 1413 | const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0);
|
|---|
| 1414 |
|
|---|
| 1415 | Double_t resreaz = 0;
|
|---|
| 1416 | if (fMac1 && !fMac1->IsZombieNode())
|
|---|
| 1417 | resreaz = fMac1->GetRes();
|
|---|
| 1418 | else
|
|---|
| 1419 | if (fMac3 && !fMac3->IsZombieNode())
|
|---|
| 1420 | resreaz = fMac3->GetRes();
|
|---|
| 1421 | else
|
|---|
| 1422 | resreaz = env.GetValue("Az_ResRE[re/U_mot]", 1500);
|
|---|
| 1423 |
|
|---|
| 1424 | Double_t resrezd = 0;
|
|---|
| 1425 | if (fMac2 && !fMac2->IsZombieNode())
|
|---|
| 1426 | resrezd = fMac2->GetRes();
|
|---|
| 1427 | else
|
|---|
| 1428 | resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500);
|
|---|
| 1429 |
|
|---|
| 1430 | Double_t ressezd = 0;
|
|---|
| 1431 | if (fZd1 && !fZd1->IsZombieNode())
|
|---|
| 1432 | ressezd = fZd1->GetPhysRes();
|
|---|
| 1433 | else
|
|---|
| 1434 | if (fZd2 && !fZd2->IsZombieNode())
|
|---|
| 1435 | ressezd = fZd2->GetPhysRes();
|
|---|
| 1436 | else
|
|---|
| 1437 | ressezd = env.GetValue("Zd_ResSE[se/U_mot]", 16384);
|
|---|
| 1438 |
|
|---|
| 1439 | Double_t resseaz = 0;
|
|---|
| 1440 | if (fAz && !fAz->IsZombieNode())
|
|---|
| 1441 | resseaz = fAz->GetPhysRes();
|
|---|
| 1442 | else
|
|---|
| 1443 | resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384);
|
|---|
| 1444 |
|
|---|
| 1445 | kGearRatio.Set (gzd*resrezd*4/ressezd, gaz*resreaz*4/resseaz); //[re/se]
|
|---|
| 1446 | kGearRatio2.Set(gzd*resrezd*4/360.0, gaz*resreaz*4/360.0); //[re/deg]
|
|---|
| 1447 | cout << "done." << endl;
|
|---|
| 1448 |
|
|---|
| 1449 | cout << " * Setting Gear Ratios:" << endl;
|
|---|
| 1450 | cout << " --------------------" << endl;
|
|---|
| 1451 | cout << " * X: " << gzd << "*" << resrezd << "/" << ressezd << "=4*" << kGearRatio.X() << endl;
|
|---|
| 1452 | cout << " * Y: " << gaz << "*" << resreaz << "/" << resseaz << "=4*" << kGearRatio.Y() << endl;
|
|---|
| 1453 | }
|
|---|
| 1454 |
|
|---|
| 1455 | void MCosy::InitSync()
|
|---|
| 1456 | {
|
|---|
| 1457 | if (!fMac3)
|
|---|
| 1458 | {
|
|---|
| 1459 | lout << "Unable to Init Sync! Mac3 not available." << endl;
|
|---|
| 1460 | return;
|
|---|
| 1461 | }
|
|---|
| 1462 |
|
|---|
| 1463 | const int res = fMac3->GetVelRes();
|
|---|
| 1464 |
|
|---|
| 1465 | fMac3->SetVelocity(0.3*res);
|
|---|
| 1466 | fMac3->SetAcceleration(0.2*res);
|
|---|
| 1467 | fMac3->SetDeceleration(0.2*res);
|
|---|
| 1468 | fMac3->StartPosSync();
|
|---|
| 1469 | }
|
|---|
| 1470 |
|
|---|
| 1471 | void MCosy::TalkThreadTracking()
|
|---|
| 1472 | {
|
|---|
| 1473 | if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
|
|---|
| 1474 | return;
|
|---|
| 1475 |
|
|---|
| 1476 | if (fAz->IsZombieNode())
|
|---|
| 1477 | return;
|
|---|
| 1478 |
|
|---|
| 1479 | if (!fMac1 || !fMac2)
|
|---|
| 1480 | return;
|
|---|
| 1481 |
|
|---|
| 1482 | lout << "Tracking Thread started..." << endl;
|
|---|
| 1483 |
|
|---|
| 1484 | SlaStars sla(fObservatory);
|
|---|
| 1485 | sla.Now();
|
|---|
| 1486 |
|
|---|
| 1487 | ZdAz old;
|
|---|
| 1488 | ZdAz ist = GetSePos(); // [se]
|
|---|
| 1489 |
|
|---|
| 1490 | ZdAz time;
|
|---|
| 1491 |
|
|---|
| 1492 | ZdAz sollzd = sla.CalcZdAz(fRaDec); // [rad]
|
|---|
| 1493 | ZdAz sollaz = sollzd; // [rad]
|
|---|
| 1494 |
|
|---|
| 1495 | //
|
|---|
| 1496 | // only update fTrackingError while tracking
|
|---|
| 1497 | //
|
|---|
| 1498 | bool phca1=false;
|
|---|
| 1499 | bool phca2=false;
|
|---|
| 1500 | bool phcaz=false;
|
|---|
| 1501 |
|
|---|
| 1502 | while (fBackground==kBgdTracking)
|
|---|
| 1503 | {
|
|---|
| 1504 | //
|
|---|
| 1505 | // Make changes (eg wind) smoother - attenuation of control function
|
|---|
| 1506 | //
|
|---|
| 1507 | const float weight = 1.; //0.3;
|
|---|
| 1508 |
|
|---|
| 1509 | //
|
|---|
| 1510 | // This is the time constant which defines how fast
|
|---|
| 1511 | // you correct for external influences (like wind)
|
|---|
| 1512 | //
|
|---|
| 1513 | fZd1->ResetPosHasChanged();
|
|---|
| 1514 | fZd2->ResetPosHasChanged();
|
|---|
| 1515 | fAz->ResetPosHasChanged();
|
|---|
| 1516 | do
|
|---|
| 1517 | {
|
|---|
| 1518 | phca1 = fZd1->PosHasChanged();
|
|---|
| 1519 | phca2 = fZd2->PosHasChanged();
|
|---|
| 1520 | phcaz = fAz->PosHasChanged();
|
|---|
| 1521 | usleep(1);
|
|---|
| 1522 | } while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking);
|
|---|
| 1523 |
|
|---|
| 1524 | //---usleep(100000); // 0.1s
|
|---|
| 1525 |
|
|---|
| 1526 | //
|
|---|
| 1527 | // get position, where we are
|
|---|
| 1528 | //
|
|---|
| 1529 | old = ist;
|
|---|
| 1530 | ist = GetSePos(); // [se]
|
|---|
| 1531 |
|
|---|
| 1532 | //
|
|---|
| 1533 | // if the position didn't change continue
|
|---|
| 1534 | //
|
|---|
| 1535 | /*---
|
|---|
| 1536 | if ((int)ist.Zd() == (int)old.Zd() &&
|
|---|
| 1537 | (int)ist.Az() == (int)old.Az())
|
|---|
| 1538 | continue;
|
|---|
| 1539 | */
|
|---|
| 1540 | ZdAz istre = GetRePosPdo();
|
|---|
| 1541 |
|
|---|
| 1542 | //
|
|---|
| 1543 | // Get time from last shaftencoder position change (position: ist)
|
|---|
| 1544 | // FIXME: I cannot take the avarage
|
|---|
| 1545 | //
|
|---|
| 1546 | // FIXME
|
|---|
| 1547 | //time.Zd(fZd1->GetMjd());
|
|---|
| 1548 | /* OLD* */
|
|---|
| 1549 | if (fZd1->GetMjd()>fZd2->GetMjd())
|
|---|
| 1550 | time.Zd(fZd1->GetMjd());
|
|---|
| 1551 | else
|
|---|
| 1552 | time.Zd(fZd2->GetMjd());
|
|---|
| 1553 |
|
|---|
| 1554 | //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
|
|---|
| 1555 | time.Az(fAz->GetMjd());
|
|---|
| 1556 |
|
|---|
| 1557 | //
|
|---|
| 1558 | // if Shaftencoder changed position
|
|---|
| 1559 | // calculate were we should be
|
|---|
| 1560 | //
|
|---|
| 1561 | if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/)
|
|---|
| 1562 | {
|
|---|
| 1563 | sollzd = sla.CalcZdAz(fRaDec, time.Zd()); // [rad]
|
|---|
| 1564 | /*
|
|---|
| 1565 | ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
|
|---|
| 1566 | sollzd = CorrectTarget(ist, dummy); // [se]
|
|---|
| 1567 | */
|
|---|
| 1568 | fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight);
|
|---|
| 1569 | }
|
|---|
| 1570 |
|
|---|
| 1571 | if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
|
|---|
| 1572 | {
|
|---|
| 1573 | sollaz = sla.CalcZdAz(fRaDec, time.Az()); // [rad]
|
|---|
| 1574 | /*
|
|---|
| 1575 | ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
|
|---|
| 1576 | sollaz = CorrectTarget(ist, dummy); // [se]
|
|---|
| 1577 | */
|
|---|
| 1578 | fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
|
|---|
| 1579 | }
|
|---|
| 1580 |
|
|---|
| 1581 | ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad]
|
|---|
| 1582 |
|
|---|
| 1583 | AlignTrackingPos(soll*kRad2Deg, fZdAzSoll);
|
|---|
| 1584 |
|
|---|
| 1585 | ist *= TMath::Pi()*2/16384;
|
|---|
| 1586 | soll = fBending(fZdAzSoll);
|
|---|
| 1587 | fTrackingError.Set(ist.Zd()-soll.Zd(), ist.Az()-soll.Az());
|
|---|
| 1588 |
|
|---|
| 1589 | //--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
|
|---|
| 1590 | //--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " ";
|
|---|
| 1591 | //--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " ";
|
|---|
| 1592 | //--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl;
|
|---|
| 1593 | }
|
|---|
| 1594 |
|
|---|
| 1595 | lout << "Tracking Thread done." << endl;
|
|---|
| 1596 |
|
|---|
| 1597 | //--- fout << endl << endl;
|
|---|
| 1598 | }
|
|---|
| 1599 |
|
|---|
| 1600 | void MCosy::TalkThreadSeTest()
|
|---|
| 1601 | {
|
|---|
| 1602 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
|---|
| 1603 | // return;
|
|---|
| 1604 |
|
|---|
| 1605 | if (fHist)
|
|---|
| 1606 | {
|
|---|
| 1607 | lout << "You are much too fast... try again." << endl;
|
|---|
| 1608 | return;
|
|---|
| 1609 | }
|
|---|
| 1610 |
|
|---|
| 1611 | fHist = new TH2F("Diff", "Difference of SE values",
|
|---|
| 1612 | 201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
|
|---|
| 1613 | fHist->SetXTitle("ZA [\\circ]");
|
|---|
| 1614 | fHist->SetYTitle("\\Delta SE");
|
|---|
| 1615 |
|
|---|
| 1616 | Double_t offset = 0;
|
|---|
| 1617 |
|
|---|
| 1618 | int cnt = 0;
|
|---|
| 1619 |
|
|---|
| 1620 | lout << "Starting Shaftencoder Test..." << endl;
|
|---|
| 1621 |
|
|---|
| 1622 | while (fBackground==kBgdSeTest)
|
|---|
| 1623 | {
|
|---|
| 1624 | fZd1->ResetPosHasChanged();
|
|---|
| 1625 | fZd2->ResetPosHasChanged();
|
|---|
| 1626 |
|
|---|
| 1627 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
|---|
| 1628 | fBackground==kBgdSeTest)
|
|---|
| 1629 | usleep(1);
|
|---|
| 1630 |
|
|---|
| 1631 | const Double_t pos[3] = {
|
|---|
| 1632 | (fZd1->GetPos()+8192)%16384,
|
|---|
| 1633 | (fZd2->GetPos()+8192)%16384,
|
|---|
| 1634 | fAz->GetPos() };
|
|---|
| 1635 |
|
|---|
| 1636 | //
|
|---|
| 1637 | // Estimate Offset from the first ten positions
|
|---|
| 1638 | //
|
|---|
| 1639 | if (cnt++<10)
|
|---|
| 1640 | {
|
|---|
| 1641 | offset += pos[0]+pos[1];
|
|---|
| 1642 | continue;
|
|---|
| 1643 | }
|
|---|
| 1644 | if (cnt==11)
|
|---|
| 1645 | {
|
|---|
| 1646 | offset /= 10;
|
|---|
| 1647 | cnt++;
|
|---|
| 1648 | }
|
|---|
| 1649 |
|
|---|
| 1650 | Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
|
|---|
| 1651 |
|
|---|
| 1652 | ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
|
|---|
| 1653 | fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
|
|---|
| 1654 | }
|
|---|
| 1655 |
|
|---|
| 1656 | lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;
|
|---|
| 1657 |
|
|---|
| 1658 | fBackground=kBgdSeTestDispl;
|
|---|
| 1659 | }
|
|---|
| 1660 |
|
|---|
| 1661 | void MCosy::TalkThreadGear()
|
|---|
| 1662 | {
|
|---|
| 1663 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
|---|
| 1664 | // return;
|
|---|
| 1665 |
|
|---|
| 1666 | if (fHist)
|
|---|
| 1667 | {
|
|---|
| 1668 | lout << "You are much too fast... try again." << endl;
|
|---|
| 1669 | return;
|
|---|
| 1670 | }
|
|---|
| 1671 |
|
|---|
| 1672 | fHist = new TH3F("Gear", "Gear Ratio Re/Se",
|
|---|
| 1673 | (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(),
|
|---|
| 1674 | (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(),
|
|---|
| 1675 | 61, 349.5, 500.5);
|
|---|
| 1676 |
|
|---|
| 1677 | fHist->SetXTitle("Zd [\\circ]");
|
|---|
| 1678 | fHist->SetYTitle("Az [\\circ]");
|
|---|
| 1679 | fHist->SetZTitle("Re/Se");
|
|---|
| 1680 |
|
|---|
| 1681 | lout << "Starting Gear determination..." << endl;
|
|---|
| 1682 |
|
|---|
| 1683 | ZdAz se0 = GetSePos();
|
|---|
| 1684 | ZdAz re0 = GetRePosPdo();
|
|---|
| 1685 |
|
|---|
| 1686 | while (fBackground==kBgdGear)
|
|---|
| 1687 | {
|
|---|
| 1688 | fZd1->ResetPosHasChanged();
|
|---|
| 1689 | fZd2->ResetPosHasChanged();
|
|---|
| 1690 | fAz->ResetPosHasChanged();
|
|---|
| 1691 |
|
|---|
| 1692 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
|---|
| 1693 | !fAz->PosHasChanged() && fBackground==kBgdGear)
|
|---|
| 1694 | usleep(1);
|
|---|
| 1695 |
|
|---|
| 1696 | ZdAz se = GetSePos();
|
|---|
| 1697 | ZdAz re = GetRePosPdo();
|
|---|
| 1698 |
|
|---|
| 1699 | ZdAz dse = se-se0;
|
|---|
| 1700 | ZdAz dre = re-re0;
|
|---|
| 1701 |
|
|---|
| 1702 | if (fabs(dse.Zd())*144>16384) // Each 2.5deg (144)
|
|---|
| 1703 | {
|
|---|
| 1704 | se0.Zd(se.Zd());
|
|---|
| 1705 | re0.Zd(re.Zd());
|
|---|
| 1706 |
|
|---|
| 1707 | se -= dse/2;
|
|---|
| 1708 |
|
|---|
| 1709 | ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
|
|---|
| 1710 | ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
|
|---|
| 1711 | }
|
|---|
| 1712 |
|
|---|
| 1713 | if (fabs(dse.Az())*144>16384) // Each 2.5deg (144)
|
|---|
| 1714 | {
|
|---|
| 1715 | se0.Az(se.Az());
|
|---|
| 1716 | re0.Az(re.Az());
|
|---|
| 1717 |
|
|---|
| 1718 | se -= dse/2;
|
|---|
| 1719 |
|
|---|
| 1720 | ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
|
|---|
| 1721 | ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az());
|
|---|
| 1722 | }
|
|---|
| 1723 | }
|
|---|
| 1724 | lout << "Gear Test Stopped... displaying Histogram." << endl;
|
|---|
| 1725 |
|
|---|
| 1726 | fBackground=kBgdGearDispl;
|
|---|
| 1727 | }
|
|---|
| 1728 |
|
|---|
| 1729 | void MCosy::TalkThread()
|
|---|
| 1730 | {
|
|---|
| 1731 | /* ========== FIXME? =============
|
|---|
| 1732 | if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
|
|---|
| 1733 | return;
|
|---|
| 1734 | */
|
|---|
| 1735 |
|
|---|
| 1736 | if (fMac1 && fMac2)
|
|---|
| 1737 | {
|
|---|
| 1738 | fMac1->ReqPos();
|
|---|
| 1739 | fMac2->ReqPos();
|
|---|
| 1740 | }
|
|---|
| 1741 |
|
|---|
| 1742 | InitSync();
|
|---|
| 1743 |
|
|---|
| 1744 | /*** FOR DEMO MODE ***/
|
|---|
| 1745 | if (!fZd1 || !fZd2 || !fAz)
|
|---|
| 1746 | return;
|
|---|
| 1747 | /*** FOR DEMO MODE ***/
|
|---|
| 1748 |
|
|---|
| 1749 | //
|
|---|
| 1750 | // Start the Network
|
|---|
| 1751 | //
|
|---|
| 1752 | while (1)
|
|---|
| 1753 | {
|
|---|
| 1754 | //
|
|---|
| 1755 | // wait until a tracking session is started
|
|---|
| 1756 | //
|
|---|
| 1757 | while (fBackground==kBgdNone)
|
|---|
| 1758 | usleep(1);
|
|---|
| 1759 |
|
|---|
| 1760 | switch (fBackground)
|
|---|
| 1761 | {
|
|---|
| 1762 | case kBgdNone:
|
|---|
| 1763 | continue;
|
|---|
| 1764 |
|
|---|
| 1765 | case kBgdTracking:
|
|---|
| 1766 | TalkThreadTracking();
|
|---|
| 1767 | continue;
|
|---|
| 1768 |
|
|---|
| 1769 | case kBgdSeTest:
|
|---|
| 1770 | TalkThreadSeTest();
|
|---|
| 1771 | continue;
|
|---|
| 1772 |
|
|---|
| 1773 | case kBgdGear:
|
|---|
| 1774 | TalkThreadGear();
|
|---|
| 1775 | continue;
|
|---|
| 1776 |
|
|---|
| 1777 | default:
|
|---|
| 1778 | continue;
|
|---|
| 1779 | }
|
|---|
| 1780 | }
|
|---|
| 1781 | }
|
|---|
| 1782 |
|
|---|
| 1783 | Bool_t MCosy::HandleTimer(TTimer *t)
|
|---|
| 1784 | {
|
|---|
| 1785 | //
|
|---|
| 1786 | // Update Gui, foremer MTGui.
|
|---|
| 1787 | //
|
|---|
| 1788 | if (fZd1)
|
|---|
| 1789 | fZd1->DisplayVal();
|
|---|
| 1790 | if (fZd2)
|
|---|
| 1791 | fZd2->DisplayVal();
|
|---|
| 1792 | if (fAz)
|
|---|
| 1793 | fAz->DisplayVal();
|
|---|
| 1794 |
|
|---|
| 1795 | ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
|
|---|
| 1796 | ZdAz bendist = fBending.CorrectBack(seist);
|
|---|
| 1797 |
|
|---|
| 1798 | Byte_t avail = 0;
|
|---|
| 1799 |
|
|---|
| 1800 | avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
|
|---|
| 1801 | avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
|
|---|
| 1802 | avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
|
|---|
| 1803 | avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0;
|
|---|
| 1804 | avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0;
|
|---|
| 1805 | avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0;
|
|---|
| 1806 |
|
|---|
| 1807 | if (HasError())
|
|---|
| 1808 | SetStatus(MCosy::kError);
|
|---|
| 1809 |
|
|---|
| 1810 | lout.UpdateGui();
|
|---|
| 1811 |
|
|---|
| 1812 | fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError,
|
|---|
| 1813 | fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail);
|
|---|
| 1814 |
|
|---|
| 1815 | const Bool_t trigger = fTriggerDisplay;
|
|---|
| 1816 | fTriggerDisplay = kFALSE;
|
|---|
| 1817 |
|
|---|
| 1818 | if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
|
|---|
| 1819 | DisplayHistTestSe(!trigger);
|
|---|
| 1820 |
|
|---|
| 1821 | if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
|
|---|
| 1822 | DisplayHistGear(!trigger);
|
|---|
| 1823 |
|
|---|
| 1824 | return kTRUE;
|
|---|
| 1825 | }
|
|---|
| 1826 |
|
|---|
| 1827 | void MCosy::DisplayHistTestSe(Bool_t del)
|
|---|
| 1828 | {
|
|---|
| 1829 | lout << "Displaying histogram..." << endl;
|
|---|
| 1830 |
|
|---|
| 1831 | TH2F &hist = *(TH2F*)fHist;
|
|---|
| 1832 |
|
|---|
| 1833 | if (del)
|
|---|
| 1834 | {
|
|---|
| 1835 | fHist = NULL;
|
|---|
| 1836 | fBackground = kBgdNone;
|
|---|
| 1837 | }
|
|---|
| 1838 |
|
|---|
| 1839 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
|---|
| 1840 | c->Divide(1,2);
|
|---|
| 1841 |
|
|---|
| 1842 | c->cd(1);
|
|---|
| 1843 | TH2 *h=(TH2*)hist.DrawCopy();
|
|---|
| 1844 |
|
|---|
| 1845 | TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1846 | p->SetLineColor(kBlue);
|
|---|
| 1847 | p->Draw("same");
|
|---|
| 1848 | p->SetBit(kCanDelete);
|
|---|
| 1849 |
|
|---|
| 1850 | c->cd(2);
|
|---|
| 1851 |
|
|---|
| 1852 | TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
|
|---|
| 1853 | hist.GetBinLowEdge(hist.GetNbinsX()+1));
|
|---|
| 1854 | p2.SetXTitle("Zd [\\circ]");
|
|---|
| 1855 | for (int i=0; i<hist.GetNbinsX(); i++)
|
|---|
| 1856 | p2.SetBinError(i, p->GetBinError(i));
|
|---|
| 1857 | p2.SetLineColor(kRed);
|
|---|
| 1858 | p2.SetStats(0);
|
|---|
| 1859 | p2.DrawCopy();
|
|---|
| 1860 |
|
|---|
| 1861 | if (del)
|
|---|
| 1862 | delete &hist;
|
|---|
| 1863 | }
|
|---|
| 1864 |
|
|---|
| 1865 | void MCosy::DisplayHistGear(Bool_t del)
|
|---|
| 1866 | {
|
|---|
| 1867 | lout << "Displaying histogram..." << endl;
|
|---|
| 1868 |
|
|---|
| 1869 | TH3F &hist = *(TH3F*)fHist;
|
|---|
| 1870 |
|
|---|
| 1871 | if (del)
|
|---|
| 1872 | {
|
|---|
| 1873 | fHist = NULL;
|
|---|
| 1874 | fBackground = kBgdNone;
|
|---|
| 1875 | }
|
|---|
| 1876 |
|
|---|
| 1877 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
|---|
| 1878 | c->Divide(2,2);
|
|---|
| 1879 |
|
|---|
| 1880 | // ----------
|
|---|
| 1881 |
|
|---|
| 1882 | c->cd(1);
|
|---|
| 1883 | TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd
|
|---|
| 1884 | h1.SetTitle(" Gear Ratio Zenith Distance [re/se] ");
|
|---|
| 1885 | h1.SetXTitle("Zd [\\circ]");
|
|---|
| 1886 | h1.Draw();
|
|---|
| 1887 | h1.SetBit(kCanDelete);
|
|---|
| 1888 |
|
|---|
| 1889 | TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1890 | p1->SetLineColor(kBlue);
|
|---|
| 1891 | p1->Draw("same");
|
|---|
| 1892 | p1->SetBit(kCanDelete);
|
|---|
| 1893 |
|
|---|
| 1894 | // ----------
|
|---|
| 1895 |
|
|---|
| 1896 | c->cd(2);
|
|---|
| 1897 | TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az
|
|---|
| 1898 | h2.SetTitle(" Gear Ratio Azimuth [re/se] ");
|
|---|
| 1899 | h2.SetXTitle("Zd [\\circ]");
|
|---|
| 1900 | h2.Draw();
|
|---|
| 1901 | h2.SetBit(kCanDelete);
|
|---|
| 1902 |
|
|---|
| 1903 | TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1904 | p2->SetLineColor(kBlue);
|
|---|
| 1905 | p2->Draw("same");
|
|---|
| 1906 | p2->SetBit(kCanDelete);
|
|---|
| 1907 |
|
|---|
| 1908 | // ----------
|
|---|
| 1909 |
|
|---|
| 1910 | c->cd(3);
|
|---|
| 1911 |
|
|---|
| 1912 | TAxis &axe1 = *h1.GetXaxis();
|
|---|
| 1913 |
|
|---|
| 1914 | TH1F f1("spreadzd", " Spread Zenith Distance ",
|
|---|
| 1915 | axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax());
|
|---|
| 1916 | f1.SetXTitle("Zd [\\circ]");
|
|---|
| 1917 | for (int i=0; i<axe1.GetNbins(); i++)
|
|---|
| 1918 | f1.SetBinError(i, p1->GetBinError(i));
|
|---|
| 1919 | f1.SetLineColor(kRed);
|
|---|
| 1920 | f1.SetStats(0);
|
|---|
| 1921 | f1.DrawCopy();
|
|---|
| 1922 |
|
|---|
| 1923 | c->cd(4);
|
|---|
| 1924 |
|
|---|
| 1925 | // ----------
|
|---|
| 1926 |
|
|---|
| 1927 | TAxis &axe2 = *h2.GetXaxis();
|
|---|
| 1928 |
|
|---|
| 1929 | TH1F f2("spreadaz", " Spread Azimuth ",
|
|---|
| 1930 | axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax());
|
|---|
| 1931 | f2.SetXTitle("Az [\\circ]");
|
|---|
| 1932 | for (int i=0; i<axe2.GetNbins(); i++)
|
|---|
| 1933 | f2.SetBinError(i, p2->GetBinError(i));
|
|---|
| 1934 | f2.SetLineColor(kRed);
|
|---|
| 1935 | f2.SetStats(0);
|
|---|
| 1936 | f2.DrawCopy();
|
|---|
| 1937 |
|
|---|
| 1938 | // ----------
|
|---|
| 1939 |
|
|---|
| 1940 | if (del)
|
|---|
| 1941 | delete &hist;
|
|---|
| 1942 | }
|
|---|
| 1943 |
|
|---|
| 1944 | // --------------------------------------------------------------------------
|
|---|
| 1945 | //
|
|---|
| 1946 | // Start the work of the application:
|
|---|
| 1947 | //
|
|---|
| 1948 | // Start the Can-Network.
|
|---|
| 1949 | // Start the MCosy::TalkThread thread.
|
|---|
| 1950 | // turn on the gui update
|
|---|
| 1951 | //
|
|---|
| 1952 | void MCosy::Start()
|
|---|
| 1953 | {
|
|---|
| 1954 | // Don't call this function twice!
|
|---|
| 1955 | Network::Start();
|
|---|
| 1956 |
|
|---|
| 1957 | ReadConfig();
|
|---|
| 1958 |
|
|---|
| 1959 | lout << "- Starting TX Thread." << endl;
|
|---|
| 1960 | fTTalk = new MTTalk(this);
|
|---|
| 1961 |
|
|---|
| 1962 | lout << "- Starting GUI update." << endl;
|
|---|
| 1963 | fUpdateGui->TurnOn();
|
|---|
| 1964 | }
|
|---|
| 1965 |
|
|---|
| 1966 | // --------------------------------------------------------------------------
|
|---|
| 1967 | //
|
|---|
| 1968 | // Start the work of the application:
|
|---|
| 1969 | //
|
|---|
| 1970 | // Turn of the gui update
|
|---|
| 1971 | // stop the MCosy::TalkThread thread.
|
|---|
| 1972 | // Stop the network
|
|---|
| 1973 | //
|
|---|
| 1974 | void MCosy::Stop()
|
|---|
| 1975 | {
|
|---|
| 1976 | lout << "- Stopping GUI update." << endl;
|
|---|
| 1977 | fUpdateGui->TurnOff();
|
|---|
| 1978 | lout << "- GUI Update stopped." << endl;
|
|---|
| 1979 |
|
|---|
| 1980 | delete fTTalk;
|
|---|
| 1981 | lout << "- TX Thread stopped." << endl;
|
|---|
| 1982 |
|
|---|
| 1983 | Network::Stop();
|
|---|
| 1984 | }
|
|---|
| 1985 |
|
|---|
| 1986 | // --------------------------------------------------------------------------
|
|---|
| 1987 | //
|
|---|
| 1988 | // Disable the synchronization by using a negative CAN Id for id2.
|
|---|
| 1989 | //
|
|---|
| 1990 | void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
|
|---|
| 1991 | Int_t id4, Int_t id5, Int_t id6)
|
|---|
| 1992 | {
|
|---|
| 1993 | //
|
|---|
| 1994 | // Create Nodes
|
|---|
| 1995 | //
|
|---|
| 1996 | lout << "- Setting up network." << endl;
|
|---|
| 1997 |
|
|---|
| 1998 | fMac1=new Macs(id1, "Mac/Az", lout);
|
|---|
| 1999 | fMac2=new Macs(id3, "Mac/Zd", lout);
|
|---|
| 2000 | if (id2>=0)
|
|---|
| 2001 | fMac3=new Macs(id2, "Mac/Az-Sync", lout);
|
|---|
| 2002 |
|
|---|
| 2003 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
|---|
| 2004 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
|---|
| 2005 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
|---|
| 2006 |
|
|---|
| 2007 | lout << "- Connecting devices to network." << endl;
|
|---|
| 2008 |
|
|---|
| 2009 | //
|
|---|
| 2010 | // Connect the devices to the network
|
|---|
| 2011 | //
|
|---|
| 2012 | SetNode(fMac1);
|
|---|
| 2013 | SetNode(fMac2);
|
|---|
| 2014 | if (id2>=0)
|
|---|
| 2015 | SetNode(fMac3);
|
|---|
| 2016 | SetNode(fZd1);
|
|---|
| 2017 | SetNode(fZd2);
|
|---|
| 2018 | SetNode(fAz);
|
|---|
| 2019 |
|
|---|
| 2020 | //
|
|---|
| 2021 | // Create Gui Event timer and Gui
|
|---|
| 2022 | //
|
|---|
| 2023 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 2024 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 2025 |
|
|---|
| 2026 | lout << "- Starting GUI." << endl;
|
|---|
| 2027 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 2028 | }
|
|---|
| 2029 |
|
|---|
| 2030 | void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
|
|---|
| 2031 | {
|
|---|
| 2032 | //
|
|---|
| 2033 | // Create Nodes
|
|---|
| 2034 | //
|
|---|
| 2035 | lout << "- Setting up network." << endl;
|
|---|
| 2036 |
|
|---|
| 2037 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
|---|
| 2038 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
|---|
| 2039 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
|---|
| 2040 |
|
|---|
| 2041 | lout << "- Connecting devices to network." << endl;
|
|---|
| 2042 |
|
|---|
| 2043 | //
|
|---|
| 2044 | // Connect the devices to the network
|
|---|
| 2045 | //
|
|---|
| 2046 | SetNode(fZd1);
|
|---|
| 2047 | SetNode(fZd2);
|
|---|
| 2048 | SetNode(fAz);
|
|---|
| 2049 |
|
|---|
| 2050 | //
|
|---|
| 2051 | // Create Gui Event timer and Gui
|
|---|
| 2052 | //
|
|---|
| 2053 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 2054 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 2055 |
|
|---|
| 2056 | lout << "- Starting GUI." << endl;
|
|---|
| 2057 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 2058 | }
|
|---|
| 2059 |
|
|---|
| 2060 | void MCosy::ConstructorDemo()
|
|---|
| 2061 | {
|
|---|
| 2062 | //
|
|---|
| 2063 | // Create Nodes
|
|---|
| 2064 | //
|
|---|
| 2065 | lout << "- Setting up network." << endl;
|
|---|
| 2066 |
|
|---|
| 2067 | //
|
|---|
| 2068 | // Create Gui Event timer and Gui
|
|---|
| 2069 | //
|
|---|
| 2070 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 2071 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 2072 |
|
|---|
| 2073 | lout << "- Starting GUI." << endl;
|
|---|
| 2074 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 2075 | }
|
|---|
| 2076 |
|
|---|
| 2077 | MCosy::MCosy(int mode, const char *dev, const int baud, MLog &out)
|
|---|
| 2078 | : Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone)
|
|---|
| 2079 | {
|
|---|
| 2080 | TEnv env(".cosyrc");
|
|---|
| 2081 | const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
|
|---|
| 2082 | const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
|
|---|
| 2083 | const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3
|
|---|
| 2084 | const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4
|
|---|
| 2085 | const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5
|
|---|
| 2086 | const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6
|
|---|
| 2087 |
|
|---|
| 2088 | lout << "- Program in ";
|
|---|
| 2089 | switch (mode)
|
|---|
| 2090 | {
|
|---|
| 2091 | case 0:
|
|---|
| 2092 | lout << "<<Stanard mode>>" << endl;
|
|---|
| 2093 | fBending.Load("bending.txt");
|
|---|
| 2094 | Constructor(id1, id2, id3, id4, id5, id6);
|
|---|
| 2095 | break;
|
|---|
| 2096 | case 1:
|
|---|
| 2097 | lout << "<<SE mode>>" << endl;
|
|---|
| 2098 | fBending.Load("bending.txt");
|
|---|
| 2099 | ConstructorSE(id4, id5, id6);
|
|---|
| 2100 | break;
|
|---|
| 2101 | default:
|
|---|
| 2102 | lout << "<<Demo mode>>" << endl;
|
|---|
| 2103 | ConstructorDemo();
|
|---|
| 2104 | }
|
|---|
| 2105 |
|
|---|
| 2106 | lout.SetOutputGui(fWin->GetLog(), kTRUE);
|
|---|
| 2107 |
|
|---|
| 2108 | fZd1->SetDisplay(fWin->GetLabel2());
|
|---|
| 2109 | fZd2->SetDisplay(fWin->GetLabel3());
|
|---|
| 2110 | fAz->SetDisplay(fWin->GetLabel1());
|
|---|
| 2111 | #ifdef EXPERT
|
|---|
| 2112 | int i=0;
|
|---|
| 2113 | char name[100];
|
|---|
| 2114 | while (1)
|
|---|
| 2115 | {
|
|---|
| 2116 | sprintf(name, "tpoint/tpoint%03d.txt", i++);
|
|---|
| 2117 | if (gSystem->AccessPathName(name, kFileExists))
|
|---|
| 2118 | break;
|
|---|
| 2119 | }
|
|---|
| 2120 |
|
|---|
| 2121 | Timer time;
|
|---|
| 2122 | time.Now();
|
|---|
| 2123 |
|
|---|
| 2124 | cout << "TPoint File ********* " << name << " ********** " << endl;
|
|---|
| 2125 |
|
|---|
| 2126 | tpout = new ofstream(name);
|
|---|
| 2127 | *tpout << "Magic Model TPOINT data file" << endl;
|
|---|
| 2128 | *tpout << ": ALTAZ" << endl;
|
|---|
| 2129 | *tpout << "49 48 0 ";
|
|---|
| 2130 | *tpout << time.Year() << " " << time.Month() << " " << time.Day() << " ";
|
|---|
| 2131 | *tpout << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
|
|---|
| 2132 | #endif
|
|---|
| 2133 | // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
|
|---|
| 2134 | }
|
|---|
| 2135 |
|
|---|
| 2136 | void MCosy::TerminateApp()
|
|---|
| 2137 | {
|
|---|
| 2138 | cout << "MCosy::TerminateApp()" << endl;
|
|---|
| 2139 | /*
|
|---|
| 2140 | Int_t rc;
|
|---|
| 2141 | TGMessageBox msg(this, gClient->GetRoot(),
|
|---|
| 2142 | "Information",
|
|---|
| 2143 | "Cosy is shutting down the system - this may take wa while!",
|
|---|
| 2144 | kMBIconExclamation,
|
|---|
| 2145 | kMBOK, //kMBClose
|
|---|
| 2146 | &rc, 0);
|
|---|
| 2147 | */
|
|---|
| 2148 |
|
|---|
| 2149 | lout.DisableOutputDevice(MLog::eGui);
|
|---|
| 2150 | // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
|
|---|
| 2151 | // lout.SetOutputGui(NULL, kFALSE);
|
|---|
| 2152 |
|
|---|
| 2153 | gApplication->Terminate(0);
|
|---|
| 2154 | }
|
|---|
| 2155 |
|
|---|
| 2156 | MCosy::~MCosy()
|
|---|
| 2157 | {
|
|---|
| 2158 | #ifdef EXPERT
|
|---|
| 2159 | *tpout << "END" << endl;
|
|---|
| 2160 | delete tpout;
|
|---|
| 2161 | #endif
|
|---|
| 2162 |
|
|---|
| 2163 | cout << "Deleting GUI timer." << endl;
|
|---|
| 2164 |
|
|---|
| 2165 | delete fUpdateGui;
|
|---|
| 2166 |
|
|---|
| 2167 | cout << "Deleting Nodes." << endl;
|
|---|
| 2168 |
|
|---|
| 2169 | delete fAz;
|
|---|
| 2170 | delete fZd1;
|
|---|
| 2171 | delete fZd2;
|
|---|
| 2172 | delete fMac1;
|
|---|
| 2173 | delete fMac2;
|
|---|
| 2174 | if (fMac3)
|
|---|
| 2175 | delete fMac3;
|
|---|
| 2176 |
|
|---|
| 2177 | cout << "Deleting MGCosy." << endl;
|
|---|
| 2178 |
|
|---|
| 2179 | lout.DisableOutputDevice(MLog::eGui);
|
|---|
| 2180 |
|
|---|
| 2181 | delete fWin;
|
|---|
| 2182 |
|
|---|
| 2183 | cout << "MGCosy destructed." << endl;
|
|---|
| 2184 | }
|
|---|
