| 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 |
|
|---|
| 404 | point.SetPointVelocity(0.1);
|
|---|
| 405 |
|
|---|
| 406 |
|
|---|
| 407 | //#endif
|
|---|
| 408 |
|
|---|
| 409 | // point.SetPointAccDec(0.4, 0.4);
|
|---|
| 410 | // point.SetPointVelocity(0.4);
|
|---|
| 411 |
|
|---|
| 412 |
|
|---|
| 413 | // original
|
|---|
| 414 | return point.SetPosition(dst, track);
|
|---|
| 415 |
|
|---|
| 416 | // test
|
|---|
| 417 | // return point.SetPosition(dst, kTRUE);
|
|---|
| 418 |
|
|---|
| 419 |
|
|---|
| 420 |
|
|---|
| 421 | }
|
|---|
| 422 |
|
|---|
| 423 | void MCosy::SetTrackingPosRE(ZdAz za)
|
|---|
| 424 | {
|
|---|
| 425 | za /= kGearTot; // [U_tel]
|
|---|
| 426 | za *= TMath::TwoPi(); // [rad]
|
|---|
| 427 | //cout << "RE1: " << za.Zd()*180/3.1415 << " " << za.Az()*180/3.1415 << endl;
|
|---|
| 428 | fTrackingPosRaw = za*TMath::RadToDeg();
|
|---|
| 429 | fTrackingPos = fBending.CorrectBack(za)*TMath::RadToDeg();
|
|---|
| 430 | //cout << "RE2: " << fTrackingPos.Zd() << " " << fTrackingPos.Az() << endl;
|
|---|
| 431 | }
|
|---|
| 432 |
|
|---|
| 433 | void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
|
|---|
| 434 | {
|
|---|
| 435 | MTracking track(this, lout);
|
|---|
| 436 | track.SetOut(fOutRep);
|
|---|
| 437 | //#ifdef EXPERT
|
|---|
| 438 | // track.SetPointAccDec(0.4, 0.4);
|
|---|
| 439 | // track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
|
|---|
| 440 | //#else
|
|---|
| 441 |
|
|---|
| 442 |
|
|---|
| 443 |
|
|---|
| 444 | track.SetPointAccDec(0.2, 0.1);
|
|---|
| 445 | track.SetPointVelocity(0.1);
|
|---|
| 446 |
|
|---|
| 447 | // track.SetPointAccDec(0.4, 0.4);
|
|---|
| 448 | // track.SetPointVelocity(0.4);
|
|---|
| 449 |
|
|---|
| 450 |
|
|---|
| 451 |
|
|---|
| 452 | //#endif
|
|---|
| 453 | track.SetTrackAccDec(0.1, 0.1);
|
|---|
| 454 |
|
|---|
| 455 | track.TrackPosition(dst);
|
|---|
| 456 | }
|
|---|
| 457 |
|
|---|
| 458 | void MCosy::TrackPositionGRB(const RaDec &dst) // ra, dec [rad]
|
|---|
| 459 | {
|
|---|
| 460 | MTracking track(this, lout);
|
|---|
| 461 | track.SetOut(fOutRep);
|
|---|
| 462 | //#ifdef EXPERT
|
|---|
| 463 | // track.SetPointAccDec(0.4, 0.4);
|
|---|
| 464 | // track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
|
|---|
| 465 | //#else
|
|---|
| 466 | track.SetPointAccDec(0.4, 0.4);
|
|---|
| 467 | track.SetPointVelocity(0.3);
|
|---|
| 468 | //#endif
|
|---|
| 469 | track.SetTrackAccDec(0.1, 0.1);
|
|---|
| 470 |
|
|---|
| 471 | track.TrackPosition(dst);
|
|---|
| 472 | }
|
|---|
| 473 |
|
|---|
| 474 | // --------------------------------------------------------------------------
|
|---|
| 475 | //
|
|---|
| 476 | // Stops the movement of both motors.
|
|---|
| 477 | //
|
|---|
| 478 | // Sets the status to stopping. Sets the deceleration to 50% of the maximum.
|
|---|
| 479 | // stops. Quits the revolution mode and wait for the end of the movement.
|
|---|
| 480 | //
|
|---|
| 481 | void MCosy::StopMovement()
|
|---|
| 482 | {
|
|---|
| 483 | //
|
|---|
| 484 | // Set status to Stopping
|
|---|
| 485 | //
|
|---|
| 486 | SetStatus(MDriveCom::kStopping);
|
|---|
| 487 |
|
|---|
| 488 | //
|
|---|
| 489 | // set deceleration to 50%
|
|---|
| 490 | //
|
|---|
| 491 | cout << "Stopping movement (dec=30%)..." << endl;
|
|---|
| 492 | if (fMac1 && fMac2)
|
|---|
| 493 | {
|
|---|
| 494 | #ifdef EXPERT
|
|---|
| 495 | fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
|
|---|
| 496 | fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
|
|---|
| 497 | #else
|
|---|
| 498 | fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
|
|---|
| 499 | fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
|
|---|
| 500 | #endif
|
|---|
| 501 | fMac1->SetRpmMode(FALSE);
|
|---|
| 502 | fMac2->SetRpmMode(FALSE);
|
|---|
| 503 | }
|
|---|
| 504 |
|
|---|
| 505 | /*
|
|---|
| 506 | fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
|
|---|
| 507 | fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
|
|---|
| 508 |
|
|---|
| 509 | fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
|
|---|
| 510 | fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
|
|---|
| 511 | fMac2->WaitForSdo(0x3000, 0);
|
|---|
| 512 | fMac1->WaitForSdo(0x3000, 0);
|
|---|
| 513 | fMac1->SetRpmMode(FALSE);
|
|---|
| 514 | fMac2->SetRpmMode(FALSE);
|
|---|
| 515 | */
|
|---|
| 516 |
|
|---|
| 517 | //
|
|---|
| 518 | // Wait for the movement to really be finished.
|
|---|
| 519 | //
|
|---|
| 520 | #ifdef EXPERT
|
|---|
| 521 | cout << "Waiting for end of movement..." << endl;
|
|---|
| 522 | #endif
|
|---|
| 523 | WaitForEndMovement();
|
|---|
| 524 |
|
|---|
| 525 | //
|
|---|
| 526 | // Check whether everything works fine.
|
|---|
| 527 | //
|
|---|
| 528 | CheckForError();
|
|---|
| 529 | #ifdef EXPERT
|
|---|
| 530 | cout << "Movement stopped." << endl;
|
|---|
| 531 | #endif
|
|---|
| 532 | }
|
|---|
| 533 |
|
|---|
| 534 | bool MCosy::CheckNetwork()
|
|---|
| 535 | {
|
|---|
| 536 | //return kTRUE;
|
|---|
| 537 | //CheckConnections();
|
|---|
| 538 |
|
|---|
| 539 | CheckForError();
|
|---|
| 540 |
|
|---|
| 541 | if (HasZombie())
|
|---|
| 542 | {
|
|---|
| 543 | lout << "- Found Zombies in Network..." << endl;
|
|---|
| 544 | if (!RebootZombies())
|
|---|
| 545 | return false;
|
|---|
| 546 | }
|
|---|
| 547 |
|
|---|
| 548 | /*
|
|---|
| 549 | FIXME HANDLING ERROR
|
|---|
| 550 | */
|
|---|
| 551 | if (HasError())
|
|---|
| 552 | {
|
|---|
| 553 | fMac1->HandleError();
|
|---|
| 554 | fMac2->HandleError();
|
|---|
| 555 | fMac3->HandleError();
|
|---|
| 556 | if (HasError() || HasZombie())
|
|---|
| 557 | return false;
|
|---|
| 558 | }
|
|---|
| 559 |
|
|---|
| 560 | CheckForError();
|
|---|
| 561 | return true;
|
|---|
| 562 | }
|
|---|
| 563 |
|
|---|
| 564 | void *MCosy::Proc(int msg, void *mp)
|
|---|
| 565 | {
|
|---|
| 566 | switch (msg)
|
|---|
| 567 | {
|
|---|
| 568 | case WM_WAIT:
|
|---|
| 569 | cout << "Wait for execution of Proc(WM_*, ): done." << endl;
|
|---|
| 570 | return NULL;
|
|---|
| 571 |
|
|---|
| 572 | case WM_STOP:
|
|---|
| 573 | //cout << "MCosy::Proc: Stop." << endl;
|
|---|
| 574 | if (!CheckNetwork())
|
|---|
| 575 | return (void*)0xebb0;
|
|---|
| 576 | StopMovement();
|
|---|
| 577 | return NULL;
|
|---|
| 578 | /*
|
|---|
| 579 | case WM_PRESET:
|
|---|
| 580 | cout << "WM_Preset: start." << endl;
|
|---|
| 581 | if (!CheckNetwork())
|
|---|
| 582 | return (void*)0xebb0;
|
|---|
| 583 | fZd1->SetPreset();
|
|---|
| 584 | fZd2->SetPreset();
|
|---|
| 585 | fAz->SetPreset();
|
|---|
| 586 | cout << "WM_Preset: done. (return 0xaffe)" << endl;
|
|---|
| 587 | return (void*)0xaffe;
|
|---|
| 588 | */
|
|---|
| 589 | /*
|
|---|
| 590 | case WM_CALIB:
|
|---|
| 591 | {
|
|---|
| 592 | cout << "WM_Calib: start." << endl;
|
|---|
| 593 | if (!CheckNetwork())
|
|---|
| 594 | return (void*)0xebb0;
|
|---|
| 595 |
|
|---|
| 596 | SlaStars sla(fObservatory);
|
|---|
| 597 | sla.Now();
|
|---|
| 598 |
|
|---|
| 599 | RaDec rd = *((RaDec*)mp);
|
|---|
| 600 |
|
|---|
| 601 | //RaDec rd(37.94, 89.2644); // POLARIS
|
|---|
| 602 | //RaDec rd(213.915417, 19.1825); // ARCTURUS
|
|---|
| 603 |
|
|---|
| 604 | cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
|
|---|
| 605 |
|
|---|
| 606 | ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;
|
|---|
| 607 |
|
|---|
| 608 | cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
|
|---|
| 609 |
|
|---|
| 610 | ZdAz sepos = GetSePos();
|
|---|
| 611 | cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
|
|---|
| 612 |
|
|---|
| 613 | fZd1->SetPreset(za.Zd());
|
|---|
| 614 | fZd2->SetPreset(-za.Zd());
|
|---|
| 615 | fAz->SetPreset(za.Az());
|
|---|
| 616 |
|
|---|
| 617 | cout << "WM_Calib: done. (return 0xaffe)" << endl;
|
|---|
| 618 | }
|
|---|
| 619 | return (void*)0xaffe;
|
|---|
| 620 | */
|
|---|
| 621 | case WM_TPOINT:
|
|---|
| 622 | {
|
|---|
| 623 | //cout << "WM_TPoint: start." << endl;
|
|---|
| 624 | SlaStars sla(fObservatory);
|
|---|
| 625 | sla.Now();
|
|---|
| 626 |
|
|---|
| 627 | RaDec rd = *((RaDec*)mp);
|
|---|
| 628 | cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
|
|---|
| 629 |
|
|---|
| 630 | AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;
|
|---|
| 631 |
|
|---|
| 632 | if (!fOutTp)
|
|---|
| 633 | {
|
|---|
| 634 | //
|
|---|
| 635 | // open tpoint file
|
|---|
| 636 | //
|
|---|
| 637 | const TString name = GetFileName("tpoint", "old-tpoint", "txt");
|
|---|
| 638 | cout << "TPoint-Cosy File ********* " << name << " ********** " << endl;
|
|---|
| 639 |
|
|---|
| 640 | fOutTp = new ofstream(name);
|
|---|
| 641 | *fOutTp << "Magic Model TPOINT data file" << endl;
|
|---|
| 642 | *fOutTp << ": ALTAZ" << endl;
|
|---|
| 643 | *fOutTp << "49 48 0 ";
|
|---|
| 644 | *fOutTp << sla.GetTime().Year() << " " << sla.GetTime().Month() << " " << sla.GetTime().Day() << " ";
|
|---|
| 645 | *fOutTp << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
|
|---|
| 646 | // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
|
|---|
| 647 | }
|
|---|
| 648 |
|
|---|
| 649 | cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
|---|
| 650 | *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " ";
|
|---|
| 651 |
|
|---|
| 652 | ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE;
|
|---|
| 653 | za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
|
|---|
| 654 | za *= kRad2Deg;
|
|---|
| 655 |
|
|---|
| 656 | cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
|---|
| 657 | *fOutTp << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";
|
|---|
| 658 |
|
|---|
| 659 | if (fStarguider)
|
|---|
| 660 | {
|
|---|
| 661 | XY tp = fStarguider->GetCoordinates();
|
|---|
| 662 | *fOutTp << 90-tp.X() << " " << tp.Y() << " ";
|
|---|
| 663 | }
|
|---|
| 664 |
|
|---|
| 665 | *fOutTp << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;
|
|---|
| 666 |
|
|---|
| 667 | //cout << "WM_TPoint: done. (return 0xaffe)" << endl;
|
|---|
| 668 | }
|
|---|
| 669 | return (void*)0xca1b;
|
|---|
| 670 |
|
|---|
| 671 | case WM_TRACKPOS:
|
|---|
| 672 | //cout << "WM_TrackPosition: start." << endl;
|
|---|
| 673 | {
|
|---|
| 674 | if (!CheckNetwork())
|
|---|
| 675 | return (void*)0xebb0;
|
|---|
| 676 |
|
|---|
| 677 | ZdAz dest = *((ZdAz*)mp) * kDeg2Rad;
|
|---|
| 678 | if (!SetPosition(dest, kTRUE))
|
|---|
| 679 | return (void*)0x1234;
|
|---|
| 680 |
|
|---|
| 681 | SlaStars sla(fObservatory);
|
|---|
| 682 | sla.Now();
|
|---|
| 683 |
|
|---|
| 684 | RaDec rd = sla.CalcRaDec(dest);
|
|---|
| 685 | TrackPosition(rd);
|
|---|
| 686 | }
|
|---|
| 687 | //cout << "WM_TrackPosition: done. (return 0xabcd)" << endl;
|
|---|
| 688 | return (void*)0xabcd;
|
|---|
| 689 | /*
|
|---|
| 690 | case WM_ARM:
|
|---|
| 691 | //cout << "WM_Position: start." << endl;
|
|---|
| 692 | {
|
|---|
| 693 | if (!CheckNetwork())
|
|---|
| 694 | return (void*)0xebb0;
|
|---|
| 695 |
|
|---|
| 696 | const bool arm = *((bool*)mp);
|
|---|
| 697 | if (arm)
|
|---|
| 698 | {
|
|---|
| 699 | fMac1->Arm();
|
|---|
| 700 | fMac2->Arm();
|
|---|
| 701 | cout << "ARMED" << endl;
|
|---|
| 702 | }
|
|---|
| 703 | else
|
|---|
| 704 | {
|
|---|
| 705 | fMac1->Disarm();
|
|---|
| 706 | fMac2->Disarm();
|
|---|
| 707 | cout << "DISARMED" << endl;
|
|---|
| 708 | }
|
|---|
| 709 | }
|
|---|
| 710 | //cout << "WM_Position: done. (return 0x7777)" << endl;
|
|---|
| 711 | return (void*)0x9999;
|
|---|
| 712 | */
|
|---|
| 713 | case WM_POSITION:
|
|---|
| 714 | //cout << "WM_Position: start." << endl;
|
|---|
| 715 | {
|
|---|
| 716 | if (!CheckNetwork())
|
|---|
| 717 | return (void*)0xebb0;
|
|---|
| 718 |
|
|---|
| 719 | ZdAz dest = *((ZdAz*)mp);
|
|---|
| 720 | SetPosition(dest*kDeg2Rad);
|
|---|
| 721 | }
|
|---|
| 722 | //cout << "WM_Position: done. (return 0x7777)" << endl;
|
|---|
| 723 | return (void*)0x7777;
|
|---|
| 724 |
|
|---|
| 725 | case WM_POSITION1:
|
|---|
| 726 | //cout << "WM_Position1: start." << endl;
|
|---|
| 727 | {
|
|---|
| 728 | if (!CheckNetwork())
|
|---|
| 729 | return (void*)0xebb0;
|
|---|
| 730 |
|
|---|
| 731 | ZdAz dest = *((ZdAz*)mp);
|
|---|
| 732 | SetPosition(dest*kDeg2Rad, kTRUE);
|
|---|
| 733 | }
|
|---|
| 734 | //cout << "WM_Position: done. (return 0x7777)" << endl;
|
|---|
| 735 | return (void*)0x7777;
|
|---|
| 736 |
|
|---|
| 737 | case WM_PREPS:
|
|---|
| 738 | //cout << "WM_Track: START" << endl;
|
|---|
| 739 | {
|
|---|
| 740 | if (!CheckNetwork())
|
|---|
| 741 | return (void*)0xebb0;
|
|---|
| 742 |
|
|---|
| 743 | const char *preps = (const char*)mp;
|
|---|
| 744 | cout << "Preposition command to " << preps << " received." << endl;
|
|---|
| 745 |
|
|---|
| 746 | ifstream fin("prepos.txt");
|
|---|
| 747 | if (!fin)
|
|---|
| 748 | {
|
|---|
| 749 | cout << "ERROR: cannot open prepos.txt." << endl;
|
|---|
| 750 | return (void*)0xebb1;
|
|---|
| 751 | }
|
|---|
| 752 |
|
|---|
| 753 | while (1)
|
|---|
| 754 | {
|
|---|
| 755 | Double_t zd, az;
|
|---|
| 756 | fin >> zd >> az;
|
|---|
| 757 |
|
|---|
| 758 | TString str;
|
|---|
| 759 | str.ReadLine(fin);
|
|---|
| 760 | if (!fin)
|
|---|
| 761 | break;
|
|---|
| 762 |
|
|---|
| 763 | str.ToLower();
|
|---|
| 764 |
|
|---|
| 765 | if (str.Strip(TString::kBoth)==preps)
|
|---|
| 766 | {
|
|---|
| 767 | ZdAz dest(zd, az);
|
|---|
| 768 | SetPosition(dest*kDeg2Rad);
|
|---|
| 769 | return (void*)0x7979;
|
|---|
| 770 | }
|
|---|
| 771 | cout << "ERROR - Requested preposition not found in file..." << endl;
|
|---|
| 772 | }
|
|---|
| 773 | }
|
|---|
| 774 | //cout << "WM_Track: done. (return 0x8888)" << endl;
|
|---|
| 775 | return (void*)0x7878;
|
|---|
| 776 |
|
|---|
| 777 | case WM_TESTSE:
|
|---|
| 778 | //cout << "WM_TestSe: start." << endl;
|
|---|
| 779 | fBackground = mp ? kBgdSeTest : kBgdNone;
|
|---|
| 780 | //cout << "WM_TestSe: done. (return 0x1e51)" << endl;
|
|---|
| 781 | return (void*)0x1e51;
|
|---|
| 782 |
|
|---|
| 783 | case WM_GEAR:
|
|---|
| 784 | //cout << "WM_Gear: start." << endl;
|
|---|
| 785 | fBackground = mp ? kBgdGear : kBgdNone;
|
|---|
| 786 | //cout << "WM_Gear: done. (return 0xfeaf)" << endl;
|
|---|
| 787 | return (void*)0xfeaf;
|
|---|
| 788 |
|
|---|
| 789 | case WM_DISPLAY:
|
|---|
| 790 | //cout << "WM_Display: start." << endl;
|
|---|
| 791 | fTriggerDisplay = kTRUE;
|
|---|
| 792 | //cout << "WM_Disply: done. (return 0xd1e1)" << endl;
|
|---|
| 793 | return (void*)0xd1e1;
|
|---|
| 794 |
|
|---|
| 795 | case WM_TRACK:
|
|---|
| 796 | case WM_GRB:
|
|---|
| 797 | //cout << "WM_Track/GRB: START" << endl;
|
|---|
| 798 | {
|
|---|
| 799 | RaDec dest = ((RaDec*)mp)[0];
|
|---|
| 800 | if (fStarguider)
|
|---|
| 801 | fStarguider->SetPointingPosition(((RaDec*)mp)[1]);
|
|---|
| 802 | if (!CheckNetwork())
|
|---|
| 803 | return (void*)0xebb0;
|
|---|
| 804 |
|
|---|
| 805 | if (msg==WM_TRACK)
|
|---|
| 806 | TrackPosition(dest*kDeg2Rad);
|
|---|
| 807 | else
|
|---|
| 808 | TrackPositionGRB(dest*kDeg2Rad);
|
|---|
| 809 | }
|
|---|
| 810 | //cout << "WM_Track/GRB: done. (return 0x8888)" << endl;
|
|---|
| 811 | return (void*)0x8888;
|
|---|
| 812 |
|
|---|
| 813 | case WM_NEWTRACK:
|
|---|
| 814 | //cout << "WM_NewTrack: START" << endl;
|
|---|
| 815 | fRaDec = *((RaDec*)mp);
|
|---|
| 816 | //cout << "WM_NewTrack: done. (return 0x9999)" << endl;
|
|---|
| 817 | return (void*)0x9999;
|
|---|
| 818 |
|
|---|
| 819 | case WM_LOADBENDING:
|
|---|
| 820 | //cout << "WM_LoadBending: START" << endl;
|
|---|
| 821 | fBending.Load("bending.txt");
|
|---|
| 822 | //cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
|
|---|
| 823 | return (void*)0xbe0d;
|
|---|
| 824 |
|
|---|
| 825 | case WM_RESETBENDING:
|
|---|
| 826 | //cout << "WM_ResetBending: START" << endl;
|
|---|
| 827 | fBending.Reset();
|
|---|
| 828 | //cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
|
|---|
| 829 | return (void*)0xbe0e;
|
|---|
| 830 |
|
|---|
| 831 | case WM_HOME:
|
|---|
| 832 | //cout << "WM_Home: START" << endl;
|
|---|
| 833 | if (!CheckNetwork())
|
|---|
| 834 | return (void*)0xebb0;
|
|---|
| 835 | else
|
|---|
| 836 | {
|
|---|
| 837 | cout << "HOME NOT ALLOWED... for Magic." << endl;
|
|---|
| 838 | /*
|
|---|
| 839 | cout << "Going Home..." << endl;
|
|---|
| 840 | TEnv env(".cosyrc");
|
|---|
| 841 |
|
|---|
| 842 | SetStatus(MDriveCom::kMoving);
|
|---|
| 843 |
|
|---|
| 844 | fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
|
|---|
| 845 | fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
|
|---|
| 846 |
|
|---|
| 847 | lout << "SETHOME DONE" << endl;
|
|---|
| 848 |
|
|---|
| 849 | SetStatus(HasError() ? MDriveCom::kError : MDriveCom::kStopped);
|
|---|
| 850 |
|
|---|
| 851 | fAz->SetPreset();
|
|---|
| 852 | fZd1->SetPreset();
|
|---|
| 853 | fZd2->SetPreset();
|
|---|
| 854 |
|
|---|
| 855 | fMac1->ReqPos();
|
|---|
| 856 | fMac2->ReqPos();
|
|---|
| 857 | fMac3->StopMotor();
|
|---|
| 858 | */
|
|---|
| 859 | }
|
|---|
| 860 | //cout << "WM_Home: done. (return 0x403e)" << endl;
|
|---|
| 861 | return (void*)0x403e;
|
|---|
| 862 |
|
|---|
| 863 | case WM_CALCALTAZ:
|
|---|
| 864 | {
|
|---|
| 865 | cout << endl;
|
|---|
| 866 |
|
|---|
| 867 | SlaStars sla(fObservatory);
|
|---|
| 868 | sla.Now();
|
|---|
| 869 |
|
|---|
| 870 | XY xy = *((XY*)mp);
|
|---|
| 871 | RaDec rd(xy.X()*15., xy.Y()); // [deg]
|
|---|
| 872 |
|
|---|
| 873 | ZdAz a1 = sla.CalcZdAz(rd*kDeg2Rad); // [rad]
|
|---|
| 874 |
|
|---|
| 875 | cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
|
|---|
| 876 | cout << "Zd/Az target: " << a1.Zd()*kRad2Deg << "° " << a1.Az()*kRad2Deg << "°" << endl;
|
|---|
| 877 |
|
|---|
| 878 | if (fZd1 && fZd2 && fAz)
|
|---|
| 879 | a1 = AlignTrackingPos(a1);
|
|---|
| 880 |
|
|---|
| 881 | a1 = fBending(a1);
|
|---|
| 882 | CheckRange(a1);
|
|---|
| 883 | a1 *= kRad2Deg;
|
|---|
| 884 |
|
|---|
| 885 | const ZdAz a2 = a1*kResSE/360;
|
|---|
| 886 |
|
|---|
| 887 | cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
|
|---|
| 888 | cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
|
|---|
| 889 | }
|
|---|
| 890 | return (void*)0xa17a;
|
|---|
| 891 |
|
|---|
| 892 | case WM_ENDSWITCH:
|
|---|
| 893 | {
|
|---|
| 894 | ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE;
|
|---|
| 895 | pos = fBending.SubtractOffsets(pos)*kRad2Deg;
|
|---|
| 896 |
|
|---|
| 897 | cout << "Endswitch Position: Zd=" << pos.Zd() << "° Az=";
|
|---|
| 898 | cout << pos.Az() << "°" << endl;
|
|---|
| 899 | }
|
|---|
| 900 |
|
|---|
| 901 | return (void*)0x1010;
|
|---|
| 902 |
|
|---|
| 903 | case WM_QUIT:
|
|---|
| 904 | cout << "WM_Quit: now." << endl;
|
|---|
| 905 | if (!CheckNetwork())
|
|---|
| 906 | {
|
|---|
| 907 | lout << "ERROR: Cannot shutdown CANbus network." << endl;
|
|---|
| 908 | return (void*)0xebb0;
|
|---|
| 909 | }
|
|---|
| 910 | TerminateApp();
|
|---|
| 911 | cout << "WM_Quit: done." << endl;
|
|---|
| 912 | return (void*)0xaaaa;
|
|---|
| 913 | }
|
|---|
| 914 | cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
|
|---|
| 915 | return (void*)0xffffffff;
|
|---|
| 916 | }
|
|---|
| 917 |
|
|---|
| 918 | void *MTTalk::Thread()
|
|---|
| 919 | {
|
|---|
| 920 | fCosy->TalkThread();
|
|---|
| 921 | return NULL;
|
|---|
| 922 | }
|
|---|
| 923 |
|
|---|
| 924 | void MCosy::ReadConfig()
|
|---|
| 925 | {
|
|---|
| 926 | cout << "Reading configuration file..." << flush;
|
|---|
| 927 | TEnv env(".cosyrc");
|
|---|
| 928 | cout << "done." << endl;
|
|---|
| 929 |
|
|---|
| 930 | cout << "Reading telescope range..." << flush;
|
|---|
| 931 | const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
|
|---|
| 932 | const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
|
|---|
| 933 | fMin.Set(zmin, amin);
|
|---|
| 934 |
|
|---|
| 935 | const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
|
|---|
| 936 | const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
|
|---|
| 937 | fMax.Set(zmax, amax);
|
|---|
| 938 | cout << "done." << endl;
|
|---|
| 939 |
|
|---|
| 940 | cout << " * Min: " << zmin << "deg " << amin << "deg" << endl;
|
|---|
| 941 | cout << " * Max: " << zmax << "deg " << amax << "deg" << endl;
|
|---|
| 942 |
|
|---|
| 943 | fMin = fBending.AddOffsets(fMin/kRad2Deg);
|
|---|
| 944 | fMax = fBending.AddOffsets(fMax/kRad2Deg);
|
|---|
| 945 |
|
|---|
| 946 | cout << " * Min': " << fMin.Zd()*kRad2Deg << "deg " << fMin.Az()*kRad2Deg << "deg" << endl;
|
|---|
| 947 | cout << " * Max': " << fMax.Zd()*kRad2Deg << "deg " << fMax.Az()*kRad2Deg << "deg" << endl;
|
|---|
| 948 |
|
|---|
| 949 | cout << "Reading gear ratios..." << flush;
|
|---|
| 950 | kGear.X(env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0));
|
|---|
| 951 | kGear.Y(env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0));
|
|---|
| 952 |
|
|---|
| 953 | kResRE.Y(0);
|
|---|
| 954 | if (fMac1 && !fMac1->IsZombieNode())
|
|---|
| 955 | kResRE.Y(fMac1->GetRes());
|
|---|
| 956 | else
|
|---|
| 957 | if (fMac3 && !fMac3->IsZombieNode())
|
|---|
| 958 | kResRE.Y(fMac3->GetRes());
|
|---|
| 959 | else
|
|---|
| 960 | kResRE.Y(env.GetValue("Az_ResRE[re/U_mot]", 1500));
|
|---|
| 961 |
|
|---|
| 962 | kResRE.X(0);
|
|---|
| 963 | if (fMac2 && !fMac2->IsZombieNode())
|
|---|
| 964 | kResRE.X(fMac2->GetRes());
|
|---|
| 965 | else
|
|---|
| 966 | kResRE.X(env.GetValue("Zd_ResRE[re/U_mot]", 1500));
|
|---|
| 967 |
|
|---|
| 968 | kResSE.X(0);
|
|---|
| 969 | if (fZd1 && !fZd1->IsZombieNode())
|
|---|
| 970 | kResSE.X(fZd1->GetPhysRes());
|
|---|
| 971 | else
|
|---|
| 972 | if (fZd2 && !fZd2->IsZombieNode())
|
|---|
| 973 | kResSE.X(fZd2->GetPhysRes());
|
|---|
| 974 | else
|
|---|
| 975 | kResSE.X(env.GetValue("Zd_ResSE[se/U_mot]", 16384));
|
|---|
| 976 |
|
|---|
| 977 | kResSE.Y(0);
|
|---|
| 978 | if (fAz && !fAz->IsZombieNode())
|
|---|
| 979 | kResSE.Y(fAz->GetPhysRes());
|
|---|
| 980 | else
|
|---|
| 981 | kResSE.Y(env.GetValue("Az_ResSE[se/U_mot]", 16384));
|
|---|
| 982 |
|
|---|
| 983 | // believing the Macs manual '*4' shouldn't be necessary, but it is.
|
|---|
| 984 | // Because the a RE is 4 quad counts.
|
|---|
| 985 | // Calculating speeds we have to convert back to qc
|
|---|
| 986 | kResRE *= 4;
|
|---|
| 987 | kGearTot = kResRE*kGear;
|
|---|
| 988 |
|
|---|
| 989 | cout << "done." << endl;
|
|---|
| 990 |
|
|---|
| 991 | cout << " * Setting Gear Ratios:" << endl;
|
|---|
| 992 | cout << " --------------------" << endl;
|
|---|
| 993 | cout << " * X: " << kGear.X() << "*" << kResRE.X()/4 << "/" << kResSE.X() << "=4*" << kGearTot.X() << "/" << kResSE.X() << endl;
|
|---|
| 994 | cout << " * Y: " << kGear.Y() << "*" << kResRE.Y()/4 << "/" << kResSE.Y() << "=4*" << kGearTot.Y() << "/" << kResSE.Y() << endl;
|
|---|
| 995 | }
|
|---|
| 996 | /*
|
|---|
| 997 | void MCosy::InitSync()
|
|---|
| 998 | {
|
|---|
| 999 | if (!fMac3)
|
|---|
| 1000 | {
|
|---|
| 1001 | lout << "Unable to Init Sync! Mac3 not available." << endl;
|
|---|
| 1002 | return;
|
|---|
| 1003 | }
|
|---|
| 1004 |
|
|---|
| 1005 | const int res = fMac3->GetVelRes();
|
|---|
| 1006 |
|
|---|
| 1007 | fMac3->SetVelocity(0.3*res);
|
|---|
| 1008 | fMac3->SetAcceleration(0.2*res);
|
|---|
| 1009 | fMac3->SetDeceleration(0.2*res);
|
|---|
| 1010 | fMac3->StartPosSync();
|
|---|
| 1011 | }
|
|---|
| 1012 | */
|
|---|
| 1013 | void MCosy::TalkThreadSeTest()
|
|---|
| 1014 | {
|
|---|
| 1015 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
|---|
| 1016 | // return;
|
|---|
| 1017 |
|
|---|
| 1018 | if (fHist)
|
|---|
| 1019 | {
|
|---|
| 1020 | lout << "You are much too fast... try again." << endl;
|
|---|
| 1021 | return;
|
|---|
| 1022 | }
|
|---|
| 1023 |
|
|---|
| 1024 | fHist = new TH2F("Diff", "Difference of SE values",
|
|---|
| 1025 | 201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
|
|---|
| 1026 | fHist->SetXTitle("ZA [\\circ]");
|
|---|
| 1027 | fHist->SetYTitle("\\Delta SE");
|
|---|
| 1028 |
|
|---|
| 1029 | Double_t offset = 0;
|
|---|
| 1030 |
|
|---|
| 1031 | int cnt = 0;
|
|---|
| 1032 |
|
|---|
| 1033 | lout << "Starting Shaftencoder Test..." << endl;
|
|---|
| 1034 |
|
|---|
| 1035 | while (fBackground==kBgdSeTest)
|
|---|
| 1036 | {
|
|---|
| 1037 | fZd1->ResetPosHasChanged();
|
|---|
| 1038 | fZd2->ResetPosHasChanged();
|
|---|
| 1039 |
|
|---|
| 1040 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
|---|
| 1041 | fBackground==kBgdSeTest)
|
|---|
| 1042 | usleep(1);
|
|---|
| 1043 |
|
|---|
| 1044 | const Double_t pos[3] = {
|
|---|
| 1045 | (fZd1->GetPos()+8192)%16384,
|
|---|
| 1046 | (fZd2->GetPos()+8192)%16384,
|
|---|
| 1047 | fAz->GetPos() };
|
|---|
| 1048 |
|
|---|
| 1049 | //
|
|---|
| 1050 | // Estimate Offset from the first ten positions
|
|---|
| 1051 | //
|
|---|
| 1052 | if (cnt++<10)
|
|---|
| 1053 | {
|
|---|
| 1054 | offset += pos[0]+pos[1];
|
|---|
| 1055 | continue;
|
|---|
| 1056 | }
|
|---|
| 1057 | if (cnt==11)
|
|---|
| 1058 | {
|
|---|
| 1059 | offset /= 10;
|
|---|
| 1060 | cnt++;
|
|---|
| 1061 | }
|
|---|
| 1062 |
|
|---|
| 1063 | Double_t apos = (pos[0]-pos[1])/2 * TMath::TwoPi() / kResSE.X();
|
|---|
| 1064 |
|
|---|
| 1065 | ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
|
|---|
| 1066 | fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
|
|---|
| 1067 | }
|
|---|
| 1068 |
|
|---|
| 1069 | lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;
|
|---|
| 1070 |
|
|---|
| 1071 | fBackground=kBgdSeTestDispl;
|
|---|
| 1072 | }
|
|---|
| 1073 |
|
|---|
| 1074 | void MCosy::TalkThreadGear()
|
|---|
| 1075 | {
|
|---|
| 1076 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
|---|
| 1077 | // return;
|
|---|
| 1078 |
|
|---|
| 1079 | if (fHist)
|
|---|
| 1080 | {
|
|---|
| 1081 | lout << "You are much too fast... try again." << endl;
|
|---|
| 1082 | return;
|
|---|
| 1083 | }
|
|---|
| 1084 |
|
|---|
| 1085 | fHist = new TH3F("Gear", "Gear Ratio Re/Se",
|
|---|
| 1086 | (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(),
|
|---|
| 1087 | (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(),
|
|---|
| 1088 | 61, 349.5, 500.5);
|
|---|
| 1089 |
|
|---|
| 1090 | fHist->SetXTitle("Zd [\\circ]");
|
|---|
| 1091 | fHist->SetYTitle("Az [\\circ]");
|
|---|
| 1092 | fHist->SetZTitle("Re/Se");
|
|---|
| 1093 |
|
|---|
| 1094 | lout << "Starting Gear determination..." << endl;
|
|---|
| 1095 |
|
|---|
| 1096 | ZdAz se0 = GetSePos();
|
|---|
| 1097 | ZdAz re0 = GetRePosPdo();
|
|---|
| 1098 |
|
|---|
| 1099 | while (fBackground==kBgdGear)
|
|---|
| 1100 | {
|
|---|
| 1101 | fZd1->ResetPosHasChanged();
|
|---|
| 1102 | fZd2->ResetPosHasChanged();
|
|---|
| 1103 | fAz->ResetPosHasChanged();
|
|---|
| 1104 |
|
|---|
| 1105 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
|---|
| 1106 | !fAz->PosHasChanged() && fBackground==kBgdGear)
|
|---|
| 1107 | usleep(1);
|
|---|
| 1108 |
|
|---|
| 1109 | ZdAz se = GetSePos();
|
|---|
| 1110 | ZdAz re = GetRePosPdo();
|
|---|
| 1111 |
|
|---|
| 1112 | ZdAz dse = se-se0;
|
|---|
| 1113 | ZdAz dre = re-re0;
|
|---|
| 1114 |
|
|---|
| 1115 | if (fabs(dse.Zd())*144>kResSE.X()) // Each 2.5deg (144)
|
|---|
| 1116 | {
|
|---|
| 1117 | se0.Zd(se.Zd());
|
|---|
| 1118 | re0.Zd(re.Zd());
|
|---|
| 1119 |
|
|---|
| 1120 | se -= dse/2;
|
|---|
| 1121 |
|
|---|
| 1122 | ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg;
|
|---|
| 1123 | ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
|
|---|
| 1124 | }
|
|---|
| 1125 |
|
|---|
| 1126 | if (fabs(dse.Az())*144>kResSE.Y()) // Each 2.5deg (144)
|
|---|
| 1127 | {
|
|---|
| 1128 | se0.Az(se.Az());
|
|---|
| 1129 | re0.Az(re.Az());
|
|---|
| 1130 |
|
|---|
| 1131 | se -= dse/2;
|
|---|
| 1132 |
|
|---|
| 1133 | ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg;
|
|---|
| 1134 | ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az());
|
|---|
| 1135 | }
|
|---|
| 1136 | }
|
|---|
| 1137 | lout << "Gear Test Stopped... displaying Histogram." << endl;
|
|---|
| 1138 |
|
|---|
| 1139 | fBackground=kBgdGearDispl;
|
|---|
| 1140 | }
|
|---|
| 1141 |
|
|---|
| 1142 | void MCosy::TalkThread()
|
|---|
| 1143 | {
|
|---|
| 1144 | /* ========== FIXME? =============
|
|---|
| 1145 | if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
|
|---|
| 1146 | return;
|
|---|
| 1147 | */
|
|---|
| 1148 |
|
|---|
| 1149 | if (fMac1 && fMac2)
|
|---|
| 1150 | {
|
|---|
| 1151 | fMac1->ReqPos();
|
|---|
| 1152 | fMac2->ReqPos();
|
|---|
| 1153 | }
|
|---|
| 1154 |
|
|---|
| 1155 | //InitSync();
|
|---|
| 1156 |
|
|---|
| 1157 | /*** FOR DEMO MODE ***/
|
|---|
| 1158 | if (!fZd1 || !fZd2 || !fAz)
|
|---|
| 1159 | return;
|
|---|
| 1160 | /*** FOR DEMO MODE ***/
|
|---|
| 1161 |
|
|---|
| 1162 | //
|
|---|
| 1163 | // Start the Network
|
|---|
| 1164 | //
|
|---|
| 1165 | while (1)
|
|---|
| 1166 | {
|
|---|
| 1167 | //
|
|---|
| 1168 | // wait until a tracking session is started
|
|---|
| 1169 | //
|
|---|
| 1170 | while (fBackground==kBgdNone)
|
|---|
| 1171 | usleep(1);
|
|---|
| 1172 |
|
|---|
| 1173 | switch (fBackground)
|
|---|
| 1174 | {
|
|---|
| 1175 | case kBgdNone:
|
|---|
| 1176 | continue;
|
|---|
| 1177 | /*#ifndef NEWALGO
|
|---|
| 1178 | case kBgdTracking:
|
|---|
| 1179 | TalkThreadTracking();
|
|---|
| 1180 | continue;
|
|---|
| 1181 | #endif*/
|
|---|
| 1182 | case kBgdSeTest:
|
|---|
| 1183 | TalkThreadSeTest();
|
|---|
| 1184 | continue;
|
|---|
| 1185 |
|
|---|
| 1186 | case kBgdGear:
|
|---|
| 1187 | TalkThreadGear();
|
|---|
| 1188 | continue;
|
|---|
| 1189 |
|
|---|
| 1190 | default:
|
|---|
| 1191 | continue;
|
|---|
| 1192 | }
|
|---|
| 1193 | }
|
|---|
| 1194 | }
|
|---|
| 1195 |
|
|---|
| 1196 | ZdAz MCosy::GetPointingPos() const
|
|---|
| 1197 | {
|
|---|
| 1198 | if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
|
|---|
| 1199 | return ZdAz(0, 0);
|
|---|
| 1200 |
|
|---|
| 1201 | // GetPointingPos [deg]
|
|---|
| 1202 | const ZdAz seist = GetSePos()*TMath::TwoPi()/kResSE; // [rad]
|
|---|
| 1203 | return fBending.CorrectBack(seist)*TMath::RadToDeg();
|
|---|
| 1204 | }
|
|---|
| 1205 |
|
|---|
| 1206 | Bool_t MCosy::HandleTimer(TTimer *t)
|
|---|
| 1207 | {
|
|---|
| 1208 | const Int_t rc = fMutexGui.TryLock();
|
|---|
| 1209 | if (rc==13)
|
|---|
| 1210 | cout << "MCosy::HandleTimer - mutex is already locked by this thread" << endl;
|
|---|
| 1211 |
|
|---|
| 1212 | if (rc)
|
|---|
| 1213 | {
|
|---|
| 1214 | lout << "* GUI update skipped due to locked mutex." << endl;
|
|---|
| 1215 | return kTRUE;
|
|---|
| 1216 | }
|
|---|
| 1217 |
|
|---|
| 1218 | //
|
|---|
| 1219 | // Update Gui, foremer MTGui.
|
|---|
| 1220 | //
|
|---|
| 1221 | if (fZd1)
|
|---|
| 1222 | fZd1->DisplayVal();
|
|---|
| 1223 | if (fZd2)
|
|---|
| 1224 | fZd2->DisplayVal();
|
|---|
| 1225 | if (fAz)
|
|---|
| 1226 | fAz->DisplayVal();
|
|---|
| 1227 |
|
|---|
| 1228 | Byte_t avail = 0;
|
|---|
| 1229 |
|
|---|
| 1230 | avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
|
|---|
| 1231 | avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
|
|---|
| 1232 | avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
|
|---|
| 1233 | avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0;
|
|---|
| 1234 | avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0;
|
|---|
| 1235 | avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0;
|
|---|
| 1236 | // avail |= (!(fStatus&MDriveCom::kError) && 1 ? 0x40 : 0;
|
|---|
| 1237 |
|
|---|
| 1238 | if (HasError())
|
|---|
| 1239 | SetStatus(MDriveCom::kError);
|
|---|
| 1240 |
|
|---|
| 1241 |
|
|---|
| 1242 | ZdAz bendist = fStatus&MDriveCom::kTracking ? fTrackingPos : GetPointingPos();
|
|---|
| 1243 |
|
|---|
| 1244 | //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;
|
|---|
| 1245 |
|
|---|
| 1246 | fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError);
|
|---|
| 1247 |
|
|---|
| 1248 | fWin->UpdateWeather(*fCom);
|
|---|
| 1249 | fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/
|
|---|
| 1250 | fRaDec, fZdAzSoll, fStatus, avail);
|
|---|
| 1251 |
|
|---|
| 1252 | lout.UpdateGui();
|
|---|
| 1253 |
|
|---|
| 1254 | const Bool_t trigger = fTriggerDisplay;
|
|---|
| 1255 | fTriggerDisplay = kFALSE;
|
|---|
| 1256 |
|
|---|
| 1257 | if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
|
|---|
| 1258 | DisplayHistTestSe(!trigger);
|
|---|
| 1259 |
|
|---|
| 1260 | if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
|
|---|
| 1261 | DisplayHistGear(!trigger);
|
|---|
| 1262 |
|
|---|
| 1263 | if (fMutexGui.UnLock()==13)
|
|---|
| 1264 | cout << "MCosy::HandleTimer - tried to unlock mutex locked by other thread." << endl;
|
|---|
| 1265 |
|
|---|
| 1266 | return kTRUE;
|
|---|
| 1267 | }
|
|---|
| 1268 |
|
|---|
| 1269 | void MCosy::DisplayHistTestSe(Bool_t del)
|
|---|
| 1270 | {
|
|---|
| 1271 | lout << "Displaying histogram..." << endl;
|
|---|
| 1272 |
|
|---|
| 1273 | TH2F &hist = *(TH2F*)fHist;
|
|---|
| 1274 |
|
|---|
| 1275 | if (del)
|
|---|
| 1276 | {
|
|---|
| 1277 | fHist = NULL;
|
|---|
| 1278 | fBackground = kBgdNone;
|
|---|
| 1279 | }
|
|---|
| 1280 |
|
|---|
| 1281 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
|---|
| 1282 | c->Divide(1,2);
|
|---|
| 1283 |
|
|---|
| 1284 | c->cd(1);
|
|---|
| 1285 | TH2 *h=(TH2*)hist.DrawCopy();
|
|---|
| 1286 |
|
|---|
| 1287 | TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1288 | p->SetLineColor(kBlue);
|
|---|
| 1289 | p->Draw("same");
|
|---|
| 1290 | p->SetBit(kCanDelete);
|
|---|
| 1291 |
|
|---|
| 1292 | c->cd(2);
|
|---|
| 1293 |
|
|---|
| 1294 | TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
|
|---|
| 1295 | hist.GetBinLowEdge(hist.GetNbinsX()+1));
|
|---|
| 1296 | p2.SetXTitle("Zd [\\circ]");
|
|---|
| 1297 | for (int i=0; i<hist.GetNbinsX(); i++)
|
|---|
| 1298 | p2.SetBinError(i, p->GetBinError(i));
|
|---|
| 1299 | p2.SetLineColor(kRed);
|
|---|
| 1300 | p2.SetStats(0);
|
|---|
| 1301 | p2.DrawCopy();
|
|---|
| 1302 |
|
|---|
| 1303 | if (del)
|
|---|
| 1304 | delete &hist;
|
|---|
| 1305 | }
|
|---|
| 1306 |
|
|---|
| 1307 | void MCosy::DisplayHistGear(Bool_t del)
|
|---|
| 1308 | {
|
|---|
| 1309 | lout << "Displaying histogram..." << endl;
|
|---|
| 1310 |
|
|---|
| 1311 | TH3F &hist = *(TH3F*)fHist;
|
|---|
| 1312 |
|
|---|
| 1313 | if (del)
|
|---|
| 1314 | {
|
|---|
| 1315 | fHist = NULL;
|
|---|
| 1316 | fBackground = kBgdNone;
|
|---|
| 1317 | }
|
|---|
| 1318 |
|
|---|
| 1319 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
|---|
| 1320 | c->Divide(2,2);
|
|---|
| 1321 |
|
|---|
| 1322 | // ----------
|
|---|
| 1323 |
|
|---|
| 1324 | c->cd(1);
|
|---|
| 1325 | TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd
|
|---|
| 1326 | h1.SetTitle(" Gear Ratio Zenith Distance [re/se] ");
|
|---|
| 1327 | h1.SetXTitle("Zd [\\circ]");
|
|---|
| 1328 | h1.Draw();
|
|---|
| 1329 | h1.SetBit(kCanDelete);
|
|---|
| 1330 |
|
|---|
| 1331 | TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1332 | p1->SetLineColor(kBlue);
|
|---|
| 1333 | p1->Draw("same");
|
|---|
| 1334 | p1->SetBit(kCanDelete);
|
|---|
| 1335 |
|
|---|
| 1336 | // ----------
|
|---|
| 1337 |
|
|---|
| 1338 | c->cd(2);
|
|---|
| 1339 | TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az
|
|---|
| 1340 | h2.SetTitle(" Gear Ratio Azimuth [re/se] ");
|
|---|
| 1341 | h2.SetXTitle("Zd [\\circ]");
|
|---|
| 1342 | h2.Draw();
|
|---|
| 1343 | h2.SetBit(kCanDelete);
|
|---|
| 1344 |
|
|---|
| 1345 | TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s");
|
|---|
| 1346 | p2->SetLineColor(kBlue);
|
|---|
| 1347 | p2->Draw("same");
|
|---|
| 1348 | p2->SetBit(kCanDelete);
|
|---|
| 1349 |
|
|---|
| 1350 | // ----------
|
|---|
| 1351 |
|
|---|
| 1352 | c->cd(3);
|
|---|
| 1353 |
|
|---|
| 1354 | TAxis &axe1 = *h1.GetXaxis();
|
|---|
| 1355 |
|
|---|
| 1356 | TH1F f1("spreadzd", " Spread Zenith Distance ",
|
|---|
| 1357 | axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax());
|
|---|
| 1358 | f1.SetXTitle("Zd [\\circ]");
|
|---|
| 1359 | for (int i=0; i<axe1.GetNbins(); i++)
|
|---|
| 1360 | f1.SetBinError(i, p1->GetBinError(i));
|
|---|
| 1361 | f1.SetLineColor(kRed);
|
|---|
| 1362 | f1.SetStats(0);
|
|---|
| 1363 | f1.DrawCopy();
|
|---|
| 1364 |
|
|---|
| 1365 | c->cd(4);
|
|---|
| 1366 |
|
|---|
| 1367 | // ----------
|
|---|
| 1368 |
|
|---|
| 1369 | TAxis &axe2 = *h2.GetXaxis();
|
|---|
| 1370 |
|
|---|
| 1371 | TH1F f2("spreadaz", " Spread Azimuth ",
|
|---|
| 1372 | axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax());
|
|---|
| 1373 | f2.SetXTitle("Az [\\circ]");
|
|---|
| 1374 | for (int i=0; i<axe2.GetNbins(); i++)
|
|---|
| 1375 | f2.SetBinError(i, p2->GetBinError(i));
|
|---|
| 1376 | f2.SetLineColor(kRed);
|
|---|
| 1377 | f2.SetStats(0);
|
|---|
| 1378 | f2.DrawCopy();
|
|---|
| 1379 |
|
|---|
| 1380 | // ----------
|
|---|
| 1381 |
|
|---|
| 1382 | if (del)
|
|---|
| 1383 | delete &hist;
|
|---|
| 1384 | }
|
|---|
| 1385 |
|
|---|
| 1386 | // --------------------------------------------------------------------------
|
|---|
| 1387 | //
|
|---|
| 1388 | // Start the work of the application:
|
|---|
| 1389 | //
|
|---|
| 1390 | // Start the Can-Network.
|
|---|
| 1391 | // Start the MCosy::TalkThread thread.
|
|---|
| 1392 | // turn on the gui update
|
|---|
| 1393 | //
|
|---|
| 1394 | void MCosy::Start()
|
|---|
| 1395 | {
|
|---|
| 1396 | // Don't call this function twice!
|
|---|
| 1397 | Network::Start();
|
|---|
| 1398 |
|
|---|
| 1399 | CheckForError();
|
|---|
| 1400 |
|
|---|
| 1401 | ReadConfig();
|
|---|
| 1402 |
|
|---|
| 1403 | lout << "- Starting TX Thread." << endl;
|
|---|
| 1404 | fTTalk = new MTTalk(this);
|
|---|
| 1405 |
|
|---|
| 1406 | lout << "- Starting GUI update." << endl;
|
|---|
| 1407 | fUpdateGui->TurnOn();
|
|---|
| 1408 | }
|
|---|
| 1409 |
|
|---|
| 1410 | // --------------------------------------------------------------------------
|
|---|
| 1411 | //
|
|---|
| 1412 | // Start the work of the application:
|
|---|
| 1413 | //
|
|---|
| 1414 | // Turn of the gui update
|
|---|
| 1415 | // stop the MCosy::TalkThread thread.
|
|---|
| 1416 | // Stop the network
|
|---|
| 1417 | //
|
|---|
| 1418 | void MCosy::Stop()
|
|---|
| 1419 | {
|
|---|
| 1420 | lout << "- Stopping GUI update." << endl;
|
|---|
| 1421 | fUpdateGui->TurnOff();
|
|---|
| 1422 | lout << "- GUI Update stopped." << endl;
|
|---|
| 1423 |
|
|---|
| 1424 | delete fTTalk;
|
|---|
| 1425 | lout << "- TX Thread stopped." << endl;
|
|---|
| 1426 |
|
|---|
| 1427 | Network::Stop();
|
|---|
| 1428 | }
|
|---|
| 1429 |
|
|---|
| 1430 | // --------------------------------------------------------------------------
|
|---|
| 1431 | //
|
|---|
| 1432 | // Disable the synchronization by using a negative CAN Id for id2.
|
|---|
| 1433 | //
|
|---|
| 1434 | void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
|
|---|
| 1435 | Int_t id4, Int_t id5, Int_t id6)
|
|---|
| 1436 | {
|
|---|
| 1437 | //
|
|---|
| 1438 | // Create Nodes
|
|---|
| 1439 | //
|
|---|
| 1440 | lout << "- Setting up network." << endl;
|
|---|
| 1441 |
|
|---|
| 1442 | fMac1=new Macs(id1, "Mac/Az", lout);
|
|---|
| 1443 | fMac2=new Macs(id3, "Mac/Zd", lout);
|
|---|
| 1444 | if (id2>=0)
|
|---|
| 1445 | fMac3=new Macs(id2, "Mac/Az-Sync", lout);
|
|---|
| 1446 |
|
|---|
| 1447 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
|---|
| 1448 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
|---|
| 1449 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
|---|
| 1450 |
|
|---|
| 1451 | fZd1->SetReport(fOutRep);
|
|---|
| 1452 | fZd2->SetReport(fOutRep);
|
|---|
| 1453 | fAz->SetReport(fOutRep);
|
|---|
| 1454 |
|
|---|
| 1455 | fAz->SetMotor(fMac1);
|
|---|
| 1456 | fZd1->SetMotor(fMac2);
|
|---|
| 1457 | fZd2->SetMotor(fMac2);
|
|---|
| 1458 |
|
|---|
| 1459 | lout << "- Connecting devices to network." << endl;
|
|---|
| 1460 |
|
|---|
| 1461 | //
|
|---|
| 1462 | // Connect the devices to the network
|
|---|
| 1463 | //
|
|---|
| 1464 | SetNode(fMac1);
|
|---|
| 1465 | SetNode(fMac2);
|
|---|
| 1466 | if (id2>=0)
|
|---|
| 1467 | SetNode(fMac3);
|
|---|
| 1468 | SetNode(fZd1);
|
|---|
| 1469 | SetNode(fZd2);
|
|---|
| 1470 | SetNode(fAz);
|
|---|
| 1471 |
|
|---|
| 1472 | //
|
|---|
| 1473 | // Create Gui Event timer and Gui
|
|---|
| 1474 | //
|
|---|
| 1475 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 1476 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 1477 |
|
|---|
| 1478 | lout << "- Starting GUI." << endl;
|
|---|
| 1479 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 1480 | }
|
|---|
| 1481 | /*
|
|---|
| 1482 | void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
|
|---|
| 1483 | {
|
|---|
| 1484 | //
|
|---|
| 1485 | // Create Nodes
|
|---|
| 1486 | //
|
|---|
| 1487 | lout << "- Setting up network." << endl;
|
|---|
| 1488 |
|
|---|
| 1489 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
|---|
| 1490 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
|---|
| 1491 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
|---|
| 1492 |
|
|---|
| 1493 | lout << "- Connecting devices to network." << endl;
|
|---|
| 1494 |
|
|---|
| 1495 | //
|
|---|
| 1496 | // Connect the devices to the network
|
|---|
| 1497 | //
|
|---|
| 1498 | SetNode(fZd1);
|
|---|
| 1499 | SetNode(fZd2);
|
|---|
| 1500 | SetNode(fAz);
|
|---|
| 1501 |
|
|---|
| 1502 | //
|
|---|
| 1503 | // Create Gui Event timer and Gui
|
|---|
| 1504 | //
|
|---|
| 1505 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 1506 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 1507 |
|
|---|
| 1508 | lout << "- Starting GUI." << endl;
|
|---|
| 1509 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 1510 | }
|
|---|
| 1511 |
|
|---|
| 1512 | void MCosy::ConstructorDemo()
|
|---|
| 1513 | {
|
|---|
| 1514 | //
|
|---|
| 1515 | // Create Nodes
|
|---|
| 1516 | //
|
|---|
| 1517 | lout << "- Setting up network." << endl;
|
|---|
| 1518 |
|
|---|
| 1519 | //
|
|---|
| 1520 | // Create Gui Event timer and Gui
|
|---|
| 1521 | //
|
|---|
| 1522 | lout << "- Initializing GUI Timer." << endl;
|
|---|
| 1523 | fUpdateGui = new TTimer(this, 100); // 100ms
|
|---|
| 1524 |
|
|---|
| 1525 | lout << "- Starting GUI." << endl;
|
|---|
| 1526 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
|---|
| 1527 | }
|
|---|
| 1528 | */
|
|---|
| 1529 |
|
|---|
| 1530 | TString MCosy::GetFileName(const char *path, const char *name, const char *ext)
|
|---|
| 1531 | {
|
|---|
| 1532 | // FIXME: Timeout missing
|
|---|
| 1533 |
|
|---|
| 1534 | while (1)
|
|---|
| 1535 | {
|
|---|
| 1536 | MTime time(-1);
|
|---|
| 1537 |
|
|---|
| 1538 | // This is the full qualified date which is part of the name
|
|---|
| 1539 | const TString clock = time.GetStringFmt("%Y%m%d_%H%M%S");
|
|---|
| 1540 |
|
|---|
| 1541 | // This gives the night in which the date belongs to
|
|---|
| 1542 | time.SetMjd(TMath::Nint(time.GetMjd()));
|
|---|
| 1543 |
|
|---|
| 1544 | const TString night = time.GetStringFmt("%Y_%m_%d");
|
|---|
| 1545 |
|
|---|
| 1546 | const TString dir = Form("%s/%s", path, night.Data());
|
|---|
| 1547 | const TString fname = Form("%s_%s.%s", name, clock.Data(), ext);
|
|---|
| 1548 |
|
|---|
| 1549 | const TString full = Form("%s/%s", dir.Data(), fname.Data());
|
|---|
| 1550 |
|
|---|
| 1551 | gSystem->mkdir(dir, kTRUE);
|
|---|
| 1552 |
|
|---|
| 1553 | if (gSystem->AccessPathName(full, kFileExists))
|
|---|
| 1554 | return full;
|
|---|
| 1555 |
|
|---|
| 1556 | break;// !!!!!!!!!!!!!!!!!!!!!!!
|
|---|
| 1557 |
|
|---|
| 1558 | usleep(1000);
|
|---|
| 1559 | }
|
|---|
| 1560 | return "";
|
|---|
| 1561 | }
|
|---|
| 1562 |
|
|---|
| 1563 | MCosy::MCosy(/*int mode,*/ const char *dev, const int baud, MLog &out)
|
|---|
| 1564 | : 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)
|
|---|
| 1565 | {
|
|---|
| 1566 | TEnv env(".cosyrc");
|
|---|
| 1567 | const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
|
|---|
| 1568 | const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
|
|---|
| 1569 | const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3
|
|---|
| 1570 | const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4
|
|---|
| 1571 | const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5
|
|---|
| 1572 | const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6
|
|---|
| 1573 |
|
|---|
| 1574 | TString name = GetFileName("rep", "cosy", "rep");
|
|---|
| 1575 | cout << "Open Repfile: " << name << endl;
|
|---|
| 1576 | fOutRep = new MLog(name, kTRUE);
|
|---|
| 1577 | *fOutRep << "[Drive Report File]" << endl;
|
|---|
| 1578 | *fOutRep << "Version <cvs>" << endl;
|
|---|
| 1579 | *fOutRep << "Date " << MTime(-1) << endl;
|
|---|
| 1580 | *fOutRep << "[Reports]" << endl;
|
|---|
| 1581 |
|
|---|
| 1582 | /*
|
|---|
| 1583 | lout << "- Program in ";
|
|---|
| 1584 | switch (mode)
|
|---|
| 1585 | {
|
|---|
| 1586 | case 0:
|
|---|
| 1587 | lout << "<<Standard mode>>" << endl;*/
|
|---|
| 1588 | fBending.Load("bending.txt");
|
|---|
| 1589 | Constructor(id1, id2, id3, id4, id5, id6);/*
|
|---|
| 1590 | break;
|
|---|
| 1591 | case 1:
|
|---|
| 1592 | lout << "<<SE mode>>" << endl;
|
|---|
| 1593 | fBending.Load("bending.txt");
|
|---|
| 1594 | ConstructorSE(id4, id5, id6);
|
|---|
| 1595 | break;
|
|---|
| 1596 | default:
|
|---|
| 1597 | lout << "<<Demo mode>>" << endl;
|
|---|
| 1598 | ConstructorDemo();
|
|---|
| 1599 | }
|
|---|
| 1600 | */
|
|---|
| 1601 | lout.SetOutputGui(fWin->GetLog(), kTRUE);
|
|---|
| 1602 |
|
|---|
| 1603 | fZd1->SetDisplay(fWin->GetLabel2());
|
|---|
| 1604 | fZd2->SetDisplay(fWin->GetLabel3());
|
|---|
| 1605 | fAz->SetDisplay(fWin->GetLabel1());
|
|---|
| 1606 |
|
|---|
| 1607 | fCom = new MDriveCom(this, *fOutRep);
|
|---|
| 1608 | fCom->Start();
|
|---|
| 1609 | }
|
|---|
| 1610 |
|
|---|
| 1611 | void MCosy::TerminateApp()
|
|---|
| 1612 | {
|
|---|
| 1613 | cout << "MCosy::TerminateApp()" << endl;
|
|---|
| 1614 | /*
|
|---|
| 1615 | Int_t rc;
|
|---|
| 1616 | TGMessageBox msg(this, gClient->GetRoot(),
|
|---|
| 1617 | "Information",
|
|---|
| 1618 | "Cosy is shutting down the system - this may take wa while!",
|
|---|
| 1619 | kMBIconExclamation,
|
|---|
| 1620 | kMBOK, //kMBClose
|
|---|
| 1621 | &rc, 0);
|
|---|
| 1622 | */
|
|---|
| 1623 |
|
|---|
| 1624 | lout.DisableOutputDevice(MLog::eGui);
|
|---|
| 1625 | // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
|
|---|
| 1626 | // lout.SetOutputGui(NULL, kFALSE);
|
|---|
| 1627 |
|
|---|
| 1628 | gApplication->Terminate(0);
|
|---|
| 1629 | }
|
|---|
| 1630 |
|
|---|
| 1631 | MCosy::~MCosy()
|
|---|
| 1632 | {
|
|---|
| 1633 | if (fOutTp)
|
|---|
| 1634 | {
|
|---|
| 1635 | *fOutTp << "END" << endl;
|
|---|
| 1636 | delete fOutTp;
|
|---|
| 1637 | }
|
|---|
| 1638 | delete fOutRep;
|
|---|
| 1639 |
|
|---|
| 1640 | cout << "Deleting GUI timer." << endl;
|
|---|
| 1641 |
|
|---|
| 1642 | delete fUpdateGui;
|
|---|
| 1643 | delete fCom;
|
|---|
| 1644 |
|
|---|
| 1645 | cout << "Deleting Nodes." << endl;
|
|---|
| 1646 |
|
|---|
| 1647 | fZd1->SetReport(0);
|
|---|
| 1648 | fZd2->SetReport(0);
|
|---|
| 1649 | fAz->SetReport(0);
|
|---|
| 1650 |
|
|---|
| 1651 | delete fAz;
|
|---|
| 1652 | delete fZd1;
|
|---|
| 1653 | delete fZd2;
|
|---|
| 1654 | delete fMac1;
|
|---|
| 1655 | delete fMac2;
|
|---|
| 1656 | if (fMac3)
|
|---|
| 1657 | delete fMac3;
|
|---|
| 1658 |
|
|---|
| 1659 | cout << "Deleting MGCosy." << endl;
|
|---|
| 1660 |
|
|---|
| 1661 | lout.DisableOutputDevice(MLog::eGui);
|
|---|
| 1662 |
|
|---|
| 1663 | delete fWin;
|
|---|
| 1664 |
|
|---|
| 1665 | cout << "MGCosy destructed." << endl;
|
|---|
| 1666 | }
|
|---|
