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