source: trunk/MagicSoft/Cosy/main/MCosy.cc@ 3106

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