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

Last change on this file since 4026 was 3897, checked in by tbretz, 21 years ago
*** empty log message ***
File size: 61.7 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//
714Bool_t MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
715{
716 Bool_t rc = kFALSE;
717
718 //
719 // How to limit the speed. If the wind comes and blowes
720 // we cannot forbid changing of the sign. But on the other hand
721 // we don't want fast changes!
722 //
723 ULong_t vrzd = fMac1->GetVelRes();
724 ULong_t vraz = fMac2->GetVelRes();
725
726#define sgn(x) (x<0?-1:1)
727
728 //
729 // When speed changes sign, the maximum allowed speed
730 // is 25% of the |v|
731 //
732 //const Float_t limit = 0.25;
733
734 //
735 // The maximum allowed speed while tracking is 10%
736 //
737 const Float_t maxtrack = 0.1;
738/*
739 if (sgn(vt->Az()) != sgn(vcalc.Az()))
740 vt->Az(0);
741// else
742 {
743 if (fabs(vt->Az()) < fabs(vcalc.Az()) *0.5)
744 vt->Az(0.5*vcalc.Az());
745
746 if (fabs(vt->Az()) > fabs(vcalc.Az()) *1.5)
747 vt->Az(1.5*vcalc.Az());
748 }
749
750 if (sgn(vt->Zd()) != sgn(vcalc.Zd()))
751 vt->Zd(0);
752// else
753 {
754 if (fabs(vt->Zd()) > fabs(vcalc.Az()) *1.5)
755 vt->Zd(1.5*vcalc.Zd());
756
757 if (fabs(vt->Zd()) < fabs(vcalc.Az()) *0.5)
758 vt->Zd(0.5*vcalc.Zd());
759 }
760 */
761 /*
762 if (sgn(vt->Az()) != sgn(vcalc.Az())
763 && fabs(vt->Az()) < limit*fabs(vcalc.Az())
764 )
765 {
766 lout << "Warning: Negative Azimuth speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
767 vt->Az(0);
768 }
769 else*/
770 if (fabs(vt->Az()) > maxtrack*vraz)
771 {
772 lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Az()) << " > " << maxtrack*vraz << ")... limited." << endl;
773 vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
774 rc=kTRUE;
775 }
776/*
777 if (sgn(vt->Zd()) != sgn(vcalc.Zd())
778 && fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
779 )
780 {
781 lout << "Warning: Negative Altitude speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
782 vt->Zd(0);
783 }
784 else
785 */ if (fabs(vt->Zd()) > maxtrack*vrzd)
786 {
787 lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Zd()) <<" > " << maxtrack*vrzd << ")... limited." << endl;
788 vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
789 rc=kTRUE;
790 }
791 return rc;
792}
793/*
794Bool_t MCosy::AlignTrackingPos(ZdAz pointing, ZdAz &za) const
795{
796 // pointing [deg]
797 if (pointing.Zd()<0)
798 {
799 pointing.Zd(-pointing.Zd());
800 pointing.Az(pointing.Az()+180);
801 }
802
803 const ZdAz se = GetSePos()*2*TMath::Pi()/16384; // [rad]
804 const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing
805
806 do
807 {
808 const Double_t d = unbendedse.Az() - pointing.Az();
809 if (d>-180 && d<=180)
810 break;
811
812 pointing.Az(pointing.Az()+TMath::Sign(360., d));
813 } while (1);
814
815 const Bool_t rc = CheckRange(pointing);
816 za = pointing/kRad2Deg; // [rad]
817
818 if (!rc)
819 lout << "Error: Aligned position out of Range." << endl;
820
821 return rc;
822}
823*/
824
825ZdAz MCosy::AlignTrackingPos(ZdAz pointing) const
826{
827 // pointing [rad]
828 // AlignTrackingPos [deg]
829
830 pointing *= kRad2Deg;
831
832 if (pointing.Zd()<0)
833 {
834 pointing.Zd(-pointing.Zd());
835 pointing.Az(pointing.Az()+180);
836 //lout << "ZD=-ZD Az+=180" << endl;
837 }
838
839 const ZdAz se = GetSePos()*2*TMath::Pi()/16384; // [rad]
840 const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing
841
842 //lout << "Unbended: " << unbendedse.Zd() << " " << unbendedse.Az() << endl;
843
844 do
845 {
846 const Double_t d = unbendedse.Az() - pointing.Az();
847 if (d>-180 && d<=180)
848 break;
849
850 //lout << "AZ += " << TMath::Sign(360., d) << endl;
851
852 pointing.Az(pointing.Az()+TMath::Sign(360., d));
853 } while (1);
854
855 return pointing/kRad2Deg;
856/*
857 const Bool_t rc = CheckRange(pointing);
858 za = pointing/kRad2Deg; // [rad]
859
860 if (!rc)
861 lout << "Error: Aligned position out of Range." << endl;
862
863 return rc;*/
864}
865
866Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
867{
868 ifstream fin("pointingpos.txt");
869 if (!fin)
870 return -1;
871
872 Double_t mjd0, zd, az;
873 fin >> mjd0 >> zd >> az;
874
875 mjd0 += 52000;
876
877 if (mjd0+1./24/60 <mjd)
878 return -1;
879
880 ZdAz point=AlignTrackingPos(ZdAz(zd, az)/kRad2Deg);
881 /*
882 if (!AlignTrackingPos(ZdAz(zd, az), point))
883 {
884 cout << "Starguider position couldn't be aligned..." << endl;
885 return -1;
886 }*/
887
888 // FIXME: Check Range missing!
889
890 const ZdAz diff = (dest-point)*kRad2Deg;
891
892 if (diff.Zd()>5 || diff.Az()>5)
893 {
894 cout << "Starguider deviation too large... dZd=" << diff.Zd() <<" dAz="<<diff.Az() << endl;
895 return -1;
896 }
897
898 dest -= point;
899 dest *= 16384/TMath::Pi()/2; // [se]
900 dest *= -kGearRatio; // [re]
901
902 cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl;
903
904 return (mjd-mjd0) * (24*60*60); // [s]
905}
906
907void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
908{
909 SlaStars sla(fObservatory);
910
911 //
912 // Position to actual position
913 //
914 sla.Now();
915 ZdAz dest = sla.CalcZdAz(dst);
916
917 lout << sla.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl;
918
919 // FIXME: Determin tracking start point by star culmination
920 if (dest.Az()<-TMath::Pi()/2)
921 {
922 lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
923 dest.Az(dest.Az() + TMath::Pi()*2);
924 }
925
926 if (dest.Az()>3*TMath::Pi()/2)
927 {
928 lout << "Substracting 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
929 dest.Az(dest.Az() -TMath::Pi()*2);
930 }
931
932 if (!SetPosition(dest, kTRUE))
933 //if (!SetPosition(dest, kFALSE))
934 {
935 lout << "Error: Cannot start tracking, positioning failed." << endl;
936 return;
937 }
938
939 //
940 // calculate offset from present se position
941 //
942 const ZdAz sepos = GetSePos()*kGearRatio;
943
944 if (!RequestRePos())
945 return;
946
947 //
948 // Estimate Offset before starting to track
949 //
950 fOffset = sepos-GetRePos();
951
952 /*
953 cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
954 cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
955 cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
956 */
957
958 //
959 // Init accelerations and Rpm Mode
960 //
961 if (!InitTracking())
962 {
963 StopMovement();
964 return;
965 }
966
967 XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
968
969 sla.Now();
970// lout << sla.GetTime() << " - Start tracking:";
971// lout << " Ra: " << xy.X() << "h " << "Dec: " << xy.Y() << "\xb0" << endl;
972
973/*#ifdef EXPERT
974 ofstream fout("coordinates.txt");
975 fout << xy;
976 fout.close();
977#endif
978*/ //
979 // Initialize Tracker (slalib or starguider)
980 //
981 fRaDec = dst;
982 fBackground = kBgdTracking;
983
984 ZdAz pos = sla.CalcZdAz(fRaDec);
985
986 lout << sla.GetTime() << " - Start Tracking: Ra=" <<xy.X() << "h Dec=";
987 lout << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;
988
989//--- ofstream fout("log/cosy.pos");
990//--- fout << "Tracking:";
991//--- fout << " Ra: " << Rad2Deg(dst.Ra()) << "\x9c ";
992//--- fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
993//--- fout << " Mjd/10ms V/re/min/4" << endl;
994
995 //
996 // We want to reach the theoretical position exactly in about 0.5s
997 //
998 // *OLD*const float dt = 1; // 1 second
999 const float dt = 5;//3; // 2 second
1000 while (!(Break() || HasError() || HasZombie()))
1001 {
1002 //
1003 // Request Target position for this moment
1004 //
1005 sla.Now(dt);
1006
1007 //
1008 // Request theoretical Position for a time in the future (To+dt) from CPU
1009 //
1010 const ZdAz pointing = sla.CalcZdAz(fRaDec); // soll pointing [rad]
1011
1012 //lout << sla.GetTime() << pointing.Zd()*kRad2Deg << " " << pointing.Az()*kRad2Deg << endl;
1013 /*
1014 ZdAz dest;
1015 if (!AlignTrackingPos(pointing, dest))
1016 break;
1017 */
1018 ZdAz dest = AlignTrackingPos(pointing);
1019
1020 // lout << "DEST: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
1021
1022 ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.;
1023 //lout << "Vcalc: " << dest.Zd() << " " << dest.Az() << endl;
1024 vcalc *= kGearRatio2*4./60.; // [re/min]
1025
1026 float dtime = -1;
1027 if (kFALSE /*fUseStarguider*/)
1028 dtime = Starguider(sla.GetMjd(), dest);
1029
1030 if (dtime<0)
1031 {
1032 dest = fBending(dest); // [rad]
1033
1034 //lout << "DEST-BEND: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
1035
1036 if (!CheckRange(dest))
1037 break;
1038
1039 dest *= 16384/TMath::Pi()/2; // [se]
1040 dest *= kGearRatio; // [re]
1041
1042 //
1043 // Request absolute position of rotary encoder from Macs
1044 //
1045 if (!RequestRePos())
1046 break;
1047
1048 //
1049 // distance between (To+dt) and To [re]
1050 // position time difference < 5usec
1051 // fOffset does the synchronization between the
1052 // Shaft- and the rotary encoders
1053 dest -= GetRePos() + fOffset;
1054
1055 dtime = dt;
1056
1057 ZdAz repos = GetRePos();
1058 // lout << "Repos: " << repos.Zd()/kGearRatio.X() << " " << repos.Az()*kGearRatio.Y() << endl;
1059 // repos /= kGearRatio;
1060 repos /= 16384/TMath::Pi()/2;
1061 repos *= kRad2Deg;
1062 }
1063
1064 //
1065 // Velocity to go [re/min] to reach the right position at time t+dt
1066 // correct for the duration of RaDec2AltAz
1067 //
1068 const ZdAz v = dest*60.0/(dtime/*-(fMac2->GetTime()-sla)*/);
1069
1070 //
1071 // calculate real velocity of future [re/min]
1072 // believing the Macs manual '/4' shouldn't be necessary, but it is.
1073 //
1074 ZdAz vt = v/4;
1075 if (LimitSpeed(&vt, vcalc))
1076 {
1077 lout << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <<endl;
1078 lout << "vt: " << vt.Zd() << " " << vt.Az() << "re/min" << endl;
1079 lout << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
1080 }
1081 vt.Round();
1082
1083 //
1084 // check if the drive is fast enough to follow the star
1085 //
1086 if (vt.Zd()>.9*fMac1->GetVelRes() || vt.Az()>.9*fMac2->GetVelRes())
1087 {
1088 lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
1089 break;
1090 }
1091
1092 //
1093 // Set theoretical velocity (as early after calculation as possible)
1094 // Maybe we should attenuate the changes
1095 //
1096 if (!SetVelocity(vt))
1097 break;
1098
1099 //
1100 // Now do 'unnecessary' things
1101 //
1102 fVelocity = vt/kGearRatio2*4;
1103
1104//--- const double mjd = fMac2->GetMjd();
1105//--- fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " ";
1106//--- fout << setw(4) << vt.Zd() << " ";
1107//--- fout << setw(4) << vt.Az() << endl;
1108 //
1109 // FIXME? Calculate an accuracy for the tracking system?
1110 // How good do we reach the calculated position in 'real'
1111 // re valus?
1112 //
1113
1114
1115 //
1116 // Update speed as often as possible.
1117 // make sure, that dt is around 10 times larger than the
1118 // update time
1119 //
1120 //
1121 // The loop should not be executed faster than the ramp of
1122 // a change in the velocity can be followed.
1123 // (This is important on fast machines >500MHz)
1124 //
1125 /*
1126 MTimeout t(1000);
1127 while (!t.HasTimedOut())
1128 usleep(1);
1129 */
1130 usleep(1000000); // 1s
1131 cout << "." << flush;
1132 //usleep(50000); // 0.05s
1133 }
1134
1135 sla.Now();
1136
1137 fBackground = kBgdNone;
1138 StopMovement();
1139
1140 lout << sla.GetTime() << " - Tracking stopped." << endl;
1141}
1142
1143// --------------------------------------------------------------------------
1144//
1145// Stops the movement of both motors.
1146//
1147// Sets the status to stopping. Sets the deceleration to 50% of the maximum.
1148// stops. Quits the revolution mode and wait for the end of the movement.
1149//
1150void MCosy::StopMovement()
1151{
1152 //
1153 // Set status to Stopping
1154 //
1155 SetStatus(MDriveCom::kStopping);
1156
1157 //
1158 // set deceleration to 50%
1159 //
1160 cout << "Stopping movement (dec=30%)..." << endl;
1161 if (fMac1 && fMac2)
1162 {
1163#ifdef EXPERT
1164 fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
1165 fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
1166#else
1167 fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
1168 fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
1169#endif
1170 fMac1->SetRpmMode(FALSE);
1171 fMac2->SetRpmMode(FALSE);
1172 }
1173
1174/*
1175 fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
1176 fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
1177
1178 fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
1179 fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
1180 fMac2->WaitForSdo(0x3000, 0);
1181 fMac1->WaitForSdo(0x3000, 0);
1182 fMac1->SetRpmMode(FALSE);
1183 fMac2->SetRpmMode(FALSE);
1184 */
1185
1186 //
1187 // Wait for the movement to really be finished.
1188 //
1189#ifdef EXPERT
1190 cout << "Waiting for end of movement..." << endl;
1191#endif
1192 WaitForEndMovement();
1193
1194 //
1195 // Check whether everything works fine.
1196 //
1197 CheckForError();
1198#ifdef EXPERT
1199 cout << "Movement stopped." << endl;
1200#endif
1201}
1202
1203void MCosy::StopTracking()
1204{
1205 //
1206 // Set status to Stopping
1207 //
1208 SetStatus(MDriveCom::kStopping);
1209
1210 //
1211 // set deceleration to 50%
1212 //
1213 cout << "Stopping tracking (dec=20%)..." << endl;
1214 fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
1215 fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
1216
1217 fMac2->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
1218 fMac1->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
1219 fMac2->WaitForSdo(0x3006, 1);
1220 fMac1->WaitForSdo(0x3006, 1);
1221
1222 cout << "Waiting for end of movement..." << endl;
1223 WaitForEndMovement();
1224
1225 //
1226 // Wait for the objects to be OKed.
1227 //
1228 fMac1->SetRpmMode(FALSE);
1229 fMac2->SetRpmMode(FALSE);
1230
1231 //
1232 // Wait for the movement to really be finished.
1233 //
1234 //cout << "Waiting for end of movement..." << endl;
1235 //WaitForEndMovement();
1236
1237 //
1238 // Check whether everything works fine.
1239 //
1240 CheckForError();
1241 cout << "Movement stopped." << endl;
1242}
1243
1244bool MCosy::CheckNetwork()
1245{
1246 //return kTRUE;
1247 //CheckConnections();
1248
1249 CheckForError();
1250
1251 if (HasZombie())
1252 {
1253 lout << "- Found Zombies in Network..." << endl;
1254 if (!RebootZombies())
1255 return false;
1256 }
1257
1258 /*
1259 FIXME HANDLING ERROR
1260 */
1261 if (HasError())
1262 {
1263 fMac1->HandleError();
1264 fMac2->HandleError();
1265 fMac3->HandleError();
1266 if (HasError() || HasZombie())
1267 return false;
1268 }
1269
1270 CheckForError();
1271 return true;
1272}
1273
1274void *MCosy::Proc(int msg, void *mp)
1275{
1276 switch (msg)
1277 {
1278 case WM_WAIT:
1279 cout << "Wait for execution of Proc(WM_*, ): done." << endl;
1280 return NULL;
1281
1282 case WM_STOP:
1283 cout << "MCosy::Proc: Stop." << endl;
1284 if (!CheckNetwork())
1285 return (void*)0xebb0;
1286 StopMovement();
1287 return NULL;
1288/*
1289 case WM_PRESET:
1290 cout << "WM_Preset: start." << endl;
1291 if (!CheckNetwork())
1292 return (void*)0xebb0;
1293 fZd1->SetPreset();
1294 fZd2->SetPreset();
1295 fAz->SetPreset();
1296 cout << "WM_Preset: done. (return 0xaffe)" << endl;
1297 return (void*)0xaffe;
1298*/
1299 /*
1300 case WM_CALIB:
1301 {
1302 cout << "WM_Calib: start." << endl;
1303 if (!CheckNetwork())
1304 return (void*)0xebb0;
1305
1306 SlaStars sla(fObservatory);
1307 sla.Now();
1308
1309 RaDec rd = *((RaDec*)mp);
1310
1311 //RaDec rd(37.94, 89.2644); // POLARIS
1312 //RaDec rd(213.915417, 19.1825); // ARCTURUS
1313
1314 cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
1315
1316 ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;
1317
1318 cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
1319
1320 ZdAz sepos = GetSePos();
1321 cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
1322
1323 fZd1->SetPreset(za.Zd());
1324 fZd2->SetPreset(-za.Zd());
1325 fAz->SetPreset(za.Az());
1326
1327 cout << "WM_Calib: done. (return 0xaffe)" << endl;
1328 }
1329 return (void*)0xaffe;
1330 */
1331 case WM_TPOINT:
1332 {
1333 cout << "WM_TPoint: start." << endl;
1334 SlaStars sla(fObservatory);
1335 sla.Now();
1336
1337 RaDec rd = *((RaDec*)mp);
1338 cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
1339
1340 AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;
1341
1342 cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
1343 *tpout << setprecision(7) << za.Az() << " " << za.Alt() << " ";
1344
1345 ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;;
1346 za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
1347 za *= kRad2Deg;
1348
1349 cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
1350 *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";
1351
1352 if (fStarguider)
1353 {
1354 XY tp = fStarguider->GetCoordinates();
1355 *tpout << 90-tp.X() << " " << tp.Y() << " ";
1356 }
1357
1358 *tpout << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;
1359
1360 cout << "WM_TPoint: done. (return 0xaffe)" << endl;
1361 }
1362 return (void*)0xca1b;
1363
1364 case WM_TRACKPOS:
1365 cout << "WM_TrackPosition: start." << endl;
1366 {
1367 if (!CheckNetwork())
1368 return (void*)0xebb0;
1369
1370 ZdAz dest = *((ZdAz*)mp) * kDeg2Rad;
1371 if (!SetPosition(dest))
1372 return (void*)0x1234;
1373
1374 SlaStars sla(fObservatory);
1375 sla.Now();
1376
1377 RaDec rd = sla.CalcRaDec(dest);
1378 cout << dest.Zd()*180/3.1415 << " " << dest.Az()*180/3.1415 << endl;
1379 cout << rd.Ra()*12/3.1415 << " " << rd.Dec()*180/3.1415 << endl;
1380 TrackPosition(rd);
1381 }
1382 cout << "WM_TrackPosition: done. (return 0xabcd)" << endl;
1383 return (void*)0xabcd;
1384
1385 case WM_POSITION:
1386 cout << "WM_Position: start." << endl;
1387 {
1388 if (!CheckNetwork())
1389 return (void*)0xebb0;
1390
1391 ZdAz dest = *((ZdAz*)mp);
1392 SetPosition(dest*kDeg2Rad);
1393 }
1394 cout << "WM_Position: done. (return 0x7777)" << endl;
1395 return (void*)0x7777;
1396
1397 case WM_POSITION1:
1398 cout << "WM_Position1: start." << endl;
1399 {
1400 if (!CheckNetwork())
1401 return (void*)0xebb0;
1402
1403 ZdAz dest = *((ZdAz*)mp);
1404 SetPosition(dest*kDeg2Rad, kTRUE);
1405 }
1406 cout << "WM_Position: done. (return 0x7777)" << endl;
1407 return (void*)0x7777;
1408
1409 case WM_TESTSE:
1410 cout << "WM_TestSe: start." << endl;
1411 fBackground = mp ? kBgdSeTest : kBgdNone;
1412 cout << "WM_TestSe: done. (return 0x1e51)" << endl;
1413 return (void*)0x1e51;
1414
1415 case WM_GEAR:
1416 cout << "WM_Gear: start." << endl;
1417 fBackground = mp ? kBgdGear : kBgdNone;
1418 cout << "WM_Gear: done. (return 0xfeaf)" << endl;
1419 return (void*)0xfeaf;
1420
1421 case WM_DISPLAY:
1422 cout << "WM_Display: start." << endl;
1423 fTriggerDisplay = kTRUE;
1424 cout << "WM_Disply: done. (return 0xd1e1)" << endl;
1425 return (void*)0xd1e1;
1426
1427 case WM_TRACK:
1428 cout << "WM_Track: START" << endl;
1429 {
1430 RaDec dest = ((RaDec*)mp)[0];
1431 if (fStarguider)
1432 fStarguider->SetPointingPosition(((RaDec*)mp)[1]);
1433 if (!CheckNetwork())
1434 return (void*)0xebb0;
1435 TrackPosition(dest*kDeg2Rad);
1436 }
1437 cout << "WM_Track: done. (return 0x8888)" << endl;
1438 return (void*)0x8888;
1439
1440 case WM_NEWTRACK:
1441 cout << "WM_NewTrack: START" << endl;
1442 fRaDec = *((RaDec*)mp);
1443 cout << "WM_NewTrack: done. (return 0x9999)" << endl;
1444 return (void*)0x9999;
1445
1446 case WM_LOADBENDING:
1447 cout << "WM_LoadBending: START" << endl;
1448 fBending.Load("bending.txt");
1449 cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
1450 return (void*)0xbe0d;
1451
1452 case WM_RESETBENDING:
1453 cout << "WM_ResetBending: START" << endl;
1454 fBending.Reset();
1455 cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
1456 return (void*)0xbe0e;
1457
1458 case WM_HOME:
1459 cout << "WM_Home: START" << endl;
1460 if (!CheckNetwork())
1461 return (void*)0xebb0;
1462 else
1463 {
1464 cout << "HOME NOT ALLOWED... for Magic." << endl;
1465 /*
1466 cout << "Going Home..." << endl;
1467 TEnv env(".cosyrc");
1468
1469 SetStatus(MDriveCom::kMoving);
1470
1471 fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
1472 fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
1473
1474 lout << "SETHOME DONE" << endl;
1475
1476 SetStatus(HasError() ? MDriveCom::kError : MDriveCom::kStopped);
1477
1478 fAz->SetPreset();
1479 fZd1->SetPreset();
1480 fZd2->SetPreset();
1481
1482 fMac1->ReqPos();
1483 fMac2->ReqPos();
1484 fMac3->StopMotor();
1485 */
1486 }
1487 cout << "WM_Home: done. (return 0x403e)" << endl;
1488 return (void*)0x403e;
1489
1490 case WM_CALCALTAZ:
1491 {
1492 cout << endl;
1493
1494 SlaStars sla(fObservatory);
1495 sla.Now();
1496
1497 XY xy = *((XY*)mp);
1498 RaDec rd(xy.X()*15., xy.Y()); // [deg]
1499
1500 ZdAz a1 = sla.CalcZdAz(rd*kDeg2Rad); // [rad]
1501
1502 cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
1503 cout << "Zd/Az target: " << a1.Zd()*kRad2Deg << "° " << a1.Az()*kRad2Deg << "°" << endl;
1504
1505 if (fZd1 && fZd2 && fAz)
1506 a1 = AlignTrackingPos(a1);
1507
1508 a1 = fBending(a1);
1509 CheckRange(a1);
1510 a1 *= kRad2Deg;
1511
1512 const ZdAz a2 = a1*16384/360;
1513
1514 cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
1515 cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
1516 }
1517 return (void*)0xa17a;
1518
1519 case WM_ENDSWITCH:
1520 {
1521 ZdAz pos = GetSePos()*TMath::Pi()*2/16384;
1522 pos = fBending.SubtractOffsets(pos)*kRad2Deg;
1523
1524 cout << "Endswitch Position: Zd=" << pos.Zd() << "° Az=";
1525 cout << pos.Az() << "°" << endl;
1526 }
1527
1528 return (void*)0x1010;
1529
1530 case WM_QUIT:
1531 cout << "WM_Quit: now." << endl;
1532 if (!CheckNetwork())
1533 {
1534 lout << "ERROR: Cannot shutdown CANbus network." << endl;
1535 return (void*)0xebb0;
1536 }
1537 TerminateApp();
1538 cout << "WM_Quit: done." << endl;
1539 return (void*)0xaaaa;
1540 }
1541 cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
1542 return (void*)0xffffffff;
1543}
1544
1545void *MTTalk::Thread()
1546{
1547 fCosy->TalkThread();
1548 return NULL;
1549}
1550
1551void MCosy::ReadConfig()
1552{
1553 cout << "Reading configuration file..." << flush;
1554 TEnv env(".cosyrc");
1555 cout << "done." << endl;
1556
1557 cout << "Reading telescope range..." << flush;
1558 const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
1559 const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
1560 fMin.Set(zmin, amin);
1561
1562 const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
1563 const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
1564 fMax.Set(zmax, amax);
1565 cout << "done." << endl;
1566
1567 cout << " * Min: " << zmin << "deg " << amin << "deg" << endl;
1568 cout << " * Max: " << zmax << "deg " << amax << "deg" << endl;
1569
1570 fMin = fBending.AddOffsets(fMin/kRad2Deg);
1571 fMax = fBending.AddOffsets(fMax/kRad2Deg);
1572
1573 cout << " * Min': " << fMin.Zd()*kRad2Deg << "deg " << fMin.Az()*kRad2Deg << "deg" << endl;
1574 cout << " * Max': " << fMax.Zd()*kRad2Deg << "deg " << fMax.Az()*kRad2Deg << "deg" << endl;
1575
1576 cout << "Reading gear ratios..." << flush;
1577 const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0);
1578 const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0);
1579
1580 Double_t resreaz = 0;
1581 if (fMac1 && !fMac1->IsZombieNode())
1582 resreaz = fMac1->GetRes();
1583 else
1584 if (fMac3 && !fMac3->IsZombieNode())
1585 resreaz = fMac3->GetRes();
1586 else
1587 resreaz = env.GetValue("Az_ResRE[re/U_mot]", 1500);
1588
1589 Double_t resrezd = 0;
1590 if (fMac2 && !fMac2->IsZombieNode())
1591 resrezd = fMac2->GetRes();
1592 else
1593 resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500);
1594
1595 Double_t ressezd = 0;
1596 if (fZd1 && !fZd1->IsZombieNode())
1597 ressezd = fZd1->GetPhysRes();
1598 else
1599 if (fZd2 && !fZd2->IsZombieNode())
1600 ressezd = fZd2->GetPhysRes();
1601 else
1602 ressezd = env.GetValue("Zd_ResSE[se/U_mot]", 16384);
1603
1604 Double_t resseaz = 0;
1605 if (fAz && !fAz->IsZombieNode())
1606 resseaz = fAz->GetPhysRes();
1607 else
1608 resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384);
1609
1610 kGearRatio.Set (gzd*resrezd*4/ressezd, gaz*resreaz*4/resseaz); //[re/se]
1611 kGearRatio2.Set(gzd*resrezd*4/360.0, gaz*resreaz*4/360.0); //[re/deg]
1612 cout << "done." << endl;
1613
1614 cout << " * Setting Gear Ratios:" << endl;
1615 cout << " --------------------" << endl;
1616 cout << " * X: " << gzd << "*" << resrezd << "/" << ressezd << "=4*" << kGearRatio.X() << endl;
1617 cout << " * Y: " << gaz << "*" << resreaz << "/" << resseaz << "=4*" << kGearRatio.Y() << endl;
1618}
1619
1620void MCosy::InitSync()
1621{
1622 if (!fMac3)
1623 {
1624 lout << "Unable to Init Sync! Mac3 not available." << endl;
1625 return;
1626 }
1627
1628 const int res = fMac3->GetVelRes();
1629
1630 fMac3->SetVelocity(0.3*res);
1631 fMac3->SetAcceleration(0.2*res);
1632 fMac3->SetDeceleration(0.2*res);
1633 fMac3->StartPosSync();
1634}
1635
1636void MCosy::TalkThreadTracking()
1637{
1638 if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
1639 return;
1640
1641 if (fAz->IsZombieNode())
1642 return;
1643
1644 if (!fMac1 || !fMac2)
1645 return;
1646
1647 lout << "- Tracking Thread started..." << endl;
1648
1649 SlaStars sla(fObservatory);
1650 sla.Now();
1651
1652 ZdAz old;
1653 ZdAz ist = GetSePos(); // [se]
1654
1655 ZdAz time;
1656
1657 ZdAz sollzd = sla.CalcZdAz(fRaDec); // [rad]
1658 ZdAz sollaz = sollzd; // [rad]
1659
1660 //
1661 // only update fTrackingError while tracking
1662 //
1663 bool phca1=false;
1664 bool phca2=false;
1665 bool phcaz=false;
1666
1667 while (fBackground==kBgdTracking)
1668 {
1669 //
1670 // Make changes (eg wind) smoother - attenuation of control function
1671 //
1672 const float weight = 1.; //0.3;
1673
1674 //
1675 // This is the time constant which defines how fast
1676 // you correct for external influences (like wind)
1677 //
1678 fZd1->ResetPosHasChanged();
1679 fZd2->ResetPosHasChanged();
1680 fAz->ResetPosHasChanged();
1681 do
1682 {
1683 phca1 = fZd1->PosHasChanged();
1684 phca2 = fZd2->PosHasChanged();
1685 phcaz = fAz->PosHasChanged();
1686 usleep(1);
1687 } while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking);
1688
1689 //---usleep(100000); // 0.1s
1690
1691 //
1692 // get position, where we are
1693 //
1694 old = ist;
1695 ist = GetSePos(); // [se]
1696
1697 //
1698 // if the position didn't change continue
1699 //
1700 /*---
1701 if ((int)ist.Zd() == (int)old.Zd() &&
1702 (int)ist.Az() == (int)old.Az())
1703 continue;
1704 */
1705 ZdAz istre = GetRePosPdo();
1706
1707 //
1708 // Get time from last shaftencoder position change (position: ist)
1709 // FIXME: I cannot take the avarage
1710 //
1711 // FIXME
1712 //time.Zd(fZd1->GetMjd());
1713 /* OLD* */
1714 if (fZd1->GetMjd()>fZd2->GetMjd())
1715 time.Zd(fZd1->GetMjd());
1716 else
1717 time.Zd(fZd2->GetMjd());
1718
1719 //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
1720 time.Az(fAz->GetMjd());
1721
1722 //
1723 // if Shaftencoder changed position
1724 // calculate were we should be
1725 //
1726 if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/)
1727 {
1728 sollzd = sla.CalcZdAz(fRaDec, time.Zd()); // [rad]
1729 /*
1730 ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
1731 sollzd = CorrectTarget(ist, dummy); // [se]
1732 */
1733 fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight);
1734 }
1735
1736 if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
1737 {
1738 sollaz = sla.CalcZdAz(fRaDec, time.Az()); // [rad]
1739 /*
1740 ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
1741 sollaz = CorrectTarget(ist, dummy); // [se]
1742 */
1743 fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
1744 }
1745
1746 ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad]
1747
1748 fZdAzSoll = AlignTrackingPos(soll);
1749
1750 ist *= TMath::Pi()*2/16384;
1751 soll = fBending(fZdAzSoll);
1752 fTrackingError.Set(ist.Zd()-soll.Zd(), ist.Az()-soll.Az());
1753
1754 //--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
1755 //--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " ";
1756 //--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " ";
1757 //--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl;
1758 }
1759
1760 lout << "- Tracking Thread done." << endl;
1761
1762 //--- fout << endl << endl;
1763}
1764
1765void MCosy::TalkThreadSeTest()
1766{
1767// if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
1768 // return;
1769
1770 if (fHist)
1771 {
1772 lout << "You are much too fast... try again." << endl;
1773 return;
1774 }
1775
1776 fHist = new TH2F("Diff", "Difference of SE values",
1777 201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
1778 fHist->SetXTitle("ZA [\\circ]");
1779 fHist->SetYTitle("\\Delta SE");
1780
1781 Double_t offset = 0;
1782
1783 int cnt = 0;
1784
1785 lout << "Starting Shaftencoder Test..." << endl;
1786
1787 while (fBackground==kBgdSeTest)
1788 {
1789 fZd1->ResetPosHasChanged();
1790 fZd2->ResetPosHasChanged();
1791
1792 while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
1793 fBackground==kBgdSeTest)
1794 usleep(1);
1795
1796 const Double_t pos[3] = {
1797 (fZd1->GetPos()+8192)%16384,
1798 (fZd2->GetPos()+8192)%16384,
1799 fAz->GetPos() };
1800
1801 //
1802 // Estimate Offset from the first ten positions
1803 //
1804 if (cnt++<10)
1805 {
1806 offset += pos[0]+pos[1];
1807 continue;
1808 }
1809 if (cnt==11)
1810 {
1811 offset /= 10;
1812 cnt++;
1813 }
1814
1815 Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
1816
1817 ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
1818 fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
1819 }
1820
1821 lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;
1822
1823 fBackground=kBgdSeTestDispl;
1824}
1825
1826void MCosy::TalkThreadGear()
1827{
1828// if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
1829 // return;
1830
1831 if (fHist)
1832 {
1833 lout << "You are much too fast... try again." << endl;
1834 return;
1835 }
1836
1837 fHist = new TH3F("Gear", "Gear Ratio Re/Se",
1838 (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(),
1839 (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(),
1840 61, 349.5, 500.5);
1841
1842 fHist->SetXTitle("Zd [\\circ]");
1843 fHist->SetYTitle("Az [\\circ]");
1844 fHist->SetZTitle("Re/Se");
1845
1846 lout << "Starting Gear determination..." << endl;
1847
1848 ZdAz se0 = GetSePos();
1849 ZdAz re0 = GetRePosPdo();
1850
1851 while (fBackground==kBgdGear)
1852 {
1853 fZd1->ResetPosHasChanged();
1854 fZd2->ResetPosHasChanged();
1855 fAz->ResetPosHasChanged();
1856
1857 while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
1858 !fAz->PosHasChanged() && fBackground==kBgdGear)
1859 usleep(1);
1860
1861 ZdAz se = GetSePos();
1862 ZdAz re = GetRePosPdo();
1863
1864 ZdAz dse = se-se0;
1865 ZdAz dre = re-re0;
1866
1867 if (fabs(dse.Zd())*144>16384) // Each 2.5deg (144)
1868 {
1869 se0.Zd(se.Zd());
1870 re0.Zd(re.Zd());
1871
1872 se -= dse/2;
1873
1874 ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
1875 ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
1876 }
1877
1878 if (fabs(dse.Az())*144>16384) // Each 2.5deg (144)
1879 {
1880 se0.Az(se.Az());
1881 re0.Az(re.Az());
1882
1883 se -= dse/2;
1884
1885 ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
1886 ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az());
1887 }
1888 }
1889 lout << "Gear Test Stopped... displaying Histogram." << endl;
1890
1891 fBackground=kBgdGearDispl;
1892}
1893
1894void MCosy::TalkThread()
1895{
1896 /* ========== FIXME? =============
1897 if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
1898 return;
1899 */
1900
1901 if (fMac1 && fMac2)
1902 {
1903 fMac1->ReqPos();
1904 fMac2->ReqPos();
1905 }
1906
1907 InitSync();
1908
1909 /*** FOR DEMO MODE ***/
1910 if (!fZd1 || !fZd2 || !fAz)
1911 return;
1912 /*** FOR DEMO MODE ***/
1913
1914 //
1915 // Start the Network
1916 //
1917 while (1)
1918 {
1919 //
1920 // wait until a tracking session is started
1921 //
1922 while (fBackground==kBgdNone)
1923 usleep(1);
1924
1925 switch (fBackground)
1926 {
1927 case kBgdNone:
1928 continue;
1929
1930 case kBgdTracking:
1931 TalkThreadTracking();
1932 continue;
1933
1934 case kBgdSeTest:
1935 TalkThreadSeTest();
1936 continue;
1937
1938 case kBgdGear:
1939 TalkThreadGear();
1940 continue;
1941
1942 default:
1943 continue;
1944 }
1945 }
1946}
1947
1948ZdAz MCosy::GetPointingPos() const
1949{
1950 if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
1951 return ZdAz(0, 0);
1952
1953 // GetPointingPos [deg]
1954 const ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
1955
1956 //cout << seist.Zd()*kRad2Deg << " " << seist.Az()*kRad2Deg << endl;
1957
1958 ZdAz back = fBending.CorrectBack(seist)*180/TMath::Pi();
1959
1960 //cout << back.Zd() << " " << back.Az() << endl;
1961
1962 return back;
1963}
1964
1965Bool_t MCosy::HandleTimer(TTimer *t)
1966{
1967 //
1968 // Update Gui, foremer MTGui.
1969 //
1970 if (fZd1)
1971 fZd1->DisplayVal();
1972 if (fZd2)
1973 fZd2->DisplayVal();
1974 if (fAz)
1975 fAz->DisplayVal();
1976
1977 ZdAz bendist = GetPointingPos();
1978
1979 Byte_t avail = 0;
1980
1981 avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
1982 avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
1983 avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
1984 avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0;
1985 avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0;
1986 avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0;
1987
1988 if (HasError())
1989 SetStatus(MDriveCom::kError);
1990
1991 lout.UpdateGui();
1992
1993 fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/
1994 fRaDec, fZdAzSoll, fStatus, avail);
1995
1996 const Bool_t trigger = fTriggerDisplay;
1997 fTriggerDisplay = kFALSE;
1998
1999 if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
2000 DisplayHistTestSe(!trigger);
2001
2002 if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
2003 DisplayHistGear(!trigger);
2004
2005 // FIXME: Not thread safe!
2006 static int i=0;
2007 if (i++==7)
2008 {
2009 fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError);
2010 i=0;
2011 }
2012 return kTRUE;
2013}
2014
2015void MCosy::DisplayHistTestSe(Bool_t del)
2016{
2017 lout << "Displaying histogram..." << endl;
2018
2019 TH2F &hist = *(TH2F*)fHist;
2020
2021 if (del)
2022 {
2023 fHist = NULL;
2024 fBackground = kBgdNone;
2025 }
2026
2027 TCanvas *c=new TCanvas("c1", "", 1000, 1000);
2028 c->Divide(1,2);
2029
2030 c->cd(1);
2031 TH2 *h=(TH2*)hist.DrawCopy();
2032
2033 TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
2034 p->SetLineColor(kBlue);
2035 p->Draw("same");
2036 p->SetBit(kCanDelete);
2037
2038 c->cd(2);
2039
2040 TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
2041 hist.GetBinLowEdge(hist.GetNbinsX()+1));
2042 p2.SetXTitle("Zd [\\circ]");
2043 for (int i=0; i<hist.GetNbinsX(); i++)
2044 p2.SetBinError(i, p->GetBinError(i));
2045 p2.SetLineColor(kRed);
2046 p2.SetStats(0);
2047 p2.DrawCopy();
2048
2049 if (del)
2050 delete &hist;
2051}
2052
2053void MCosy::DisplayHistGear(Bool_t del)
2054{
2055 lout << "Displaying histogram..." << endl;
2056
2057 TH3F &hist = *(TH3F*)fHist;
2058
2059 if (del)
2060 {
2061 fHist = NULL;
2062 fBackground = kBgdNone;
2063 }
2064
2065 TCanvas *c=new TCanvas("c1", "", 1000, 1000);
2066 c->Divide(2,2);
2067
2068 // ----------
2069
2070 c->cd(1);
2071 TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd
2072 h1.SetTitle(" Gear Ratio Zenith Distance [re/se] ");
2073 h1.SetXTitle("Zd [\\circ]");
2074 h1.Draw();
2075 h1.SetBit(kCanDelete);
2076
2077 TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s");
2078 p1->SetLineColor(kBlue);
2079 p1->Draw("same");
2080 p1->SetBit(kCanDelete);
2081
2082 // ----------
2083
2084 c->cd(2);
2085 TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az
2086 h2.SetTitle(" Gear Ratio Azimuth [re/se] ");
2087 h2.SetXTitle("Zd [\\circ]");
2088 h2.Draw();
2089 h2.SetBit(kCanDelete);
2090
2091 TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s");
2092 p2->SetLineColor(kBlue);
2093 p2->Draw("same");
2094 p2->SetBit(kCanDelete);
2095
2096 // ----------
2097
2098 c->cd(3);
2099
2100 TAxis &axe1 = *h1.GetXaxis();
2101
2102 TH1F f1("spreadzd", " Spread Zenith Distance ",
2103 axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax());
2104 f1.SetXTitle("Zd [\\circ]");
2105 for (int i=0; i<axe1.GetNbins(); i++)
2106 f1.SetBinError(i, p1->GetBinError(i));
2107 f1.SetLineColor(kRed);
2108 f1.SetStats(0);
2109 f1.DrawCopy();
2110
2111 c->cd(4);
2112
2113 // ----------
2114
2115 TAxis &axe2 = *h2.GetXaxis();
2116
2117 TH1F f2("spreadaz", " Spread Azimuth ",
2118 axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax());
2119 f2.SetXTitle("Az [\\circ]");
2120 for (int i=0; i<axe2.GetNbins(); i++)
2121 f2.SetBinError(i, p2->GetBinError(i));
2122 f2.SetLineColor(kRed);
2123 f2.SetStats(0);
2124 f2.DrawCopy();
2125
2126 // ----------
2127
2128 if (del)
2129 delete &hist;
2130}
2131
2132// --------------------------------------------------------------------------
2133//
2134// Start the work of the application:
2135//
2136// Start the Can-Network.
2137// Start the MCosy::TalkThread thread.
2138// turn on the gui update
2139//
2140void MCosy::Start()
2141{
2142 // Don't call this function twice!
2143 Network::Start();
2144
2145 CheckForError();
2146
2147 ReadConfig();
2148
2149 lout << "- Starting TX Thread." << endl;
2150 fTTalk = new MTTalk(this);
2151
2152 lout << "- Starting GUI update." << endl;
2153 fUpdateGui->TurnOn();
2154}
2155
2156// --------------------------------------------------------------------------
2157//
2158// Start the work of the application:
2159//
2160// Turn of the gui update
2161// stop the MCosy::TalkThread thread.
2162// Stop the network
2163//
2164void MCosy::Stop()
2165{
2166 lout << "- Stopping GUI update." << endl;
2167 fUpdateGui->TurnOff();
2168 lout << "- GUI Update stopped." << endl;
2169
2170 delete fTTalk;
2171 lout << "- TX Thread stopped." << endl;
2172
2173 Network::Stop();
2174}
2175
2176// --------------------------------------------------------------------------
2177//
2178// Disable the synchronization by using a negative CAN Id for id2.
2179//
2180void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
2181 Int_t id4, Int_t id5, Int_t id6)
2182{
2183 //
2184 // Create Nodes
2185 //
2186 lout << "- Setting up network." << endl;
2187
2188 fMac1=new Macs(id1, "Mac/Az", lout);
2189 fMac2=new Macs(id3, "Mac/Zd", lout);
2190 if (id2>=0)
2191 fMac3=new Macs(id2, "Mac/Az-Sync", lout);
2192
2193 fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
2194 fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
2195 fAz =new ShaftEncoder(id6, "SE/Az", lout);
2196
2197 lout << "- Connecting devices to network." << endl;
2198
2199 //
2200 // Connect the devices to the network
2201 //
2202 SetNode(fMac1);
2203 SetNode(fMac2);
2204 if (id2>=0)
2205 SetNode(fMac3);
2206 SetNode(fZd1);
2207 SetNode(fZd2);
2208 SetNode(fAz);
2209
2210 //
2211 // Create Gui Event timer and Gui
2212 //
2213 lout << "- Initializing GUI Timer." << endl;
2214 fUpdateGui = new TTimer(this, 100); // 100ms
2215
2216 lout << "- Starting GUI." << endl;
2217 fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
2218}
2219/*
2220void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
2221{
2222 //
2223 // Create Nodes
2224 //
2225 lout << "- Setting up network." << endl;
2226
2227 fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
2228 fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
2229 fAz =new ShaftEncoder(id6, "SE/Az", lout);
2230
2231 lout << "- Connecting devices to network." << endl;
2232
2233 //
2234 // Connect the devices to the network
2235 //
2236 SetNode(fZd1);
2237 SetNode(fZd2);
2238 SetNode(fAz);
2239
2240 //
2241 // Create Gui Event timer and Gui
2242 //
2243 lout << "- Initializing GUI Timer." << endl;
2244 fUpdateGui = new TTimer(this, 100); // 100ms
2245
2246 lout << "- Starting GUI." << endl;
2247 fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
2248}
2249
2250void MCosy::ConstructorDemo()
2251{
2252 //
2253 // Create Nodes
2254 //
2255 lout << "- Setting up network." << endl;
2256
2257 //
2258 // Create Gui Event timer and Gui
2259 //
2260 lout << "- Initializing GUI Timer." << endl;
2261 fUpdateGui = new TTimer(this, 100); // 100ms
2262
2263 lout << "- Starting GUI." << endl;
2264 fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
2265}
2266*/
2267MCosy::MCosy(/*int mode,*/ const char *dev, const int baud, MLog &out)
2268: 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)
2269{
2270 TEnv env(".cosyrc");
2271 const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
2272 const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
2273 const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3
2274 const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4
2275 const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5
2276 const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6
2277/*
2278 lout << "- Program in ";
2279 switch (mode)
2280 {
2281 case 0:
2282 lout << "<<Standard mode>>" << endl;*/
2283 fBending.Load("bending.txt");
2284 Constructor(id1, id2, id3, id4, id5, id6);/*
2285 break;
2286 case 1:
2287 lout << "<<SE mode>>" << endl;
2288 fBending.Load("bending.txt");
2289 ConstructorSE(id4, id5, id6);
2290 break;
2291 default:
2292 lout << "<<Demo mode>>" << endl;
2293 ConstructorDemo();
2294 }
2295*/
2296 lout.SetOutputGui(fWin->GetLog(), kTRUE);
2297
2298 fZd1->SetDisplay(fWin->GetLabel2());
2299 fZd2->SetDisplay(fWin->GetLabel3());
2300 fAz->SetDisplay(fWin->GetLabel1());
2301
2302 //
2303 // open tpoint file
2304 //
2305 MTime time;
2306 TString name;
2307 while (1)
2308 {
2309 time.Now();
2310 name = Form("tpoint/tpoint_%s.txt", (const char*)time.GetFileName());
2311 if (gSystem->AccessPathName(name, kFileExists))
2312 break;
2313 }
2314
2315 cout << "TPoint File ********* " << name << " ********** " << endl;
2316
2317 tpout = new ofstream(name);
2318 *tpout << "Magic Model TPOINT data file" << endl;
2319 *tpout << ": ALTAZ" << endl;
2320 *tpout << "49 48 0 ";
2321 *tpout << time.Year() << " " << time.Month() << " " << time.Day() << " ";
2322 *tpout << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
2323 // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
2324
2325 fCom = new MDriveCom(this, out);
2326 fCom->Start();
2327}
2328
2329void MCosy::TerminateApp()
2330{
2331 cout << "MCosy::TerminateApp()" << endl;
2332/*
2333 Int_t rc;
2334 TGMessageBox msg(this, gClient->GetRoot(),
2335 "Information",
2336 "Cosy is shutting down the system - this may take wa while!",
2337 kMBIconExclamation,
2338 kMBOK, //kMBClose
2339 &rc, 0);
2340*/
2341
2342 lout.DisableOutputDevice(MLog::eGui);
2343 // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
2344 // lout.SetOutputGui(NULL, kFALSE);
2345
2346 gApplication->Terminate(0);
2347}
2348
2349MCosy::~MCosy()
2350{
2351 *tpout << "END" << endl;
2352 //streampos size = tpout.tellp();
2353 delete tpout;
2354
2355 cout << "Deleting GUI timer." << endl;
2356
2357 delete fUpdateGui;
2358 delete fCom;
2359
2360 cout << "Deleting Nodes." << endl;
2361
2362 delete fAz;
2363 delete fZd1;
2364 delete fZd2;
2365 delete fMac1;
2366 delete fMac2;
2367 if (fMac3)
2368 delete fMac3;
2369
2370 cout << "Deleting MGCosy." << endl;
2371
2372 lout.DisableOutputDevice(MLog::eGui);
2373
2374 delete fWin;
2375
2376 cout << "MGCosy destructed." << endl;
2377}
Note: See TracBrowser for help on using the repository browser.