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

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