| 1 | #include "MCosy.h"
|
|---|
| 2 |
|
|---|
| 3 | #include <iomanip.h>
|
|---|
| 4 | #include <fstream.h>
|
|---|
| 5 | #include <iostream.h>
|
|---|
| 6 |
|
|---|
| 7 | #include <TROOT.h>
|
|---|
| 8 | #include <TEnv.h>
|
|---|
| 9 | #include <TSystem.h>
|
|---|
| 10 | #include <TApplication.h>
|
|---|
| 11 | #include <TTimer.h>
|
|---|
| 12 |
|
|---|
| 13 | #include <TH2.h>
|
|---|
| 14 | #include <TH3.h>
|
|---|
| 15 | #include <TProfile.h>
|
|---|
| 16 | #include <TCanvas.h>
|
|---|
| 17 |
|
|---|
| 18 | #include "MGCosy.h"
|
|---|
| 19 | #include "MTime.h"
|
|---|
| 20 | #include "MDriveCom.h"
|
|---|
| 21 | #include "MStarguider.h"
|
|---|
| 22 | #include "SlaStars.h"
|
|---|
| 23 | #include "MPointing.h"
|
|---|
| 24 | #include "MTracking.h"
|
|---|
| 25 |
|
|---|
| 26 | #include "slalib/slalib.h" // FIXME: REMOVE
|
|---|
| 27 |
|
|---|
| 28 | #include "macs.h"
|
|---|
| 29 | #include "shaftencoder.h"
|
|---|
| 30 |
|
|---|
| 31 | ClassImp(MCosy);
|
|---|
| 32 |
|
|---|
| 33 | typedef struct tm tm_t;
|
|---|
| 34 |
|
|---|
| 35 | /* +===================================+
|
|---|
| 36 | FIXME: What if fMac3 (Sync) died?
|
|---|
| 37 | +===================================+
|
|---|
| 38 | */
|
|---|
| 39 |
|
|---|
| 40 | //#define EXPERT
|
|---|
| 41 | #undef EXPERT
|
|---|
| 42 |
|
|---|
| 43 | /*
|
|---|
| 44 | ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
|
|---|
| 45 | {
|
|---|
| 46 | // CorrectTarget [se]
|
|---|
| 47 |
|
|---|
| 48 | // src [se]
|
|---|
| 49 | // dst [rad]
|
|---|
| 50 |
|
|---|
| 51 | // fAltMax = 70
|
|---|
| 52 | // fAltMin = -105/110
|
|---|
| 53 | // fAzMin = -355
|
|---|
| 54 | // fAzMax = 355
|
|---|
| 55 |
|
|---|
| 56 | ZdAz source = src * 360.0/16384.0;
|
|---|
| 57 | ZdAz dest = dst * kRad2Deg;
|
|---|
| 58 |
|
|---|
| 59 | if (dest.Zd()>-3 && dest.Zd()<3)
|
|---|
| 60 | dest.Zd(dest.Zd()<0?-3:3);
|
|---|
| 61 |
|
|---|
| 62 | if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
|
|---|
| 63 | return dst*(16384.0/k2Pi);
|
|---|
| 64 |
|
|---|
| 65 | const float fZdMin = -67;
|
|---|
| 66 | const float fZdMax = 67;
|
|---|
| 67 | const float fAzMin = -29;
|
|---|
| 68 | const float fAzMax = 423;
|
|---|
| 69 |
|
|---|
| 70 | //
|
|---|
| 71 | // This corrects to target for the shortest distance, not for the fastest move!
|
|---|
| 72 | //
|
|---|
| 73 | ZdAz s = source-dest;
|
|---|
| 74 |
|
|---|
| 75 | float min = s.Sqr();
|
|---|
| 76 |
|
|---|
| 77 | //
|
|---|
| 78 | // Is it enought to search inside one revolution?
|
|---|
| 79 | //
|
|---|
| 80 | ZdAz ret = dest;
|
|---|
| 81 |
|
|---|
| 82 | for (int i=-5; i<5+1; i++)
|
|---|
| 83 | {
|
|---|
| 84 | const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);
|
|---|
| 85 |
|
|---|
| 86 | //
|
|---|
| 87 | // Range Check
|
|---|
| 88 | //
|
|---|
| 89 | if (p.Zd()<fZdMin || p.Zd()>fZdMax)
|
|---|
| 90 | continue;
|
|---|
| 91 |
|
|---|
| 92 | if (p.Az()<fAzMin || p.Az()>fAzMax)
|
|---|
| 93 | continue;
|
|---|
| 94 |
|
|---|
| 95 | //
|
|---|
| 96 | // Calculate distance
|
|---|
| 97 | //
|
|---|
| 98 | s = source-p;
|
|---|
| 99 |
|
|---|
| 100 | const float dist = s.Sqr();
|
|---|
| 101 |
|
|---|
| 102 | if (dist > min)
|
|---|
| 103 | continue;
|
|---|
| 104 |
|
|---|
| 105 | //
|
|---|
| 106 | // New shortest distance
|
|---|
| 107 | //
|
|---|
| 108 | ret = p;
|
|---|
| 109 | min = dist;
|
|---|
| 110 | }
|
|---|
| 111 | return ret*(16384.0/360.0);
|
|---|
| 112 | }
|
|---|
| 113 | */
|
|---|
| 114 |
|
|---|
| 115 | // --------------------------------------------------------------------------
|
|---|
| 116 | //
|
|---|
| 117 | // GetSePos, reads the Shaftencoder positions from the Can-drivers
|
|---|
| 118 | // for the shaftencoders. The two shaft encoders at the elevation axis
|
|---|
| 119 | // are avaraged. The values are returned as a ZdAz object.
|
|---|
| 120 | //
|
|---|
| 121 | // If one of the two shaftencoders on the elevation axis is missing
|
|---|
| 122 | // the other one's position is returned.
|
|---|
| 123 | //
|
|---|
| 124 | // The positions are alway up-to-date because the shaftencoders are
|
|---|
| 125 | // sending all changes immediatly.
|
|---|
| 126 | //
|
|---|
| 127 | ZdAz MCosy::GetSePos() const
|
|---|
| 128 | {
|
|---|
| 129 | const int pa = fAz->GetPos();
|
|---|
| 130 | if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
|
|---|
| 131 | return ZdAz(0, pa);
|
|---|
| 132 |
|
|---|
| 133 | //
|
|---|
| 134 | // Get the values
|
|---|
| 135 | //
|
|---|
| 136 | int p1 = (fZd1->GetPos()+8192)%16384;
|
|---|
| 137 | int p2 = -(fZd2->GetPos()+8192)%16384;
|
|---|
| 138 |
|
|---|
| 139 | if (fZd1->IsZombieNode())
|
|---|
| 140 | return ZdAz(p2, pa);
|
|---|
| 141 | if (fZd2->IsZombieNode())
|
|---|
| 142 | return ZdAz(p1, pa);
|
|---|
| 143 |
|
|---|
| 144 | //
|
|---|
| 145 | // interpolate shaft encoder positions
|
|---|
| 146 | //
|
|---|
| 147 | float p = (float)(p1+p2)/2;
|
|---|
| 148 |
|
|---|
| 149 | return ZdAz(p, pa);
|
|---|
| 150 | }
|
|---|
| 151 |
|
|---|
| 152 | // --------------------------------------------------------------------------
|
|---|
| 153 | //
|
|---|
| 154 | // reads the Rotary encoder positions from the last request of the Macs.
|
|---|
| 155 | //
|
|---|
| 156 | // The positions are returned as a ZdAz object. Use RequestRePos to request
|
|---|
| 157 | // the current positions first.
|
|---|
| 158 | //
|
|---|
| 159 | ZdAz MCosy::GetRePos()
|
|---|
| 160 | {
|
|---|
| 161 | return ZdAz(fMac2->GetPos(), fMac1->GetPos());
|
|---|
| 162 | }
|
|---|
| 163 |
|
|---|
| 164 | // --------------------------------------------------------------------------
|
|---|
| 165 | //
|
|---|
| 166 | // reads the Rotary encoder positions from the Macs.
|
|---|
| 167 | //
|
|---|
| 168 | // The positions are returned as a ZdAz object. The positions are the ones
|
|---|
| 169 | // which are send as PDOs to the computer. This is done at a given
|
|---|
| 170 | // frequency. Which means, that this positions are not ought to be
|
|---|
| 171 | // up-to-date.
|
|---|
| 172 | //
|
|---|
| 173 | ZdAz MCosy::GetRePosPdo()
|
|---|
| 174 | {
|
|---|
| 175 | return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
|
|---|
| 176 | }
|
|---|
| 177 |
|
|---|
| 178 | // --------------------------------------------------------------------------
|
|---|
| 179 | //
|
|---|
| 180 | // check for a break-signal (from the msgqueue) and errors.
|
|---|
| 181 | //
|
|---|
| 182 | int MCosy::StopWaitingForSDO() const
|
|---|
| 183 | {
|
|---|
| 184 | return 0/*Break() || HasError()*/;
|
|---|
| 185 | }
|
|---|
| 186 |
|
|---|
| 187 | // --------------------------------------------------------------------------
|
|---|
| 188 | //
|
|---|
| 189 | // Waits for a movement to become finished.
|
|---|
| 190 | //
|
|---|
| 191 | // First waits for all peding Sdos, then waits until both motors are stopped
|
|---|
| 192 | // or waiting for SDOs was stopped (either by an error or by Break)
|
|---|
| 193 | //
|
|---|
| 194 | void MCosy::WaitForEndMovement()
|
|---|
| 195 | {
|
|---|
| 196 | // FIXME, what when waiting times out (Zombie)
|
|---|
| 197 | if (!fMac1 || !fMac2)
|
|---|
| 198 | return;
|
|---|
| 199 |
|
|---|
| 200 | while ((fMac1->IsPositioning() || fMac2->IsPositioning()) &&
|
|---|
| 201 | !(Break() || HasError() || HasZombie()))
|
|---|
| 202 | usleep(1);
|
|---|
| 203 |
|
|---|
| 204 | if (!Break() && !HasError() && !HasZombie())
|
|---|
| 205 | return;
|
|---|
| 206 |
|
|---|
| 207 | MTime t(-1);
|
|---|
| 208 | lout << t << " - MCosy::WaitForEndMovement aborted...";
|
|---|
| 209 | if (Break())
|
|---|
| 210 | lout << " Break signal...";
|
|---|
| 211 | if (HasError())
|
|---|
| 212 | lout << " Network has error...";
|
|---|
| 213 | if (HasZombie())
|
|---|
| 214 | lout << " Network has zombie...";
|
|---|
| 215 | lout << endl;
|
|---|
| 216 | }
|
|---|
| 217 |
|
|---|
| 218 | // --------------------------------------------------------------------------
|
|---|
| 219 | //
|
|---|
| 220 | // Check for an error...
|
|---|
| 221 | //
|
|---|
| 222 | // This is ment for usage after the Action: All Motors Stop.
|
|---|
| 223 | //
|
|---|
| 224 | void MCosy::CheckForError()
|
|---|
| 225 | {
|
|---|
| 226 | //
|
|---|
| 227 | // Check all Can-Nodes for an Error. If there is no error the motor
|
|---|
| 228 | // status is set to stopped.
|
|---|
| 229 | //
|
|---|
| 230 | if (HasError() || HasZombie())
|
|---|
| 231 | {
|
|---|
| 232 | SetStatus(MDriveCom::kError);
|
|---|
| 233 | return;
|
|---|
| 234 | }
|
|---|
| 235 |
|
|---|
| 236 | if (fMac1->IsPositioning() || fMac2->IsPositioning())
|
|---|
| 237 | SetStatus(MDriveCom::kMoving);
|
|---|
| 238 | else
|
|---|
| 239 | SetStatus(MDriveCom::kStopped);
|
|---|
| 240 |
|
|---|
| 241 | //
|
|---|
| 242 | // If there is an error, the error status is set to Error.
|
|---|
| 243 | //
|
|---|
| 244 |
|
|---|
| 245 | /*
|
|---|
| 246 | FIXME: HANDLINGE ERROR
|
|---|
| 247 |
|
|---|
| 248 | //
|
|---|
| 249 | // Now try to handle the error.
|
|---|
| 250 | //
|
|---|
| 251 | fMac1->HandleError();
|
|---|
| 252 | fMac2->HandleError();
|
|---|
| 253 |
|
|---|
| 254 | //
|
|---|
| 255 | // If the error couldn't get solved return
|
|---|
| 256 | //
|
|---|
| 257 | if (HasError())
|
|---|
| 258 | return;
|
|---|
| 259 |
|
|---|
| 260 | //
|
|---|
| 261 | // Set motor status to stopped
|
|---|
| 262 | //
|
|---|
| 263 | SetStatus(MDriveCom::kStopped);
|
|---|
| 264 | */
|
|---|
| 265 | }
|
|---|
| 266 |
|
|---|
| 267 | Bool_t MCosy::CheckRange(const ZdAz &d) const
|
|---|
| 268 | {
|
|---|
| 269 | // d [rad]
|
|---|
| 270 |
|
|---|
| 271 | if (d.Zd()<fMin.Zd())
|
|---|
| 272 | {
|
|---|
| 273 | lout << "ERROR: Requested Zenith Angle below negative endswitch." << endl;
|
|---|
| 274 | return kFALSE;
|
|---|
| 275 | }
|
|---|
| 276 |
|
|---|
| 277 | if (d.Zd()>fMax.Zd())
|
|---|
| 278 | {
|
|---|
| 279 | lout << "ERROR: Requested Zenith Angle behind positive endswitch." << endl;
|
|---|
| 280 | return kFALSE;
|
|---|
| 281 | }
|
|---|
| 282 |
|
|---|
| 283 | if (d.Az()<fMin.Az())
|
|---|
| 284 | {
|
|---|
| 285 | lout << "ERROR: Requested Azimuth Angle below negative endswitch." << endl;
|
|---|
| 286 | return kFALSE;
|
|---|
| 287 | }
|
|---|
| 288 |
|
|---|
| 289 | if (d.Az()>fMax.Az())
|
|---|
| 290 | {
|
|---|
| 291 | lout << "ERROR: Requested Azimuth Angle behind positive endswitch." << endl;
|
|---|
| 292 | return kFALSE;
|
|---|
| 293 | }
|
|---|
| 294 |
|
|---|
| 295 |
|
|---|
| 296 | return kTRUE;
|
|---|
| 297 | }
|
|---|
| 298 |
|
|---|
| 299 | ZdAz MCosy::AlignTrackingPos(ZdAz pointing) const
|
|---|
| 300 | {
|
|---|
| 301 | // pointing [rad]
|
|---|
| 302 | // AlignTrackingPos [deg]
|
|---|
| 303 |
|
|---|
| 304 | pointing *= kRad2Deg;
|
|---|
| 305 |
|
|---|
| 306 | if (pointing.Zd()<0)
|
|---|
| 307 | {
|
|---|
| 308 | pointing.Zd(-pointing.Zd());
|
|---|
| 309 | pointing.Az(pointing.Az()+180);
|
|---|
| 310 | //lout << "ZD=-ZD Az+=180" << endl;
|
|---|
| 311 | }
|
|---|
| 312 |
|
|---|
| 313 | const ZdAz se = GetSePos()*TMath::TwoPi()/kResSE; // [rad]
|
|---|
| 314 | const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing
|
|---|
| 315 |
|
|---|
| 316 | //lout << "Unbended: " << unbendedse.Zd() << " " << unbendedse.Az() << endl;
|
|---|
| 317 |
|
|---|
| 318 | do
|
|---|
| 319 | {
|
|---|
| 320 | const Double_t d = unbendedse.Az() - pointing.Az();
|
|---|
| 321 | if (d>-180 && d<=180)
|
|---|
| 322 | break;
|
|---|
| 323 |
|
|---|
| 324 | //lout << "AZ += " << TMath::Sign(360., d) << endl;
|
|---|
| 325 |
|
|---|
| 326 | pointing.Az(pointing.Az()+TMath::Sign(360., d));
|
|---|
| 327 | } while (1);
|
|---|
| 328 |
|
|---|
| 329 | return pointing/kRad2Deg;
|
|---|
| 330 | /*
|
|---|
| 331 | const Bool_t rc = CheckRange(pointing);
|
|---|
| 332 | za = pointing/kRad2Deg; // [rad]
|
|---|
| 333 |
|
|---|
| 334 | if (!rc)
|
|---|
| 335 | lout << "Error: Aligned position out of Range." << endl;
|
|---|
| 336 |
|
|---|
| 337 | return rc;*/
|
|---|
| 338 | }
|
|---|
| 339 |
|
|---|
| 340 | Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
|
|---|
| 341 | {
|
|---|
| 342 | ifstream fin("pointingpos.txt");
|
|---|
| 343 | if (!fin)
|
|---|
| 344 | return -1;
|
|---|
| 345 |
|
|---|
| 346 | Double_t mjd0, zd, az;
|
|---|
| 347 | fin >> mjd0 >> zd >> az;
|
|---|
| 348 |
|
|---|
| 349 | mjd0 += 52000;
|
|---|
| 350 |
|
|---|
| 351 | if (mjd0+1./24/60 <mjd)
|
|---|
| 352 | return -1;
|
|---|
| 353 |
|
|---|
| 354 | ZdAz point=AlignTrackingPos(ZdAz(zd, az)/kRad2Deg);
|
|---|
| 355 | /*
|
|---|
| 356 | if (!AlignTrackingPos(ZdAz(zd, az), point))
|
|---|
| 357 | {
|
|---|
| 358 | cout << "Starguider position couldn't be aligned..." << endl;
|
|---|
| 359 | return -1;
|
|---|
| 360 | }*/
|
|---|
| 361 |
|
|---|
| 362 | // FIXME: Check Range missing!
|
|---|
| 363 |
|
|---|
| 364 | const ZdAz diff = (dest-point)*kRad2Deg;
|
|---|
| 365 |
|
|---|
| 366 | if (diff.Zd()>5 || diff.Az()>5)
|
|---|
| 367 | {
|
|---|
| 368 | cout << "Starguider deviation too large... dZd=" << diff.Zd() <<" dAz="<<diff.Az() << endl;
|
|---|
| 369 | return -1;
|
|---|
| 370 | }
|
|---|
| 371 |
|
|---|
| 372 | dest -= point;
|
|---|
| 373 | dest *= -kGearTot/TMath::TwoPi(); // [re]
|
|---|
| 374 |
|
|---|
| 375 | cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl;
|
|---|
| 376 |
|
|---|
| 377 | return (mjd-mjd0) * (24*60*60); // [s]
|
|---|
| 378 | }
|
|---|
| 379 |
|
|---|
| 380 | // --------------------------------------------------------------------------
|
|---|
| 381 | //
|
|---|
| 382 | // Move the telescope to the given position. The position must be given in
|
|---|
| 383 | // a ZdAz object in rad.
|
|---|
| 384 | //
|
|---|
| 385 | // The first positioning is done absolutely. If we didn't reach the
|
|---|
| 386 | // correct psotion we try to correct for this by 10 relative position
|
|---|
| 387 | // maneuvers. If this doesn't help positioning failed.
|
|---|
| 388 | //
|
|---|
| 389 | // As a reference the shaftencoder values are used.
|
|---|
| 390 | //
|
|---|
| 391 | int MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
|
|---|
| 392 | {
|
|---|
| 393 | MPointing point(this, lout);
|
|---|
| 394 |
|
|---|
| 395 | //#ifdef EXPERT
|
|---|
| 396 | // point.SetAccDec(0.4, 0.4);
|
|---|
| 397 | // point.SetVelocity(0.2); // fast: 0.6, slow: 0.2
|
|---|
| 398 | //#else
|
|---|
| 399 |
|
|---|
| 400 | // original settings
|
|---|
| 401 | //
|
|---|
| 402 | point.SetPointAccDec(0.2, 0.1);
|
|---|
| 403 | point.SetPointVelocity(0.1);
|
|---|
| 404 |
|
|---|
| 405 |
|
|---|
| 406 | //#endif
|
|---|
| 407 |
|
|---|
| 408 | // point.SetPointAccDec(0.4, 0.4);
|
|---|
| 409 | // point.SetPointVelocity(0.4);
|
|---|
| 410 |
|
|---|
| 411 |
|
|---|
| 412 | // original
|
|---|
| 413 | return point.SetPosition(dst, track);
|
|---|
| 414 |
|
|---|
| 415 | // test
|
|---|
| 416 | // return point.SetPosition(dst, kTRUE);
|
|---|
| 417 |
|
|---|
| 418 |
|
|---|
| 419 |
|
|---|
| 420 | }
|
|---|
| 421 |
|
|---|
| 422 | void MCosy::SetTrackingPosRE(ZdAz za)
|
|---|
| 423 | {
|
|---|
| 424 | za /= kGearTot; // [U_tel]
|
|---|
| 425 | za *= TMath::TwoPi(); // [rad]
|
|---|
| 426 | //cout << "RE1: " << za.Zd()*180/3.1415 << " " << za.Az()*180/3.1415 << endl;
|
|---|
| 427 | fTrackingPosRaw = za*TMath::RadToDeg();
|
|---|
| 428 | fTrackingPos = fBending.CorrectBack(za)*TMath::RadToDeg();
|
|---|
| 429 | //cout << "RE2: " << fTrackingPos.Zd() << " " << fTrackingPos.Az() << endl;
|
|---|
| 430 | }
|
|---|
| 431 |
|
|---|
| 432 | void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
|
|---|
| 433 | {
|
|---|
| 434 | MTracking track(this, lout);
|
|---|
| 435 | track.SetOut(fOutRep);
|
|---|
| 436 | //#ifdef EXPERT
|
|---|
| 437 | // track.SetPointAccDec(0.4, 0.4);
|
|---|
| 438 | // track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
|
|---|
| 439 | //#else
|
|---|
| 440 |
|
|---|
| 441 |
|
|---|
| 442 |
|
|---|
| 443 | track.SetPointAccDec(0.2, 0.1);
|
|---|
| 444 | track.SetPointVelocity(0.1);
|
|---|
| 445 |
|
|---|
| 446 | // track.SetPointAccDec(0.4, 0.4);
|
|---|
| 447 | // track.SetPointVelocity(0.4);
|
|---|
| 448 |
|
|---|
| 449 |
|
|---|
| 450 |
|
|---|
| 451 | //#endif
|
|---|
| 452 | track.SetTrackAccDec(0.1, 0.1);
|
|---|
| 453 |
|
|---|
| 454 | track.TrackPosition(dst);
|
|---|
| 455 | }
|
|---|
| 456 |
|
|---|
| 457 | void MCosy::TrackPositionGRB(const RaDec &dst) // ra, dec [rad]
|
|---|
| 458 | {
|
|---|
| 459 | MTracking track(this, lout);
|
|---|
| 460 | track.SetOut(fOutRep);
|
|---|
| 461 | //#ifdef EXPERT
|
|---|
| 462 | // track.SetPointAccDec(0.4, 0.4);
|
|---|
| 463 | // track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
|
|---|
| 464 | //#else
|
|---|
| 465 | track.SetPointAccDec(0.4, 0.4);
|
|---|
| 466 | track.SetPointVelocity(0.3);
|
|---|
| 467 | //#endif
|
|---|
| 468 | track.SetTrackAccDec(0.1, 0.1);
|
|---|
| 469 |
|
|---|
| 470 | track.TrackPosition(dst);
|
|---|
| 471 | }
|
|---|
| 472 |
|
|---|
| 473 | // --------------------------------------------------------------------------
|
|---|
| 474 | //
|
|---|
| 475 | // Stops the movement of both motors.
|
|---|
| 476 | //
|
|---|
| 477 | // Sets the status to stopping. Sets the deceleration to 50% of the maximum.
|
|---|
| 478 | // stops. Quits the revolution mode and wait for the end of the movement.
|
|---|
| 479 | //
|
|---|
| 480 | void MCosy::StopMovement()
|
|---|
| 481 | {
|
|---|
| 482 | //
|
|---|
| 483 | // Set status to Stopping
|
|---|
| 484 | //
|
|---|
| 485 | SetStatus(MDriveCom::kStopping);
|
|---|
| 486 |
|
|---|
| 487 | //
|
|---|
| 488 | // set deceleration to 50%
|
|---|
| 489 | //
|
|---|
| 490 | cout << "Stopping movement (dec=30%)..." << endl;
|
|---|
| 491 | if (fMac1 && fMac2)
|
|---|
| 492 | {
|
|---|
| 493 | #ifdef EXPERT
|
|---|
| 494 | fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
|
|---|
| 495 | fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
|
|---|
| 496 | #else
|
|---|
| 497 | fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
|
|---|
| 498 | fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
|
|---|
| 499 | #endif
|
|---|
| 500 | fMac1->SetRpmMode(FALSE);
|
|---|
| 501 | fMac2->SetRpmMode(FALSE);
|
|---|
| 502 | }
|
|---|
| 503 |
|
|---|
| 504 | /*
|
|---|
| 505 | fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
|
|---|
| 506 | fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
|
|---|
| 507 |
|
|---|
| 508 | fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
|
|---|
| 509 | fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
|
|---|
| 510 | fMac2->WaitForSdo(0x3000, 0);
|
|---|
| 511 | fMac1->WaitForSdo(0x3000, 0);
|
|---|
| 512 | fMac1->SetRpmMode(FALSE);
|
|---|
| 513 | fMac2->SetRpmMode(FALSE);
|
|---|
| 514 | */
|
|---|
| 515 |
|
|---|
| 516 | //
|
|---|
| 517 | // Wait for the movement to really be finished.
|
|---|
| 518 | //
|
|---|
| 519 | #ifdef EXPERT
|
|---|
| 520 | cout << "Waiting for end of movement..." << endl;
|
|---|
| 521 | #endif
|
|---|
| 522 | WaitForEndMovement();
|
|---|
| 523 |
|
|---|
| 524 | //
|
|---|
| 525 | // Check whether everything works fine.
|
|---|
| 526 | //
|
|---|
| 527 | CheckForError();
|
|---|
| 528 | #ifdef EXPERT
|
|---|
| 529 | cout << "Movement stopped." << endl;
|
|---|
| 530 | #endif
|
|---|
| 531 | }
|
|---|
| 532 |
|
|---|
| 533 | bool MCosy::CheckNetwork()
|
|---|
| 534 | {
|
|---|
| 535 | //return kTRUE;
|
|---|
| 536 | //CheckConnections();
|
|---|
| 537 |
|
|---|
| 538 | CheckForError();
|
|---|
| 539 |
|
|---|
| 540 | if (HasZombie())
|
|---|
| 541 | {
|
|---|
| 542 | lout << "- Found Zombies in Network..." << endl;
|
|---|
| 543 | if (!RebootZombies())
|
|---|
| 544 | return false;
|
|---|
| 545 | }
|
|---|
| 546 |
|
|---|
| 547 | /*
|
|---|
| 548 | FIXME HANDLING ERROR
|
|---|
| 549 | */
|
|---|
| 550 | if (HasError())
|
|---|
| 551 | {
|
|---|
| 552 | fMac1->HandleError();
|
|---|
| 553 | fMac2->HandleError();
|
|---|
| 554 | fMac3->HandleError();
|
|---|
| 555 | if (HasError() || HasZombie())
|
|---|
| 556 | return false;
|
|---|
| 557 | }
|
|---|
| 558 |
|
|---|
| 559 | CheckForError();
|
|---|
| 560 | return true;
|
|---|
| 561 | }
|
|---|
| 562 |
|
|---|
| 563 | void *MCosy::Proc(int msg, void *mp)
|
|---|
| 564 | {
|
|---|
| 565 | switch (msg)
|
|---|
| 566 | {
|
|---|
| 567 | case WM_WAIT:
|
|---|
| 568 | cout << "Wait for execution of Proc(WM_*, ): done." << endl;
|
|---|
| 569 | return NULL;
|
|---|
| 570 |
|
|---|
| 571 | case WM_STOP:
|
|---|
| 572 | //cout << "MCosy::Proc: Stop." << endl;
|
|---|
| 573 | if (!CheckNetwork())
|
|---|
| 574 | return (void*)0xebb0;
|
|---|
| 575 | StopMovement();
|
|---|
| 576 | return NULL;
|
|---|
| 577 | /*
|
|---|
| 578 | case WM_PRESET:
|
|---|
| 579 | cout << "WM_Preset: start." << endl;
|
|---|
| 580 | if (!CheckNetwork())
|
|---|
| 581 | return (void*)0xebb0;
|
|---|
| 582 | fZd1->SetPreset();
|
|---|
| 583 | fZd2->SetPreset();
|
|---|
| 584 | fAz->SetPreset();
|
|---|
| 585 | cout << "WM_Preset: done. (return 0xaffe)" << endl;
|
|---|
| 586 | return (void*)0xaffe;
|
|---|
| 587 | */
|
|---|
| 588 | /*
|
|---|
| 589 | case WM_CALIB:
|
|---|
| 590 | {
|
|---|
| 591 | cout << "WM_Calib: start." << endl;
|
|---|
| 592 | if (!CheckNetwork())
|
|---|
| 593 | return (void*)0xebb0;
|
|---|
| 594 |
|
|---|
| 595 | SlaStars sla(fObservatory);
|
|---|
| 596 | sla.Now();
|
|---|
| 597 |
|
|---|
| 598 | RaDec rd = *((RaDec*)mp);
|
|---|
| 599 |
|
|---|
| 600 | //RaDec rd(37.94, 89.2644); // POLARIS
|
|---|
| 601 | //RaDec rd(213.915417, 19.1825); // ARCTURUS
|
|---|
| 602 |
|
|---|
| 603 | cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
|
|---|
| 604 |
|
|---|
| 605 | ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;
|
|---|
| 606 |
|
|---|
| 607 | cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
|
|---|
| 608 |
|
|---|
| 609 | ZdAz sepos = GetSePos();
|
|---|
| 610 | cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
|
|---|
| 611 |
|
|---|
| 612 | fZd1->SetPreset(za.Zd());
|
|---|
| 613 | fZd2->SetPreset(-za.Zd());
|
|---|
| 614 | fAz->SetPreset(za.Az());
|
|---|
| 615 |
|
|---|
| 616 | cout << "WM_Calib: done. (return 0xaffe)" << endl;
|
|---|
| 617 | }
|
|---|
| 618 | return (void*)0xaffe;
|
|---|
| 619 | */
|
|---|
| 620 | case WM_TPOINT:
|
|---|
| 621 | {
|
|---|
| 622 | //cout << "WM_TPoint: start." << endl;
|
|---|
| 623 | SlaStars sla(fObservatory);
|
|---|
| 624 | sla.Now();
|
|---|
| 625 |
|
|---|
| 626 | RaDec rd = *((RaDec*)mp);
|
|---|
| 627 | cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
|
|---|
| 628 |
|
|---|
| 629 | AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;
|
|---|
| 630 |
|
|---|
| 631 | if (!fOutTp)
|
|---|
| 632 | {
|
|---|
| 633 | //
|
|---|
| 634 | // open tpoint file
|
|---|
| 635 | //
|
|---|
| 636 | const TString name = GetFileName("tpoint/tpoint_%s.txt");
|
|---|
| 637 | cout << "TPoint-Cosy File ********* " << name << " ********** " << endl;
|
|---|
| 638 |
|
|---|
| 639 | fOutTp = new ofstream(name);
|
|---|
| 640 | *fOutTp << "Magic Model TPOINT data file" << endl;
|
|---|
| 641 | *fOutTp << ": ALTAZ" << endl;
|
|---|
| 642 | *fOutTp << "49 48 0 ";
|
|---|
| 643 | *fOutTp << sla.GetTime().Year() << " " << sla.GetTime().Month() << " " << sla.GetTime().Day() << " ";
|
|---|
| 644 | *fOutTp << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
|
|---|
| 645 | // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
|
|---|
| 646 | }
|
|---|
| 647 |
|
|---|
| 648 | cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
|---|
| 649 | *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " ";
|
|---|
| 650 |
|
|---|
| 651 | ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE;
|
|---|
| 652 | za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
|
|---|
| 653 | za *= kRad2Deg;
|
|---|
| 654 |
|
|---|
| 655 | cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
|---|
| 656 | *fOutTp << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";
|
|---|
| 657 |
|
|---|
| 658 | if (fStarguider)
|
|---|
| 659 | {
|
|---|
| 660 | XY tp = fStarguider->GetCoordinates();
|
|---|
| 661 | *fOutTp << 90-tp.X() << " " << tp.Y() << " ";
|
|---|
| 662 | }
|
|---|
| 663 |
|
|---|
| 664 | *fOutTp << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;
|
|---|
| 665 |
|
|---|
| 666 | //cout << "WM_TPoint: done. (return 0xaffe)" << endl;
|
|---|
| 667 | }
|
|---|
| 668 | return (void*)0xca1b;
|
|---|
| 669 |
|
|---|
| 670 | case WM_TRACKPOS:
|
|---|
| 671 | //cout << "WM_TrackPosition: start." << endl;
|
|---|
| 672 | {
|
|---|
| 673 | if (!CheckNetwork())
|
|---|
| 674 | return (void*)0xebb0;
|
|---|
| 675 |
|
|---|
| 676 | ZdAz dest = *((ZdAz*)mp) * kDeg2Rad;
|
|---|
| 677 | if (!SetPosition(dest))
|
|---|
| 678 | return (void*)0x1234;
|
|---|
| 679 |
|
|---|
| 680 | SlaStars sla(fObservatory);
|
|---|
| 681 | sla.Now();
|
|---|
| 682 |
|
|---|
| 683 | RaDec rd = sla.CalcRaDec(dest);
|
|---|
| 684 | TrackPosition(rd);
|
|---|
| 685 | }
|
|---|
| 686 | //cout << "WM_TrackPosition: done. (return 0xabcd)" << endl;
|
|---|
| 687 | return (void*)0xabcd;
|
|---|
| 688 |
|
|---|
| 689 | case WM_POSITION:
|
|---|
| 690 | //cout << "WM_Position: start." << endl;
|
|---|
| 691 | {
|
|---|
| 692 | if (!CheckNetwork())
|
|---|
| 693 | return (void*)0xebb0;
|
|---|
| 694 |
|
|---|
| 695 | ZdAz dest = *((ZdAz*)mp);
|
|---|
| 696 | SetPosition(dest*kDeg2Rad);
|
|---|
| 697 | }
|
|---|
| 698 | //cout << "WM_Position: done. (return 0x7777)" << endl;
|
|---|
| 699 | return (void*)0x7777;
|
|---|
| 700 |
|
|---|
| 701 | case WM_POSITION1:
|
|---|
| 702 | //cout << "WM_Position1: start." << endl;
|
|---|
| 703 | {
|
|---|
| 704 | if (!CheckNetwork())
|
|---|
| 705 | return (void*)0xebb0;
|
|---|
| 706 |
|
|---|
| 707 | ZdAz dest = *((ZdAz*)mp);
|
|---|
| 708 | SetPosition(dest*kDeg2Rad, kTRUE);
|
|---|
| 709 | }
|
|---|
| 710 | //cout << "WM_Position: done. (return 0x7777)" << endl;
|
|---|
| 711 | return (void*)0x7777;
|
|---|
| 712 |
|
|---|
| 713 | case WM_TESTSE:
|
|---|
| 714 | //cout << "WM_TestSe: start." << endl;
|
|---|
| 715 | fBackground = mp ? kBgdSeTest : kBgdNone;
|
|---|
| 716 | //cout << "WM_TestSe: done. (return 0x1e51)" << endl;
|
|---|
| 717 | return (void*)0x1e51;
|
|---|
| 718 |
|
|---|
| 719 | case WM_GEAR:
|
|---|
| 720 | //cout << "WM_Gear: start." << endl;
|
|---|
| 721 | fBackground = mp ? kBgdGear : kBgdNone;
|
|---|
| 722 | //cout << "WM_Gear: done. (return 0xfeaf)" << endl;
|
|---|
| 723 | return (void*)0xfeaf;
|
|---|
| 724 |
|
|---|
| 725 | case WM_DISPLAY:
|
|---|
| 726 | //cout << "WM_Display: start." << endl;
|
|---|
| 727 | fTriggerDisplay = kTRUE;
|
|---|
| 728 | //cout << "WM_Disply: done. (return 0xd1e1)" << endl;
|
|---|
| 729 | return (void*)0xd1e1;
|
|---|
| 730 |
|
|---|
| 731 | case WM_TRACK:
|
|---|
| 732 | //cout << "WM_Track: START" << endl;
|
|---|
| 733 | {
|
|---|
| 734 | RaDec dest = ((RaDec*)mp)[0];
|
|---|
| 735 | if (fStarguider)
|
|---|
| 736 | fStarguider->SetPointingPosition(((RaDec*)mp)[1]);
|
|---|
| 737 | if (!CheckNetwork())
|
|---|
| 738 | return (void*)0xebb0;
|
|---|
| 739 | TrackPosition(dest*kDeg2Rad);
|
|---|
| 740 | }
|
|---|
| 741 | //cout << "WM_Track: done. (return 0x8888)" << endl;
|
|---|
| 742 | return (void*)0x8888;
|
|---|
| 743 |
|
|---|
| 744 | case WM_GRB:
|
|---|
| 745 | //cout << "WM_Track: START" << endl;
|
|---|
| 746 | {
|
|---|
| 747 | RaDec dest = ((RaDec*)mp)[0];
|
|---|
| 748 | if (fStarguider)
|
|---|
| 749 | fStarguider->SetPointingPosition(((RaDec*)mp)[1]);
|
|---|
| 750 | if (!CheckNetwork())
|
|---|
| 751 | return (void*)0xebb0;
|
|---|
| 752 | TrackPositionGRB(dest*kDeg2Rad);
|
|---|
| 753 | }
|
|---|
| 754 | //cout << "WM_Track: done. (return 0x8888)" << endl;
|
|---|
| 755 | return (void*)0x8888;
|
|---|
| 756 |
|
|---|
| 757 | case WM_NEWTRACK:
|
|---|
| 758 | //cout << "WM_NewTrack: START" << endl;
|
|---|
| 759 | fRaDec = *((RaDec*)mp);
|
|---|
| 760 | //cout << "WM_NewTrack: done. (return 0x9999)" << endl;
|
|---|
| 761 | return (void*)0x9999;
|
|---|
| 762 |
|
|---|
| 763 | case WM_LOADBENDING:
|
|---|
| 764 | //cout << "WM_LoadBending: START" << endl;
|
|---|
| 765 | fBending.Load("bending.txt");
|
|---|
| 766 | //cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
|
|---|
| 767 | return (void*)0xbe0d;
|
|---|
| 768 |
|
|---|
| 769 | case WM_RESETBENDING:
|
|---|
| 770 | //cout << "WM_ResetBending: START" << endl;
|
|---|
| 771 | fBending.Reset();
|
|---|
| 772 | //cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
|
|---|
| 773 | return (void*)0xbe0e;
|
|---|
| 774 |
|
|---|
| 775 | case WM_HOME:
|
|---|
| 776 | //cout << "WM_Home: START" << endl;
|
|---|
| 777 | if (!CheckNetwork())
|
|---|
| 778 | return (void*)0xebb0;
|
|---|
| 779 | else
|
|---|
| 780 | {
|
|---|
| 781 | cout << "HOME NOT ALLOWED... for Magic." << endl;
|
|---|
| 782 | /*
|
|---|
| 783 | cout << "Going Home..." << endl;
|
|---|
| 784 | TEnv env(".cosyrc");
|
|---|
| 785 |
|
|---|
| 786 | SetStatus(MDriveCom::kMoving);
|
|---|
| 787 |
|
|---|
| 788 | fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
|
|---|
| 789 | fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
|
|---|
| 790 |
|
|---|
| 791 | lout << "SETHOME DONE" << endl;
|
|---|
| 792 |
|
|---|
| 793 | SetStatus(HasError() ? MDriveCom::kError : MDriveCom::kStopped);
|
|---|
| 794 |
|
|---|
| 795 | fAz->SetPreset();
|
|---|
| 796 | fZd1->SetPreset();
|
|---|
| 797 | fZd2->SetPreset();
|
|---|
| 798 |
|
|---|
| 799 | fMac1->ReqPos();
|
|---|
| 800 | fMac2->ReqPos();
|
|---|
| 801 | fMac3->StopMotor();
|
|---|
| 802 | */
|
|---|
| 803 | }
|
|---|
| 804 | //cout << "WM_Home: done. (return 0x403e)" << endl;
|
|---|
| 805 | return (void*)0x403e;
|
|---|
| 806 |
|
|---|
| 807 | case WM_CALCALTAZ:
|
|---|
| 808 | {
|
|---|
| 809 | cout << endl;
|
|---|
| 810 |
|
|---|
| 811 | SlaStars sla(fObservatory);
|
|---|
| 812 | sla.Now();
|
|---|
| 813 |
|
|---|
| 814 | XY xy = *((XY*)mp);
|
|---|
| 815 | RaDec rd(xy.X()*15., xy.Y()); // [deg]
|
|---|
| 816 |
|
|---|
| 817 | ZdAz a1 = sla.CalcZdAz(rd*kDeg2Rad); // [rad]
|
|---|
| 818 |
|
|---|
| 819 | cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
|
|---|
| 820 | cout << "Zd/Az target: " << a1.Zd()*kRad2Deg << "° " << a1.Az()*kRad2Deg << "°" << endl;
|
|---|
| 821 |
|
|---|
| 822 | if (fZd1 && fZd2 && fAz)
|
|---|
| 823 | a1 = AlignTrackingPos(a1);
|
|---|
| 824 |
|
|---|
| 825 | a1 = fBending(a1);
|
|---|
| 826 | CheckRange(a1);
|
|---|
| 827 | a1 *= kRad2Deg;
|
|---|
| 828 |
|
|---|
| 829 | const ZdAz a2 = a1*kResSE/360;
|
|---|
| 830 |
|
|---|
| 831 | cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
|
|---|
| 832 | cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
|
|---|
| 833 | }
|
|---|
| 834 | return (void*)0xa17a;
|
|---|
| 835 |
|
|---|
| 836 | case WM_ENDSWITCH:
|
|---|
| 837 | {
|
|---|
| 838 | ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE;
|
|---|
| 839 | pos = fBending.SubtractOffsets(pos)*kRad2Deg;
|
|---|
| 840 |
|
|---|
| 841 | cout << "Endswitch Position: Zd=" << pos.Zd() << "° Az=";
|
|---|
| 842 | cout << pos.Az() << "°" << endl;
|
|---|
| 843 | }
|
|---|
| 844 |
|
|---|
| 845 | return (void*)0x1010;
|
|---|
| 846 |
|
|---|
| 847 | case WM_QUIT:
|
|---|
| 848 | cout << "WM_Quit: now." << endl;
|
|---|
| 849 | if (!CheckNetwork())
|
|---|
| 850 | {
|
|---|
| 851 | lout << "ERROR: Cannot shutdown CANbus network." << endl;
|
|---|
| 852 | return (void*)0xebb0;
|
|---|
| 853 | }
|
|---|
| 854 | TerminateApp();
|
|---|
| 855 | cout << "WM_Quit: done." << endl;
|
|---|
| 856 | return (void*)0xaaaa;
|
|---|
| 857 | }
|
|---|
| 858 | cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
|
|---|
| 859 | return (void*)0xffffffff;
|
|---|
| 860 | }
|
|---|
| 861 |
|
|---|
| 862 | void *MTTalk::Thread()
|
|---|
| 863 | {
|
|---|
| 864 | fCosy->TalkThread();
|
|---|
| 865 | return NULL;
|
|---|
| 866 | }
|
|---|
| 867 |
|
|---|
| 868 | void MCosy::ReadConfig()
|
|---|
| 869 | {
|
|---|
| 870 | cout << "Reading configuration file..." << flush;
|
|---|
| 871 | TEnv env(".cosyrc");
|
|---|
| 872 | cout << "done." << endl;
|
|---|
| 873 |
|
|---|
| 874 | cout << "Reading telescope range..." << flush;
|
|---|
| 875 | const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
|
|---|
| 876 | const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
|
|---|
| 877 | fMin.Set(zmin, amin);
|
|---|
| 878 |
|
|---|
| 879 | const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
|
|---|
| 880 | const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
|
|---|
| 881 | fMax.Set(zmax, amax);
|
|---|
| 882 | cout << "done." << endl;
|
|---|
| 883 |
|
|---|
| 884 | cout << " * Min: " << zmin << "deg " << amin << "deg" << endl;
|
|---|
| 885 | cout << " * Max: " << zmax << "deg " << amax << "deg" << endl;
|
|---|
| 886 |
|
|---|
| 887 | fMin = fBending.AddOffsets(fMin/kRad2Deg);
|
|---|
| 888 | fMax = fBending.AddOffsets(fMax/kRad2Deg);
|
|---|
| 889 |
|
|---|
| 890 | cout << " * Min': " << fMin.Zd()*kRad2Deg << "deg " << fMin.Az()*kRad2Deg << "deg" << endl;
|
|---|
| 891 | cout << " * Max': " << fMax.Zd()*kRad2Deg << "deg " << fMax.Az()*kRad2Deg << "deg" << endl;
|
|---|
| 892 |
|
|---|
| 893 | cout << "Reading gear ratios..." << flush;
|
|---|
| 894 | kGear.X(env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0));
|
|---|
| 895 | kGear.Y(env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0));
|
|---|
| 896 |
|
|---|
| 897 | kResRE.Y(0);
|
|---|
| 898 | if (fMac1 && !fMac1->IsZombieNode())
|
|---|
| 899 | kResRE.Y(fMac1->GetRes());
|
|---|
| 900 | else
|
|---|
| 901 | if (fMac3 && !fMac3->IsZombieNode())
|
|---|
| 902 | kResRE.Y(fMac3->GetRes());
|
|---|
| 903 | else
|
|---|
| 904 | kResRE.Y(env.GetValue("Az_ResRE[re/U_mot]", 1500));
|
|---|
| 905 |
|
|---|
| 906 | kResRE.X(0);
|
|---|
| 907 | if (fMac2 && !fMac2->IsZombieNode())
|
|---|
| 908 | kResRE.X(fMac2->GetRes());
|
|---|
| 909 | else
|
|---|
| 910 | kResRE.X(env.GetValue("Zd_ResRE[re/U_mot]", 1500));
|
|---|
| 911 |
|
|---|
| 912 | kResSE.X(0);
|
|---|
| 913 | if (fZd1 && !fZd1->IsZombieNode())
|
|---|
| 914 | kResSE.X(fZd1->GetPhysRes());
|
|---|
| 915 | else
|
|---|
| 916 | if (fZd2 && !fZd2->IsZombieNode())
|
|---|
| 917 | kResSE.X(fZd2->GetPhysRes());
|
|---|
| 918 | else
|
|---|
| 919 | kResSE.X(env.GetValue("Zd_ResSE[se/U_mot]", 16384));
|
|---|
| 920 |
|
|---|
| 921 | kResSE.Y(0);
|
|---|
| 922 | if (fAz && !fAz->IsZombieNode())
|
|---|
| 923 | kResSE.Y(fAz->GetPhysRes());
|
|---|
| 924 | else
|
|---|
| 925 | kResSE.Y(env.GetValue("Az_ResSE[se/U_mot]", 16384));
|
|---|
| 926 |
|
|---|
| 927 | // believing the Macs manual '*4' shouldn't be necessary, but it is.
|
|---|
| 928 | // Because the a RE is 4 quad counts.
|
|---|
| 929 | // Calculating speeds we have to convert back to qc
|
|---|
| 930 | kResRE *= 4;
|
|---|
| 931 | kGearTot = kResRE*kGear;
|
|---|
| 932 |
|
|---|
| 933 | cout << "done." << endl;
|
|---|
| 934 |
|
|---|
| 935 | cout << " * Setting Gear Ratios:" << endl;
|
|---|
| 936 | cout << " --------------------" << endl;
|
|---|
| 937 | cout << " * X: " << kGear.X() << "*" << kResRE.X()/4 << "/" << kResSE.X() << "=4*" << kGearTot.X() << "/" << kResSE.X() << endl;
|
|---|
| 938 | cout << " * Y: " << kGear.Y() << "*" << kResRE.Y()/4 << "/" << kResSE.Y() << "=4*" << kGearTot.Y() << "/" << kResSE.Y() << endl;
|
|---|
| 939 | }
|
|---|
| 940 | /*
|
|---|
| 941 | void MCosy::InitSync()
|
|---|
| 942 | {
|
|---|
| 943 | if (!fMac3)
|
|---|
| 944 | {
|
|---|
| 945 | lout << "Unable to Init Sync! Mac3 not available." << endl;
|
|---|
| 946 | return;
|
|---|
| 947 | }
|
|---|
| 948 |
|
|---|
| 949 | const int res = fMac3->GetVelRes();
|
|---|
| 950 |
|
|---|
| 951 | fMac3->SetVelocity(0.3*res);
|
|---|
| 952 | fMac3->SetAcceleration(0.2*res);
|
|---|
| 953 | fMac3->SetDeceleration(0.2*res);
|
|---|
| 954 | fMac3->StartPosSync();
|
|---|
| 955 | }
|
|---|
| 956 | */
|
|---|
| 957 | void MCosy::TalkThreadSeTest()
|
|---|
| 958 | {
|
|---|
| 959 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
|---|
| 960 | // return;
|
|---|
| 961 |
|
|---|
| 962 | if (fHist)
|
|---|
| 963 | {
|
|---|
| 964 | lout << "You are much too fast... try again." << endl;
|
|---|
| 965 | return;
|
|---|
| 966 | }
|
|---|
| 967 |
|
|---|
| 968 | fHist = new TH2F("Diff", "Difference of SE values",
|
|---|
| 969 | 201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
|
|---|
| 970 | fHist->SetXTitle("ZA [\\circ]");
|
|---|
| 971 | fHist->SetYTitle("\\Delta SE");
|
|---|
| 972 |
|
|---|
| 973 | Double_t offset = 0;
|
|---|
| 974 |
|
|---|
| 975 | int cnt = 0;
|
|---|
| 976 |
|
|---|
| 977 | lout << "Starting Shaftencoder Test..." << endl;
|
|---|
| 978 |
|
|---|
| 979 | while (fBackground==kBgdSeTest)
|
|---|
| 980 | {
|
|---|
| 981 | fZd1->ResetPosHasChanged();
|
|---|
| 982 | fZd2->ResetPosHasChanged();
|
|---|
| 983 |
|
|---|
| 984 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
|---|
| 985 | fBackground==kBgdSeTest)
|
|---|
| 986 | usleep(1);
|
|---|
| 987 |
|
|---|
| 988 | const Double_t pos[3] = {
|
|---|
| 989 | (fZd1->GetPos()+8192)%16384,
|
|---|
| 990 | (fZd2->GetPos()+8192)%16384,
|
|---|
| 991 | fAz->GetPos() };
|
|---|
| 992 |
|
|---|
| 993 | //
|
|---|
| 994 | // Estimate Offset from the first ten positions
|
|---|
| 995 | //
|
|---|
| 996 | if (cnt++<10)
|
|---|
| 997 | {
|
|---|
| 998 | offset += pos[0]+pos[1];
|
|---|
| 999 | continue;
|
|---|
| 1000 | }
|
|---|
| 1001 | if (cnt==11)
|
|---|
| 1002 | {
|
|---|
| 1003 | offset /= 10;
|
|---|
| 1004 | cnt++;
|
|---|
| 1005 | }
|
|---|
| 1006 |
|
|---|
| 1007 | Double_t apos = (pos[0]-pos[1])/2 * TMath::TwoPi() / kResSE.X();
|
|---|
| 1008 |
|
|---|
| 1009 | ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
|
|---|
| 1010 | fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
|
|---|
| 1011 | }
|
|---|
| 1012 |
|
|---|
| 1013 | lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;
|
|---|
| 1014 |
|
|---|
| 1015 | fBackground=kBgdSeTestDispl;
|
|---|
| 1016 | }
|
|---|
| 1017 |
|
|---|
| 1018 | void MCosy::TalkThreadGear()
|
|---|
| 1019 | {
|
|---|
| 1020 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
|---|
| 1021 | // return;
|
|---|
| 1022 |
|
|---|
| 1023 | if (fHist)
|
|---|
| 1024 | {
|
|---|
| 1025 | lout << "You are much too fast... try again." << endl;
|
|---|
| 1026 | return;
|
|---|
| 1027 | }
|
|---|
| 1028 |
|
|---|
| 1029 | fHist = new TH3F("Gear", "Gear Ratio Re/Se",
|
|---|
| 1030 | (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(),
|
|---|
| 1031 | (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(),
|
|---|
| 1032 | 61, 349.5, 500.5);
|
|---|
| 1033 |
|
|---|
| 1034 | fHist->SetXTitle("Zd [\\circ]");
|
|---|
| 1035 | fHist->SetYTitle("Az [\\circ]");
|
|---|
| 1036 | fHist->SetZTitle("Re/Se");
|
|---|
| 1037 |
|
|---|
| 1038 | lout << "Starting Gear determination..." << endl;
|
|---|
| 1039 |
|
|---|
| 1040 | ZdAz se0 = GetSePos();
|
|---|
| 1041 | ZdAz re0 = GetRePosPdo();
|
|---|
| 1042 |
|
|---|
| 1043 | while (fBackground==kBgdGear)
|
|---|
| 1044 | {
|
|---|
| 1045 | fZd1->ResetPosHasChanged();
|
|---|
| 1046 | fZd2->ResetPosHasChanged();
|
|---|
| 1047 | fAz->ResetPosHasChanged();
|
|---|
| 1048 |
|
|---|
| 1049 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
|---|
| 1050 | !fAz->PosHasChanged() && fBackground==kBgdGear)
|
|---|
| 1051 | usleep(1);
|
|---|
| 1052 |
|
|---|
| 1053 | ZdAz se = GetSePos();
|
|---|
| 1054 | ZdAz re = GetRePosPdo();
|
|---|
| 1055 |
|
|---|
| 1056 | ZdAz dse = se-se0;
|
|---|
| 1057 | ZdAz dre = re-re0;
|
|---|
| 1058 |
|
|---|
| 1059 | if (fabs(dse.Zd())*144>kResSE.X()) // Each 2.5deg (144)
|
|---|
| 1060 | {
|
|---|
| 1061 | se0.Zd(se.Zd());
|
|---|
| 1062 | re0.Zd(re.Zd());
|
|---|
| 1063 |
|
|---|
| 1064 | se -= dse/2;
|
|---|
| 1065 |
|
|---|
| 1066 | ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg;
|
|---|
| 1067 | ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
|
|---|
| 1068 | }
|
|---|
| 1069 |
|
|---|
| 1070 | if (fabs(dse.Az())*144>kResSE.Y()) // Each 2.5deg (144)
|
|---|
| 1071 | {
|
|---|
| 1072 | se0.Az(se.Az());
|
|---|
| 1073 | re0.Az(re.Az());
|
|---|
| 1074 |
|
|---|
| 1075 | se -= dse/2;
|
|---|
| 1076 |
|
|---|
| 1077 | ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg;
|
|---|
| 1078 | ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az());
|
|---|
| 1079 | }
|
|---|
| 1080 | }
|
|---|
| 1081 | lout << "Gear Test Stopped... displaying Histogram." << endl;
|
|---|
| 1082 |
|
|---|
| 1083 | fBackground=kBgdGearDispl;
|
|---|
| 1084 | }
|
|---|
| 1085 |
|
|---|
| 1086 | void MCosy::TalkThread()
|
|---|
| 1087 | {
|
|---|
| 1088 | /* ========== FIXME? =============
|
|---|
| 1089 | if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
|
|---|
| 1090 | return;
|
|---|
| 1091 | */
|
|---|
| 1092 |
|
|---|
| 1093 | if (fMac1 && fMac2)
|
|---|
| 1094 | {
|
|---|
| 1095 | fMac1->ReqPos();
|
|---|
| 1096 | fMac2->ReqPos();
|
|---|
| 1097 | }
|
|---|
| 1098 |
|
|---|
| 1099 | //InitSync();
|
|---|
| 1100 |
|
|---|
| 1101 | /*** FOR DEMO MODE ***/
|
|---|
| 1102 | if (!fZd1 || !fZd2 || !fAz)
|
|---|
| 1103 | return;
|
|---|
| 1104 | /*** FOR DEMO MODE ***/
|
|---|
| 1105 |
|
|---|
| 1106 | //
|
|---|
| 1107 | // Start the Network
|
|---|
| 1108 | //
|
|---|
| 1109 | while (1)
|
|---|
| 1110 | {
|
|---|
| 1111 | //
|
|---|
| 1112 | // wait until a tracking session is started
|
|---|
| 1113 | //
|
|---|
| 1114 | while (fBackground==kBgdNone)
|
|---|
| 1115 | usleep(1);
|
|---|
| 1116 |
|
|---|
| 1117 | switch (fBackground)
|
|---|
| 1118 | {
|
|---|
| 1119 | case kBgdNone:
|
|---|
| 1120 | continue;
|
|---|
| 1121 | /*#ifndef NEWALGO
|
|---|
| 1122 | case kBgdTracking:
|
|---|
| 1123 | TalkThreadTracking();
|
|---|
| 1124 | continue;
|
|---|
| 1125 | #endif*/
|
|---|
| 1126 | case kBgdSeTest:
|
|---|
| 1127 | TalkThreadSeTest();
|
|---|
| 1128 | continue;
|
|---|
| 1129 |
|
|---|
| 1130 | case kBgdGear:
|
|---|
| 1131 | TalkThreadGear();
|
|---|
| 1132 | continue;
|
|---|
| 1133 |
|
|---|
| 1134 | default:
|
|---|
| 1135 | continue;
|
|---|
| 1136 | }
|
|---|
| 1137 | }
|
|---|
| 1138 | }
|
|---|
| 1139 |
|
|---|
| 1140 | ZdAz MCosy::GetPointingPos() const
|
|---|
| 1141 | {
|
|---|
| 1142 | if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
|
|---|
| 1143 | return ZdAz(0, 0);
|
|---|
| 1144 |
|
|---|
| 1145 | // GetPointingPos [deg]
|
|---|
| 1146 | const ZdAz seist = GetSePos()*TMath::TwoPi()/kResSE; // [rad]
|
|---|
| 1147 | return fBending.CorrectBack(seist)*TMath::RadToDeg();
|
|---|
| 1148 | }
|
|---|
| 1149 |
|
|---|
| 1150 | Bool_t MCosy::HandleTimer(TTimer *t)
|
|---|
| 1151 | {
|
|---|
| 1152 | const Int_t rc = fMutexGui.TryLock();
|
|---|
| 1153 | if (rc==13)
|
|---|
| 1154 | cout << "MCosy::HandleTimer - mutex is already locked by this thread" << endl;
|
|---|
| 1155 |
|
|---|
| 1156 | if (rc)
|
|---|
| 1157 | {
|
|---|
| 1158 | lout << "* GUI update skipped due to locked mutex." << endl;
|
|---|
| 1159 | return kTRUE;
|
|---|
| 1160 | }
|
|---|
| 1161 |
|
|---|
| 1162 | //
|
|---|
| 1163 | // Update Gui, foremer MTGui.
|
|---|
| 1164 | //
|
|---|
| 1165 | if (fZd1)
|
|---|
| 1166 | fZd1->DisplayVal();
|
|---|
| 1167 | if (fZd2)
|
|---|
| 1168 | fZd2->DisplayVal();
|
|---|
| 1169 | if (fAz)
|
|---|
| 1170 | fAz->DisplayVal();
|
|---|
| 1171 |
|
|---|
| 1172 | Byte_t avail = 0;
|
|---|
| 1173 |
|
|---|
| 1174 | avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
|
|---|
| 1175 | avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
|
|---|
| 1176 | avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
|
|---|
| 1177 | avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0;
|
|---|
| 1178 | avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0;
|
|---|
| 1179 | avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0;
|
|---|
| 1180 |
|
|---|
| 1181 | if (HasError())
|
|---|
| 1182 | SetStatus(MDriveCom::kError);
|
|---|
| 1183 |
|
|---|
| 1184 |
|
|---|
| 1185 | ZdAz bendist = fStatus&MDriveCom::kTracking ? fTrackingPos : GetPointingPos();
|
|---|
| 1186 |
|
|---|
| 1187 | //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;
|
|---|
| 1188 |
|
|---|
| 1189 | fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError);
|
|---|
| 1190 |
|
|---|
| 1191 | fWin->UpdateWeather(*fCom);
|
|---|
| 1192 | fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/
|
|---|
| 1193 | fRaDec, fZdAzSoll, fStatus, avail);
|
|---|
| 1194 |
|
|---|
| 1195 | lout.UpdateGui();
|
|---|
| 1196 |
|
|---|
| 1197 | const Bool_t trigger = fTriggerDisplay;
|
|---|
| 1198 | fTriggerDisplay = kFALSE;
|
|---|
| 1199 |
|
|---|
| 1200 | if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
|
|---|
| 1201 | DisplayHistTestSe(!trigger);
|
|---|
| 1202 |
|
|---|
| 1203 | if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
|
|---|
| 1204 | DisplayHistGear(!trigger);
|
|---|
| 1205 |
|
|---|
| 1206 | if (fMutexGui.UnLock()==13)
|
|---|
| 1207 | cout << "MCosy::HandleTimer - tried to unlock mutex locked by other thread." << endl;
|
|---|
| 1208 |
|
|---|
| 1209 | return kTRUE;
|
|---|
| 1210 | }
|
|---|
| 1211 |
|
|---|
| 1212 | void MCosy::DisplayHistTestSe(Bool_t del)
|
|---|
| 1213 | {
|
|---|
| 1214 | lout << "Displaying histogram..." << endl;
|
|---|
| 1215 |
|
|---|
| 1216 | TH2F &hist = *(TH2F*)fHist;
|
|---|
| 1217 |
|
|---|
| 1218 | if (del)
|
|---|
| 1219 | {
|
|---|
| 1220 | fHist = NULL;
|
|---|
| 1221 | fBackground = kBgdNone;
|
|---|
| 1222 | }
|
|---|
| 1223 |
|
|---|
| 1224 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
|---|
| 1225 | c->Divide(1,2);
|
|---|
| 1226 |
|
|---|
| 1227 | c->cd(1);
|
|---|
| 1228 | TH2 *h=(TH2*)hist.DrawCopy();
|
|---|
| 1229 |
|
|---|
| 1230 | TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1231 | p->SetLineColor(kBlue);
|
|---|
| 1232 | p->Draw("same");
|
|---|
| 1233 | p->SetBit(kCanDelete);
|
|---|
| 1234 |
|
|---|
| 1235 | c->cd(2);
|
|---|
| 1236 |
|
|---|
| 1237 | TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
|
|---|
| 1238 | hist.GetBinLowEdge(hist.GetNbinsX()+1));
|
|---|
| 1239 | p2.SetXTitle("Zd [\\circ]");
|
|---|
| 1240 | for (int i=0; i<hist.GetNbinsX(); i++)
|
|---|
| 1241 | p2.SetBinError(i, p->GetBinError(i));
|
|---|
| 1242 | p2.SetLineColor(kRed);
|
|---|
| 1243 | p2.SetStats(0);
|
|---|
| 1244 | p2.DrawCopy();
|
|---|
| 1245 |
|
|---|
| 1246 | if (del)
|
|---|
| 1247 | delete &hist;
|
|---|
| 1248 | }
|
|---|
| 1249 |
|
|---|
| 1250 | void MCosy::DisplayHistGear(Bool_t del)
|
|---|
| 1251 | {
|
|---|
| 1252 | lout << "Displaying histogram..." << endl;
|
|---|
| 1253 |
|
|---|
| 1254 | TH3F &hist = *(TH3F*)fHist;
|
|---|
| 1255 |
|
|---|
| 1256 | if (del)
|
|---|
| 1257 | {
|
|---|
| 1258 | fHist = NULL;
|
|---|
| 1259 | fBackground = kBgdNone;
|
|---|
| 1260 | }
|
|---|
| 1261 |
|
|---|
| 1262 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
|---|
| 1263 | c->Divide(2,2);
|
|---|
| 1264 |
|
|---|
| 1265 | // ----------
|
|---|
| 1266 |
|
|---|
| 1267 | c->cd(1);
|
|---|
| 1268 | TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd
|
|---|
| 1269 | h1.SetTitle(" Gear Ratio Zenith Distance [re/se] ");
|
|---|
| 1270 | h1.SetXTitle("Zd [\\circ]");
|
|---|
| 1271 | h1.Draw();
|
|---|
| 1272 | h1.SetBit(kCanDelete);
|
|---|
| 1273 |
|
|---|
| 1274 | TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1275 | p1->SetLineColor(kBlue);
|
|---|
| 1276 | p1->Draw("same");
|
|---|
| 1277 | p1->SetBit(kCanDelete);
|
|---|
| 1278 |
|
|---|
| 1279 | // ----------
|
|---|
| 1280 |
|
|---|
| 1281 | c->cd(2);
|
|---|
| 1282 | TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az
|
|---|
| 1283 | h2.SetTitle(" Gear Ratio Azimuth [re/se] ");
|
|---|
| 1284 | h2.SetXTitle("Zd [\\circ]");
|
|---|
| 1285 | h2.Draw();
|
|---|
| 1286 | h2.SetBit(kCanDelete);
|
|---|
| 1287 |
|
|---|
| 1288 | TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1289 | p2->SetLineColor(kBlue);
|
|---|
| 1290 | p2->Draw("same");
|
|---|
| 1291 | p2->SetBit(kCanDelete);
|
|---|
| 1292 |
|
|---|
| 1293 | // ----------
|
|---|
| 1294 |
|
|---|
| 1295 | c->cd(3);
|
|---|
| 1296 |
|
|---|
| 1297 | TAxis &axe1 = *h1.GetXaxis();
|
|---|
| 1298 |
|
|---|
| 1299 | TH1F f1("spreadzd", " Spread Zenith Distance ",
|
|---|
| 1300 | axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax());
|
|---|
| 1301 | f1.SetXTitle("Zd [\\circ]");
|
|---|
| 1302 | for (int i=0; i<axe1.GetNbins(); i++)
|
|---|
| 1303 | f1.SetBinError(i, p1->GetBinError(i));
|
|---|
| 1304 | f1.SetLineColor(kRed);
|
|---|
| 1305 | f1.SetStats(0);
|
|---|
| 1306 | f1.DrawCopy();
|
|---|
| 1307 |
|
|---|
| 1308 | c->cd(4);
|
|---|
| 1309 |
|
|---|
| 1310 | // ----------
|
|---|
| 1311 |
|
|---|
| 1312 | TAxis &axe2 = *h2.GetXaxis();
|
|---|
| 1313 |
|
|---|
| 1314 | TH1F f2("spreadaz", " Spread Azimuth ",
|
|---|
| 1315 | axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax());
|
|---|
| 1316 | f2.SetXTitle("Az [\\circ]");
|
|---|
| 1317 | for (int i=0; i<axe2.GetNbins(); i++)
|
|---|
| 1318 | f2.SetBinError(i, p2->GetBinError(i));
|
|---|
| 1319 | f2.SetLineColor(kRed);
|
|---|
| 1320 | f2.SetStats(0);
|
|---|
| 1321 | f2.DrawCopy();
|
|---|
| 1322 |
|
|---|
| 1323 | // ----------
|
|---|
| 1324 |
|
|---|
| 1325 | if (del)
|
|---|
| 1326 | delete &hist;
|
|---|
| 1327 | }
|
|---|
| 1328 |
|
|---|
| 1329 | // --------------------------------------------------------------------------
|
|---|
| 1330 | //
|
|---|
| 1331 | // Start the work of the application:
|
|---|
| 1332 | //
|
|---|
| 1333 | // Start the Can-Network.
|
|---|
| 1334 | // Start the MCosy::TalkThread thread.
|
|---|
| 1335 | // turn on the gui update
|
|---|
| 1336 | //
|
|---|
| 1337 | void MCosy::Start()
|
|---|
| 1338 | {
|
|---|
| 1339 | // Don't call this function twice!
|
|---|
| 1340 | Network::Start();
|
|---|
| 1341 |
|
|---|
| 1342 | CheckForError();
|
|---|
| 1343 |
|
|---|
| 1344 | ReadConfig();
|
|---|
| 1345 |
|
|---|
| 1346 | lout << "- Starting TX Thread." << endl;
|
|---|
| 1347 | fTTalk = new MTTalk(this);
|
|---|
| 1348 |
|
|---|
| 1349 | lout << "- Starting GUI update." << endl;
|
|---|
| 1350 | fUpdateGui->TurnOn();
|
|---|
| 1351 | }
|
|---|
| 1352 |
|
|---|
| 1353 | // --------------------------------------------------------------------------
|
|---|
| 1354 | //
|
|---|
| 1355 | // Start the work of the application:
|
|---|
| 1356 | //
|
|---|
| 1357 | // Turn of the gui update
|
|---|
| 1358 | // stop the MCosy::TalkThread thread.
|
|---|
| 1359 | // Stop the network
|
|---|
| 1360 | //
|
|---|
| 1361 | void MCosy::Stop()
|
|---|
| 1362 | {
|
|---|
| 1363 | lout << "- Stopping GUI update." << endl;
|
|---|
| 1364 | fUpdateGui->TurnOff();
|
|---|
| 1365 | lout << "- GUI Update stopped." << endl;
|
|---|
| 1366 |
|
|---|
| 1367 | delete fTTalk;
|
|---|
| 1368 | lout << "- TX Thread stopped." << endl;
|
|---|
| 1369 |
|
|---|
| 1370 | Network::Stop();
|
|---|
| 1371 | }
|
|---|
| 1372 |
|
|---|
| 1373 | // --------------------------------------------------------------------------
|
|---|
| 1374 | //
|
|---|
| 1375 | // Disable the synchronization by using a negative CAN Id for id2.
|
|---|
| 1376 | //
|
|---|
| 1377 | void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
|
|---|
| 1378 | Int_t id4, Int_t id5, Int_t id6)
|
|---|
| 1379 | {
|
|---|
| 1380 | //
|
|---|
| 1381 | // Create Nodes
|
|---|
| 1382 | //
|
|---|
| 1383 | lout << "- Setting up network." << endl;
|
|---|
| 1384 |
|
|---|
| 1385 | fMac1=new Macs(id1, "Mac/Az", lout);
|
|---|
| 1386 | fMac2=new Macs(id3, "Mac/Zd", lout);
|
|---|
| 1387 | if (id2>=0)
|
|---|
| 1388 | fMac3=new Macs(id2, "Mac/Az-Sync", lout);
|
|---|
| 1389 |
|
|---|
| 1390 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
|---|
| 1391 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
|---|
| 1392 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
|---|
| 1393 |
|
|---|
| 1394 | fZd1->SetReport(fOutRep);
|
|---|
| 1395 | fZd2->SetReport(fOutRep);
|
|---|
| 1396 | fAz->SetReport(fOutRep);
|
|---|
| 1397 |
|
|---|
| 1398 | fAz->SetMotor(fMac1);
|
|---|
| 1399 | fZd1->SetMotor(fMac2);
|
|---|
| 1400 | fZd2->SetMotor(fMac2);
|
|---|
| 1401 |
|
|---|
| 1402 | lout << "- Connecting devices to network." << endl;
|
|---|
| 1403 |
|
|---|
| 1404 | //
|
|---|
| 1405 | // Connect the devices to the network
|
|---|
| 1406 | //
|
|---|
| 1407 | SetNode(fMac1);
|
|---|
| 1408 | SetNode(fMac2);
|
|---|
| 1409 | if (id2>=0)
|
|---|
| 1410 | SetNode(fMac3);
|
|---|
| 1411 | SetNode(fZd1);
|
|---|
| 1412 | SetNode(fZd2);
|
|---|
| 1413 | SetNode(fAz);
|
|---|
| 1414 |
|
|---|
| 1415 | //
|
|---|
| 1416 | // Create Gui Event timer and Gui
|
|---|
| 1417 | //
|
|---|
| 1418 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 1419 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 1420 |
|
|---|
| 1421 | lout << "- Starting GUI." << endl;
|
|---|
| 1422 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 1423 | }
|
|---|
| 1424 | /*
|
|---|
| 1425 | void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
|
|---|
| 1426 | {
|
|---|
| 1427 | //
|
|---|
| 1428 | // Create Nodes
|
|---|
| 1429 | //
|
|---|
| 1430 | lout << "- Setting up network." << endl;
|
|---|
| 1431 |
|
|---|
| 1432 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
|---|
| 1433 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
|---|
| 1434 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
|---|
| 1435 |
|
|---|
| 1436 | lout << "- Connecting devices to network." << endl;
|
|---|
| 1437 |
|
|---|
| 1438 | //
|
|---|
| 1439 | // Connect the devices to the network
|
|---|
| 1440 | //
|
|---|
| 1441 | SetNode(fZd1);
|
|---|
| 1442 | SetNode(fZd2);
|
|---|
| 1443 | SetNode(fAz);
|
|---|
| 1444 |
|
|---|
| 1445 | //
|
|---|
| 1446 | // Create Gui Event timer and Gui
|
|---|
| 1447 | //
|
|---|
| 1448 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 1449 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 1450 |
|
|---|
| 1451 | lout << "- Starting GUI." << endl;
|
|---|
| 1452 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 1453 | }
|
|---|
| 1454 |
|
|---|
| 1455 | void MCosy::ConstructorDemo()
|
|---|
| 1456 | {
|
|---|
| 1457 | //
|
|---|
| 1458 | // Create Nodes
|
|---|
| 1459 | //
|
|---|
| 1460 | lout << "- Setting up network." << endl;
|
|---|
| 1461 |
|
|---|
| 1462 | //
|
|---|
| 1463 | // Create Gui Event timer and Gui
|
|---|
| 1464 | //
|
|---|
| 1465 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 1466 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 1467 |
|
|---|
| 1468 | lout << "- Starting GUI." << endl;
|
|---|
| 1469 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 1470 | }
|
|---|
| 1471 | */
|
|---|
| 1472 |
|
|---|
| 1473 | TString MCosy::GetFileName(const char *fmt)
|
|---|
| 1474 | {
|
|---|
| 1475 | // FIXME: Timeout missing
|
|---|
| 1476 | while (1)
|
|---|
| 1477 | {
|
|---|
| 1478 | MTime time(-1);
|
|---|
| 1479 | const TString name = Form(fmt, (const char*)time.GetFileName());
|
|---|
| 1480 | if (gSystem->AccessPathName(name, kFileExists))
|
|---|
| 1481 | return name;
|
|---|
| 1482 | break;
|
|---|
| 1483 |
|
|---|
| 1484 | usleep(1000);
|
|---|
| 1485 | }
|
|---|
| 1486 | return "";
|
|---|
| 1487 | }
|
|---|
| 1488 |
|
|---|
| 1489 | MCosy::MCosy(/*int mode,*/ const char *dev, const int baud, MLog &out)
|
|---|
| 1490 | : Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fStarguider(NULL), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0)
|
|---|
| 1491 | {
|
|---|
| 1492 | TEnv env(".cosyrc");
|
|---|
| 1493 | const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
|
|---|
| 1494 | const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
|
|---|
| 1495 | const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3
|
|---|
| 1496 | const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4
|
|---|
| 1497 | const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5
|
|---|
| 1498 | const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6
|
|---|
| 1499 |
|
|---|
| 1500 | TString name = GetFileName("rep/cosy_%s.rep");
|
|---|
| 1501 | cout << "Open Repfile: " << name << endl;
|
|---|
| 1502 | fOutRep = new MLog(name, kTRUE);
|
|---|
| 1503 | *fOutRep << "[Drive Report File]" << endl;
|
|---|
| 1504 | *fOutRep << "Version <cvs>" << endl;
|
|---|
| 1505 | *fOutRep << "Date " << MTime(-1) << endl;
|
|---|
| 1506 | *fOutRep << "[Reports]" << endl;
|
|---|
| 1507 |
|
|---|
| 1508 | /*
|
|---|
| 1509 | lout << "- Program in ";
|
|---|
| 1510 | switch (mode)
|
|---|
| 1511 | {
|
|---|
| 1512 | case 0:
|
|---|
| 1513 | lout << "<<Standard mode>>" << endl;*/
|
|---|
| 1514 | fBending.Load("bending.txt");
|
|---|
| 1515 | Constructor(id1, id2, id3, id4, id5, id6);/*
|
|---|
| 1516 | break;
|
|---|
| 1517 | case 1:
|
|---|
| 1518 | lout << "<<SE mode>>" << endl;
|
|---|
| 1519 | fBending.Load("bending.txt");
|
|---|
| 1520 | ConstructorSE(id4, id5, id6);
|
|---|
| 1521 | break;
|
|---|
| 1522 | default:
|
|---|
| 1523 | lout << "<<Demo mode>>" << endl;
|
|---|
| 1524 | ConstructorDemo();
|
|---|
| 1525 | }
|
|---|
| 1526 | */
|
|---|
| 1527 | lout.SetOutputGui(fWin->GetLog(), kTRUE);
|
|---|
| 1528 |
|
|---|
| 1529 | fZd1->SetDisplay(fWin->GetLabel2());
|
|---|
| 1530 | fZd2->SetDisplay(fWin->GetLabel3());
|
|---|
| 1531 | fAz->SetDisplay(fWin->GetLabel1());
|
|---|
| 1532 |
|
|---|
| 1533 | fCom = new MDriveCom(this, *fOutRep);
|
|---|
| 1534 | fCom->Start();
|
|---|
| 1535 | }
|
|---|
| 1536 |
|
|---|
| 1537 | void MCosy::TerminateApp()
|
|---|
| 1538 | {
|
|---|
| 1539 | cout << "MCosy::TerminateApp()" << endl;
|
|---|
| 1540 | /*
|
|---|
| 1541 | Int_t rc;
|
|---|
| 1542 | TGMessageBox msg(this, gClient->GetRoot(),
|
|---|
| 1543 | "Information",
|
|---|
| 1544 | "Cosy is shutting down the system - this may take wa while!",
|
|---|
| 1545 | kMBIconExclamation,
|
|---|
| 1546 | kMBOK, //kMBClose
|
|---|
| 1547 | &rc, 0);
|
|---|
| 1548 | */
|
|---|
| 1549 |
|
|---|
| 1550 | lout.DisableOutputDevice(MLog::eGui);
|
|---|
| 1551 | // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
|
|---|
| 1552 | // lout.SetOutputGui(NULL, kFALSE);
|
|---|
| 1553 |
|
|---|
| 1554 | gApplication->Terminate(0);
|
|---|
| 1555 | }
|
|---|
| 1556 |
|
|---|
| 1557 | MCosy::~MCosy()
|
|---|
| 1558 | {
|
|---|
| 1559 | if (fOutTp)
|
|---|
| 1560 | {
|
|---|
| 1561 | *fOutTp << "END" << endl;
|
|---|
| 1562 | delete fOutTp;
|
|---|
| 1563 | }
|
|---|
| 1564 | delete fOutRep;
|
|---|
| 1565 |
|
|---|
| 1566 | cout << "Deleting GUI timer." << endl;
|
|---|
| 1567 |
|
|---|
| 1568 | delete fUpdateGui;
|
|---|
| 1569 | delete fCom;
|
|---|
| 1570 |
|
|---|
| 1571 | cout << "Deleting Nodes." << endl;
|
|---|
| 1572 |
|
|---|
| 1573 | fZd1->SetReport(0);
|
|---|
| 1574 | fZd2->SetReport(0);
|
|---|
| 1575 | fAz->SetReport(0);
|
|---|
| 1576 |
|
|---|
| 1577 | delete fAz;
|
|---|
| 1578 | delete fZd1;
|
|---|
| 1579 | delete fZd2;
|
|---|
| 1580 | delete fMac1;
|
|---|
| 1581 | delete fMac2;
|
|---|
| 1582 | if (fMac3)
|
|---|
| 1583 | delete fMac3;
|
|---|
| 1584 |
|
|---|
| 1585 | cout << "Deleting MGCosy." << endl;
|
|---|
| 1586 |
|
|---|
| 1587 | lout.DisableOutputDevice(MLog::eGui);
|
|---|
| 1588 |
|
|---|
| 1589 | delete fWin;
|
|---|
| 1590 |
|
|---|
| 1591 | cout << "MGCosy destructed." << endl;
|
|---|
| 1592 | }
|
|---|
