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

Last change on this file since 1822 was 1819, checked in by tbretz, 22 years ago
*** empty log message ***
File size: 54.4 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 * re(se) = -28.1*cos(x*360/16384/se - 14.4) + 443.2*re/se
471 *
472 * int(re) = -1278.86*sin(0.021972656 *x/se - 14.4 )re + 443.2re x/se
473 * int(re) = -73273.4*sin(0.000383495197*x/se - .2513274)re + 443.2re x/se
474 *
475 * int (-75deg..95deg) = 3.28343e7 re
476 *
477 */
478
479 //
480 // Set velocities
481 //
482 const int vr = fMac1->GetVelRes();
483
484 int i;
485 for (i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++)
486 {
487
488 lout << "Step #" << i << endl;
489 //
490 // Get Shaft Encoder Positions
491 //
492 const ZdAz p=GetSePos();
493
494 //
495 // calculate control deviation and rounded cd
496 //
497 ZdAz rd = dest-p; // [se]
498/*
499 if (rd.Az()>0 && rd.Az()<0.5)
500 rd.Az(1);
501 if (rd.Az()<0 && rd.Az()>-0.5)
502 rd.Az(-1);
503*/
504 cout << "Deviation: " << rd.Zd() << "se Az:" << rd.Az() << "se" << endl;
505
506
507 // ===========================================
508 Float_t k1 = dest.Zd()*360/16384 - 14.3979; //[deg]
509 Float_t k0 = p.Zd()*360/16384 - 14.3979; //[deg]
510
511 //
512 // -28re/se*cos((x/deg-14.4)*pi/180) + 443.2re/se x
513 // 28re/se*2*pi/360*sin((x/deg-14.4)*pi/180)*deg + 443.2re/se*x/deg
514 //
515
516 Float_t dk = 443.152*(dest.Zd()-p.Zd());
517 Float_t dx = 28.0148/2/TMath::Pi()*(sin(k1/kRad2Deg)-sin(k0/kRad2Deg));
518
519 cout << "Using: " << rd.Zd()*kGearRatio.X() << endl; // [re]
520 cout << "Estimated1: " << 16384*dx + dk << endl;
521 cout << "Estimated2: " << (16384*dx + dk)/1.085949688 << endl;
522 // ===========================================
523
524
525 ZdAz cd = rd; // [se]
526 cd.Round();
527/*
528 if (rd.Az()>0 && rd.Az()<0.5)
529 rd.Az(0.5);
530 if (rd.Az()<0 && rd.Az()>-0.5)
531 rd.Az(-0.5);
532*/
533 //
534 // Check if there is a control deviation on the axis
535 //
536 const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
537 const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
538
539 //
540 // check if we reached the correct position already
541 //
542 if (!cdzd && !cdaz)
543 {
544 lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl;
545 SetStatus(MCosy::kStopped);
546
547 // ====================================
548 RequestRePos();
549 ZdAz after = GetRePos();
550 // ====================================
551
552 after -= before;
553
554 cout << "Drove: " << after.Zd() << " " << after.Az() << endl;
555 return TRUE;
556 }
557
558 //
559 // change units from se to re
560 //
561 rd *= kGearRatio; // [re]
562
563 //
564 // Initialize Velocities so that we reach both positions
565 // at the same time
566 //
567 if (i)
568 {
569 fMac1->SetAcceleration(0.1*vr);
570 fMac2->SetAcceleration(0.1*vr);
571
572 fMac1->SetDeceleration(0.1*vr);
573 fMac2->SetDeceleration(0.1*vr);
574
575 SetPosVelocity(1.0, 0.05);
576 }
577 else
578 {
579 if (rd.Az()>-15*kGearRatio.Y() && rd.Az()<15*kGearRatio.Y())
580 {
581 cout << " -------------- LO ---------------- " << endl;
582 fMac1->SetAcceleration(0.05*vr);
583 fMac1->SetDeceleration(0.05*vr);
584 }
585 else
586 {
587 cout << " -------------- HI ---------------- " << endl;
588 fMac1->SetAcceleration(0.2*vr);
589 fMac1->SetDeceleration(0.1*vr);
590 }
591
592 fMac2->SetAcceleration(0.2*vr);
593 fMac2->SetDeceleration(0.1*vr);
594
595 // vel(0.6) acc(0.5)
596 SetPosVelocity(fabs(rd.Ratio()), 0.175);
597 // SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3);
598 }
599
600 rd.Round();
601
602 // FIXME? Check for Error or Zombie?
603
604 /*
605 cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
606 cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
607 cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
608 cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
609 cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
610 */
611
612 //
613 // repositioning (relative)
614 //
615 lout << "Do Relative Positioning Done" << endl;
616 DoRelPos(rd, cdzd, cdaz);
617
618 lout << "Relative Positioning Done" << endl;
619 }
620
621 if (i<10)
622 StopMovement();
623
624 lout << "Warning: Requested position not reached (i=" << i << ")" << endl;
625 return FALSE;
626}
627
628// --------------------------------------------------------------------------
629//
630// Sets the tracking velocity
631//
632// The velocities are given in a ZdAz object in re/min. Return kTRUE
633// in case of success, kFALSE in case of failure.
634//
635Bool_t MCosy::SetVelocity(const ZdAz &v)
636{
637 //
638 // Send the new velocities for both axes.
639 //
640 fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min]
641 fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min]
642
643 //
644 // Wait for the objects to be OKed.
645 //
646
647 fMac2->WaitForSdo(0x3006, 1);
648 fMac1->WaitForSdo(0x3006, 1);
649
650 // FIXME, what when waiting times out (Zombie)
651 // WaitForSdos();
652
653 //
654 // If the waiting for the objects wasn't interrupted return kTRUE
655 //
656 if (!(Break() || HasError()) && !HasZombie())
657 return kTRUE;
658
659 //
660 // print a message if the interruption was due to a Can-node Error
661 //
662 if (HasError())
663 lout << "Error while setting velocity (SDO #3006)" << endl;
664
665 return kFALSE;
666}
667
668// --------------------------------------------------------------------------
669//
670// Initializes Tracking mode
671//
672// Initializes the accelerations of both axes with 90% of the maximum
673// acceleration. Set the status for moving and tracking and starts thr
674// revolution mode.
675//
676bool MCosy::InitTracking()
677{
678 // FIXME? Handling of Zombie OK?
679 if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
680 return false;
681
682 //
683 // Start revolution mode
684 //
685 fMac2->SetAcceleration(0.2*fMac2->GetVelRes());
686 fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
687 if (fMac2->IsZombieNode())
688 return false;
689
690 fMac1->SetAcceleration(0.2*fMac1->GetVelRes());
691 fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
692 if (fMac1->IsZombieNode())
693 return false;
694
695 SetStatus(MCosy::kMoving | MCosy::kTracking);
696
697 fMac2->SetRpmMode(TRUE);
698 if (fMac2->IsZombieNode())
699 return false;
700
701 fMac1->SetRpmMode(TRUE);
702 if (fMac1->IsZombieNode())
703 return false;
704
705 return true;
706}
707
708// --------------------------------------------------------------------------
709//
710// Limits the speed.
711//
712// This function should work as a limiter. If a tracking error is too large
713// to be corrected fast enough we would get enormous velocities. These
714// velocities are limited to the maximum velocity.
715//
716void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
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
724 ULong_t vrzd = fMac1->GetVelRes();
725 ULong_t vraz = fMac2->GetVelRes();
726
727#define sgn(x) (x<0?-1:1)
728
729 const Float_t limit = 0.25;
730/*
731 if (sgn(vt->Az()) != sgn(vcalc.Az()))
732 vt->Az(0);
733// else
734 {
735 if (fabs(vt->Az()) < fabs(vcalc.Az()) *0.5)
736 vt->Az(0.5*vcalc.Az());
737
738 if (fabs(vt->Az()) > fabs(vcalc.Az()) *1.5)
739 vt->Az(1.5*vcalc.Az());
740 }
741
742 if (sgn(vt->Zd()) != sgn(vcalc.Zd()))
743 vt->Zd(0);
744// else
745 {
746 if (fabs(vt->Zd()) > fabs(vcalc.Az()) *1.5)
747 vt->Zd(1.5*vcalc.Zd());
748
749 if (fabs(vt->Zd()) < fabs(vcalc.Az()) *0.5)
750 vt->Zd(0.5*vcalc.Zd());
751 }
752 */
753
754 if (sgn(vt->Az()) != sgn(vcalc.Az())
755 && fabs(vt->Az()) < limit*fabs(vcalc.Az())
756 )
757 vt->Az(0);
758 else
759 if (fabs(vt->Az()) > 0.9*vraz)
760 {
761 lout << "Warning: Azimuth speed limit exceeded. Limiting speed to 90% max." << endl;
762 vt->Az(0.9*vraz*sgn(vt->Az()));
763 }
764
765 if (sgn(vt->Zd()) != sgn(vcalc.Zd())
766 && fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
767 )
768 vt->Zd(0);
769 else
770 if (fabs(vt->Zd()) > 0.9*vrzd)
771 {
772 lout << "Warning: Altitude speed limit exceeded. Limiting speed to 90% max." << endl;
773 vt->Zd(0.9*vrzd*sgn(vt->Zd()));
774 }
775}
776
777void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
778{
779 lout << "Track Position: " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg << "deg (Ra/Dec)" << endl;
780
781 SlaStars sla(fObservatory);
782
783 //
784 // Position to actual position
785 //
786 sla.Now();
787 ZdAz dest = sla.CalcZdAz(dst);
788
789 if (dest.Az()<-TMath::Pi()/2)
790 dest.Az(dest.Az() + TMath::Pi()*2);
791
792 if (dest.Az()>3*TMath::Pi()/2)
793 dest.Az(dest.Az() -TMath::Pi()*2);
794
795 if (!SetPosition(dest, kTRUE))
796 {
797 lout << "Error: Cannot start tracking, positioning failed." << endl;
798 return;
799 }
800
801 //
802 // calculate offset from present se position
803 //
804 const ZdAz sepos = GetSePos()*kGearRatio;
805
806 if (!RequestRePos())
807 return;
808
809 //
810 // Estimate Offset before starting to track
811 //
812 fOffset = sepos-GetRePos();
813
814 /*
815 cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
816 cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
817 cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
818 */
819
820 //
821 // Init accelerations and Rpm Mode
822 //
823 if (!InitTracking())
824 return;
825
826 XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
827
828 lout << "Start tracking:";
829 lout << " Ra: " << xy.X() << "h " << "Dec: " << xy.Y() << "\xb0" << endl;
830
831 ofstream fout("coordinates.txt");
832 fout << xy;
833 fout.close();
834
835 //
836 // Initialize Tracker (slalib or starguider)
837 //
838 fRaDec = dst;
839 fBackground = kBgdTracking;
840
841//--- ofstream fout("log/cosy.pos");
842//--- fout << "Tracking:";
843//--- fout << " Ra: " << Rad2Deg(dst.Ra()) << "\x9c ";
844//--- fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
845//--- fout << " Mjd/10ms V/re/min/4" << endl;
846
847 //
848 // We want to reach the theoretical position exactly in about 0.5s
849 //
850 // *OLD*const float dt = 1; // 1 second
851 const float dt = 3; // 2 second
852 while (!(Break() || HasError()) && !HasZombie())
853 {
854 //
855 // Request Target position for this moment
856 //
857 sla.Now();
858
859 //
860 // Request theoretical Position for a time in the future (To+dt) from CPU
861 //
862 sla.SetMjd(sla.GetMjd()+dt/(60*60*24));
863 ZdAz dummy1 = sla.CalcZdAz(fRaDec);
864
865 if (dummy1.Az()<-TMath::Pi()/2)
866 dummy1.Az(dummy1.Az() + TMath::Pi()*2);
867 if (dummy1.Az()>3*TMath::Pi()/2)
868 dummy1.Az(dummy1.Az() -TMath::Pi()*2);
869
870 ZdAz dummy = fBending(dummy1);
871 dest = CorrectTarget(GetSePos(), dummy); // [se]
872
873 const ZdAz d = dest*360./16384; // [deg]
874 dest *= kGearRatio; // [re]
875
876 ZdAz min = fBending(fMin/kRad2Deg)*kRad2Deg;
877 ZdAz max = fBending(fMax/kRad2Deg)*kRad2Deg;
878
879 /*
880 if (d.Zd()<min.Zd() || d.Az()<min.Az())
881 {
882 lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
883 lout << "< " << d.Zd() << " " << min.Zd() << " " << d.Az() << " " << min.Az() << endl;
884 break;
885 }
886 if (d.Zd()>max.Zd() || d.Az()>max.Az())
887 {
888 lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
889 lout << "> " << d.Zd() << " " << max.Zd() << " " << d.Az() << " " << max.Az() << endl;
890 break;
891 }
892 */
893 ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min]
894
895 //
896 // Request absolute position of rotary encoder from Macs
897 //
898 if (!RequestRePos())
899 break;
900
901 //
902 // distance between (To+dt) and To [re]
903 // position time difference < 5usec
904 // fOffset does the synchronization between the
905 // Shaft- and the rotary encoders
906 //
907 dest -= GetRePos() + fOffset;
908
909 //
910 // Velocity to go [re/min] to reach the right position at time t+dt
911 // correct for the duration of RaDec2AltAz
912 //
913 const ZdAz v = dest*60.0/(dt/*-(fMac2->GetTime()-sla)*/);
914
915 //
916 // calculate real velocity of future [re/min]
917 // believing the Macs manual '/4' shouldn't be necessary, but it is.
918 //
919 ZdAz vt = v/4;
920 LimitSpeed(&vt, vcalc);
921 vt.Round();
922
923 //
924 // check if the drive is fast enough to follow the star
925 //
926 if (vt.Zd()>.9*fMac1->GetVelRes() || vt.Az()>.9*fMac2->GetVelRes())
927 {
928 lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
929 break;
930 }
931
932 //
933 // Set theoretical velocity (as early after calculation as possible)
934 // Maybe we should attenuate the changes
935 //
936 if (!SetVelocity(vt))
937 break;
938
939 //
940 // Now do 'unnecessary' things
941 //
942 fVelocity = vt/kGearRatio2*4;
943
944//--- const double mjd = fMac2->GetMjd();
945//--- fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " ";
946//--- fout << setw(4) << vt.Zd() << " ";
947//--- fout << setw(4) << vt.Az() << endl;
948 //
949 // FIXME? Calculate an accuracy for the tracking system?
950 // How good do we reach the calculated position in 'real'
951 // re valus?
952 //
953
954
955 //
956 // Update speed as often as possible.
957 // make sure, that dt is around 10 times larger than the
958 // update time
959 //
960 //
961 // The loop should not be executed faster than the ramp of
962 // a change in the velocity can be followed.
963 // (This is important on fast machines >500MHz)
964 //
965 usleep(50000); // 0.25s
966 //usleep(50000); // 0.05s
967 }
968
969 fBackground = kBgdNone;
970 StopMovement();
971 lout << "Tracking stopped." << endl;
972}
973
974// --------------------------------------------------------------------------
975//
976// Stops the movement of both motors.
977//
978// Sets the status to stopping. Sets the deceleration to 50% of the maximum.
979// stops. Quits the revolution mode and wait for the end of the movement.
980//
981void MCosy::StopMovement()
982{
983 //
984 // Set status to Stopping
985 //
986 SetStatus(MCosy::kStopping);
987
988 //
989 // set deceleration to 50%
990 //
991 cout << "Stopping movement (dec=20%)..." << endl;
992
993 fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
994 fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
995 fMac1->SetRpmMode(FALSE);
996 fMac2->SetRpmMode(FALSE);
997
998/*
999 fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
1000 fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
1001
1002 fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
1003 fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
1004 fMac2->WaitForSdo(0x3000, 0);
1005 fMac1->WaitForSdo(0x3000, 0);
1006 fMac1->SetRpmMode(FALSE);
1007 fMac2->SetRpmMode(FALSE);
1008 */
1009
1010 //
1011 // Wait for the movement to really be finished.
1012 //
1013 cout << "Waiting for end of movement..." << endl;
1014 WaitForEndMovement();
1015
1016 //
1017 // Check whether everything works fine.
1018 //
1019 CheckForError();
1020 cout << "Movement stopped." << endl;
1021}
1022
1023void MCosy::StopTracking()
1024{
1025 //
1026 // Set status to Stopping
1027 //
1028 SetStatus(MCosy::kStopping);
1029
1030 //
1031 // set deceleration to 50%
1032 //
1033 cout << "Stopping tracking (dec=20%)..." << endl;
1034 fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
1035 fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
1036
1037 fMac2->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
1038 fMac1->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min]
1039 fMac2->WaitForSdo(0x3006, 1);
1040 fMac1->WaitForSdo(0x3006, 1);
1041
1042 cout << "Waiting for end of movement..." << endl;
1043 WaitForEndMovement();
1044
1045 //
1046 // Wait for the objects to be OKed.
1047 //
1048 fMac1->SetRpmMode(FALSE);
1049 fMac2->SetRpmMode(FALSE);
1050
1051 //
1052 // Wait for the movement to really be finished.
1053 //
1054 //cout << "Waiting for end of movement..." << endl;
1055 //WaitForEndMovement();
1056
1057 //
1058 // Check whether everything works fine.
1059 //
1060 CheckForError();
1061 cout << "Movement stopped." << endl;
1062}
1063
1064bool MCosy::CheckNetwork()
1065{
1066 //return kTRUE;
1067 //CheckConnections();
1068
1069 if (HasZombie())
1070 {
1071 lout << "- Found Zombies in Network..." << endl;
1072 if (!RebootZombies())
1073 return false;
1074 }
1075
1076 /*
1077 FIXME HANDLING ERROR
1078 */
1079 if (HasError())
1080 {
1081 fMac1->HandleError();
1082 fMac2->HandleError();
1083 fMac3->HandleError();
1084 if (HasError() || HasZombie())
1085 return false;
1086 }
1087
1088 return true;
1089}
1090
1091void *MCosy::Proc(int msg, void *mp)
1092{
1093 switch (msg)
1094 {
1095 case WM_WAIT:
1096 cout << "Wait for execution of Proc(WM_*, ): done." << endl;
1097 return NULL;
1098
1099 case WM_STOP:
1100 cout << "MCosy::Proc: Stop." << endl;
1101 if (!CheckNetwork())
1102 return (void*)0xebb0;
1103 StopMovement();
1104 return NULL;
1105
1106 case WM_PRESET:
1107 cout << "WM_Preset: start." << endl;
1108 if (!CheckNetwork())
1109 return (void*)0xebb0;
1110 fZd1->SetPreset();
1111 fZd2->SetPreset();
1112 fAz->SetPreset();
1113 cout << "WM_Preset: done. (return 0xaffe)" << endl;
1114 return (void*)0xaffe;
1115
1116 case WM_CALIB:
1117 {
1118 cout << "WM_Calib: start." << endl;
1119 if (!CheckNetwork())
1120 return (void*)0xebb0;
1121
1122 SlaStars sla(fObservatory);
1123 sla.Now();
1124
1125 RaDec rd = *((RaDec*)mp);
1126
1127 //RaDec rd(37.94, 89.2644); // POLARIS
1128 //RaDec rd(213.915417, 19.1825); // ACTURUS
1129
1130 cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
1131
1132 ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;
1133
1134 cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
1135
1136 ZdAz sepos = GetSePos();
1137 cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
1138
1139 fZd1->SetPreset(za.Zd());
1140 fZd2->SetPreset(-za.Zd());
1141 fAz->SetPreset(za.Az());
1142
1143 cout << "WM_Calib: done. (return 0xaffe)" << endl;
1144 }
1145 return (void*)0xaffe;
1146
1147 case WM_TPOINT:
1148 {
1149 cout << "WM_TPoint: start." << endl;
1150 SlaStars sla(fObservatory);
1151 sla.Now();
1152
1153 RaDec rd = *((RaDec*)mp);
1154 cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
1155
1156 AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;
1157
1158 cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
1159 *tpout << setprecision(7) << za.Az() << " " << za.Alt() << " ";
1160
1161 ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;;
1162 za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
1163 za *= kRad2Deg;
1164
1165 cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
1166 *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";
1167 *tpout << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;
1168
1169 cout << "WM_TPoint: done. (return 0xaffe)" << endl;
1170 }
1171 return (void*)0xca1b;
1172
1173 case WM_POSITION:
1174 cout << "WM_Position: start." << endl;
1175 {
1176 if (!CheckNetwork())
1177 return (void*)0xebb0;
1178
1179 ZdAz dest = *((ZdAz*)mp);
1180 SetPosition(dest*kDeg2Rad);
1181 }
1182 cout << "WM_Position: done. (return 0x7777)" << endl;
1183 return (void*)0x7777;
1184
1185 case WM_TESTSE:
1186 cout << "WM_TestSe: start." << endl;
1187 fBackground = mp ? kBgdSeTest : kBgdNone;
1188 cout << "WM_TestSe: done. (return 0x1e51)" << endl;
1189 return (void*)0x1e51;
1190
1191 case WM_GEAR:
1192 cout << "WM_Gear: start." << endl;
1193 fBackground = mp ? kBgdGear : kBgdNone;
1194 cout << "WM_Gear: done. (return 0xfeaf)" << endl;
1195 return (void*)0xfeaf;
1196
1197 case WM_DISPLAY:
1198 cout << "WM_Display: start." << endl;
1199 fTriggerDisplay = kTRUE;
1200 cout << "WM_Disply: done. (return 0xd1e1)" << endl;
1201 return (void*)0xd1e1;
1202
1203 case WM_TRACK:
1204 cout << "WM_Track: START" << endl;
1205 {
1206 if (!CheckNetwork())
1207 return (void*)0xebb0;
1208
1209 RaDec dest = *((RaDec*)mp);
1210 TrackPosition(dest*kDeg2Rad);
1211 }
1212 cout << "WM_Track: done. (return 0x8888)" << endl;
1213 return (void*)0x8888;
1214
1215 case WM_NEWTRACK:
1216 cout << "WM_NewTrack: START" << endl;
1217 fRaDec = *((RaDec*)mp);
1218 cout << "WM_NewTrack: done. (return 0x9999)" << endl;
1219 return (void*)0x9999;
1220
1221 case WM_LOADBENDING:
1222 cout << "WM_LoadBending: START" << endl;
1223 fBending.Load("bending.txt");
1224 cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
1225 return (void*)0xbe0d;
1226
1227 case WM_RESETBENDING:
1228 cout << "WM_ResetBending: START" << endl;
1229 fBending.Reset();
1230 cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
1231 return (void*)0xbe0e;
1232
1233 case WM_HOME:
1234 cout << "WM_Home: START" << endl;
1235 if (!CheckNetwork())
1236 return (void*)0xebb0;
1237 else
1238 {
1239 cout << "HOME NOT ALLOWED... for Magic." << endl;
1240 /*
1241 cout << "Going Home..." << endl;
1242 TEnv env(".cosyrc");
1243
1244 SetStatus(MCosy::kMoving);
1245
1246 fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
1247 fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
1248
1249 lout << "SETHOME DONE" << endl;
1250
1251 SetStatus(HasError() ? MCosy::kError : MCosy::kStopped);
1252
1253 fAz->SetPreset();
1254 fZd1->SetPreset();
1255 fZd2->SetPreset();
1256
1257 fMac1->ReqPos();
1258 fMac2->ReqPos();
1259 fMac3->StopMotor();
1260 */
1261 }
1262 cout << "WM_Home: done. (return 0x403e)" << endl;
1263 return (void*)0x403e;
1264
1265 case WM_CALCALTAZ:
1266 {
1267 cout << endl;
1268
1269 SlaStars sla(fObservatory);
1270 sla.Now();
1271
1272 XY xy = *((XY*)mp);
1273 RaDec rd(xy.X()*15., xy.Y()); // [deg]
1274
1275 ZdAz a0 = sla.CalcZdAz(rd*kDeg2Rad);
1276 ZdAz a1 = fBending(a0);
1277 ZdAz se = CorrectTarget(GetSePos(), a1);
1278 a0 *= kRad2Deg;
1279 a1 *= kRad2Deg;
1280 ZdAz a2 = a1*16384/360;
1281 cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
1282 cout << "Zd/Az source: " << a0.Zd() << "° " << a0.Az() << "°" << endl;
1283 cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
1284 cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
1285 cout << "SE target: " << se.Zd() << " " << se.Az() << endl;
1286 }
1287 return (void*)0xa17a;
1288
1289 case WM_QUIT:
1290 cout << "WM_Quit: now." << endl;
1291 if (!CheckNetwork())
1292 {
1293 lout << "ERROR: Cannot shutdown CANbus network." << endl;
1294 return (void*)0xebb0;
1295 }
1296 TerminateApp();
1297 cout << "WM_Quit: done." << endl;
1298 return (void*)0xaaaa;
1299 }
1300 cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
1301 return (void*)0xffffffff;
1302}
1303
1304void *MTTalk::Thread()
1305{
1306 fCosy->TalkThread();
1307 return NULL;
1308}
1309
1310void MCosy::ReadConfig()
1311{
1312 cout << "Reading configuration file..." << flush;
1313 TEnv env(".cosyrc");
1314 cout << "done." << endl;
1315
1316 cout << "Reading telescope range..." << flush;
1317 const Double_t amin = env.GetValue("Az_Min[deg]", -90.0);
1318 const Double_t zmin = env.GetValue("Zd_Min[deg]", -74.5);
1319 fMin.Set(zmin, amin);
1320
1321 cout << " Min: " << zmin << "deg " << amin << "deg" << endl;
1322
1323 const Double_t amax = env.GetValue("Az_Max[deg]", 318.0);
1324 const Double_t zmax = env.GetValue("Zd_Max[deg]", 100.5);
1325 fMax.Set(zmax, amax);
1326
1327 cout << " Max: " << zmax << "deg " << amax << "deg" << endl;
1328
1329 cout << "Reading gear ratios..." << flush;
1330 const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0);
1331 const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0);
1332
1333 Double_t resreaz = 0;
1334 if (fMac1 && !fMac1->IsZombieNode())
1335 resreaz = fMac1->GetRes();
1336 else
1337 if (fMac3 && !fMac3->IsZombieNode())
1338 resreaz = fMac3->GetRes();
1339 else
1340 resreaz = env.GetValue("Az_ResRE[re/U_mot]", 1500);
1341
1342 Double_t resrezd = 0;
1343 if (fMac2 && !fMac2->IsZombieNode())
1344 resrezd = fMac2->GetRes();
1345 else
1346 resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500);
1347
1348 Double_t ressezd = 0;
1349 if (fZd1 && !fZd1->IsZombieNode())
1350 ressezd = fZd1->GetPhysRes();
1351 else
1352 if (fZd2 && !fZd2->IsZombieNode())
1353 ressezd = fZd2->GetPhysRes();
1354 else
1355 ressezd = env.GetValue("Zd_ResSE[se/U_mot]", 16384);
1356
1357 Double_t resseaz = 0;
1358 if (fAz && !fAz->IsZombieNode())
1359 resseaz = fAz->GetPhysRes();
1360 else
1361 resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384);
1362
1363 kGearRatio.Set (gzd*resrezd*4/ressezd, gaz*resreaz*4/resseaz); //[re/se]
1364 kGearRatio2.Set(gzd*resrezd*4/360.0, gaz*resreaz*4/360.0); //[re/deg]
1365 cout << "done." << endl;
1366
1367 cout << " * Setting Gear Ratios:" << endl;
1368 cout << " --------------------" << endl;
1369 cout << " * X: " << gzd << "*" << resrezd << "/" << ressezd << "=" << kGearRatio.X() << endl;
1370 cout << " * Y: " << gaz << "*" << resreaz << "/" << resseaz << "=" << kGearRatio.Y() << endl;
1371}
1372
1373void MCosy::InitSync()
1374{
1375 if (!fMac3)
1376 {
1377 lout << "Unable to Init Sync! Mac3 not available." << endl;
1378 return;
1379 }
1380
1381 const int res = fMac3->GetVelRes();
1382
1383 fMac3->SetVelocity(0.3*res);
1384 fMac3->SetAcceleration(0.2*res);
1385 fMac3->SetDeceleration(0.2*res);
1386 fMac3->StartPosSync();
1387}
1388
1389void MCosy::TalkThreadTracking()
1390{
1391 if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
1392 return;
1393
1394 if (fAz->IsZombieNode())
1395 return;
1396
1397 if (!fMac1 || !fMac2)
1398 return;
1399
1400 lout << "Tracking Thread started..." << endl;
1401
1402 SlaStars sla(fObservatory);
1403
1404 ZdAz old;
1405 ZdAz ist;
1406
1407 ZdAz sollzd;
1408 ZdAz sollaz;
1409
1410 ZdAz istre = -fOffset; // [re]
1411 ZdAz time;
1412
1413 //
1414 // only update fTrackingError while tracking
1415 //
1416 bool phca1=false;
1417 bool phca2=false;
1418 bool phcaz=false;
1419
1420 while (fBackground==kBgdTracking)
1421 {
1422 //
1423 // Make changes (eg wind) smoother - attenuation of control function
1424 //
1425 const float weight = 1.; //0.3;
1426
1427 //
1428 // This is the time constant which defines how fast
1429 // you correct for external influences (like wind)
1430 //
1431 fZd1->ResetPosHasChanged();
1432 fZd2->ResetPosHasChanged();
1433 fAz->ResetPosHasChanged();
1434 do
1435 {
1436 phca1 = fZd1->PosHasChanged();
1437 phca2 = fZd2->PosHasChanged();
1438 phcaz = fAz->PosHasChanged();
1439 usleep(1);
1440 } while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking);
1441
1442 //---usleep(100000); // 0.1s
1443
1444 //
1445 // get position, where we are
1446 //
1447 old = ist;
1448 ist = GetSePos(); // [se]
1449
1450 //
1451 // if the position didn't change continue
1452 //
1453 /*---
1454 if ((int)ist.Zd() == (int)old.Zd() &&
1455 (int)ist.Az() == (int)old.Az())
1456 continue;
1457 */
1458 istre = GetRePosPdo();
1459
1460 //
1461 // Get time from last shaftencoder position change (position: ist)
1462 // FIXME: I cannot take the avarage
1463 //
1464 // FIXME
1465 //time.Zd(fZd1->GetMjd());
1466 /* OLD* */
1467 if (fZd1->GetMjd()>fZd2->GetMjd())
1468 time.Zd(fZd1->GetMjd());
1469 else
1470 time.Zd(fZd2->GetMjd());
1471
1472 //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
1473 time.Az(fAz->GetMjd());
1474
1475 //
1476 // if Shaftencoder changed position
1477 // calculate were we should be
1478 //
1479 if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/)
1480 {
1481 sla.SetMjd(time.Zd());
1482
1483 ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
1484 sollzd = CorrectTarget(ist, dummy); // [se]
1485
1486 fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight);
1487 }
1488
1489 if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
1490 {
1491 sla.SetMjd(time.Az());
1492
1493 ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
1494 sollaz = CorrectTarget(ist, dummy); // [se]
1495
1496 fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
1497 }
1498
1499 ZdAz soll(sollzd.Zd(), sollaz.Az()); // [se]
1500 fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384);
1501
1502 fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
1503 (ist.Az()-sollaz.Az())*kGearRatio.Y());
1504
1505 //--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
1506 //--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " ";
1507 //--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " ";
1508 //--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl;
1509 }
1510
1511 lout << "Tracking Thread done." << endl;
1512
1513 //--- fout << endl << endl;
1514}
1515
1516void MCosy::TalkThreadSeTest()
1517{
1518// if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
1519 // return;
1520
1521 if (fHist)
1522 {
1523 lout << "You are much too fast... try again." << endl;
1524 return;
1525 }
1526
1527 fHist = new TH2F("Diff", "Difference of SE values",
1528 201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
1529 fHist->SetXTitle("ZA [\\circ]");
1530 fHist->SetYTitle("\\Delta SE");
1531
1532 Double_t offset = 0;
1533
1534 int cnt = 0;
1535
1536 lout << "Starting Shaftencoder Test..." << endl;
1537
1538 while (fBackground==kBgdSeTest)
1539 {
1540 fZd1->ResetPosHasChanged();
1541 fZd2->ResetPosHasChanged();
1542
1543 while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
1544 fBackground==kBgdSeTest)
1545 usleep(1);
1546
1547 const Double_t pos[3] = {
1548 (fZd1->GetPos()+8192)%16384,
1549 (fZd2->GetPos()+8192)%16384,
1550 fAz->GetPos() };
1551
1552 //
1553 // Estimate Offset from the first ten positions
1554 //
1555 if (cnt++<10)
1556 {
1557 offset += pos[0]+pos[1];
1558 continue;
1559 }
1560 if (cnt==11)
1561 {
1562 offset /= 10;
1563 cnt++;
1564 }
1565
1566 Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
1567
1568 ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
1569 fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
1570 }
1571
1572 lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;
1573
1574 fBackground=kBgdSeTestDispl;
1575}
1576
1577void MCosy::TalkThreadGear()
1578{
1579// if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
1580 // return;
1581
1582 if (fHist)
1583 {
1584 lout << "You are much too fast... try again." << endl;
1585 return;
1586 }
1587
1588 // fHist = new TH2F("Gear", "Gear Ratio Re/Se",
1589 // 201, fMin.Zd(), fMax.Zd(), 61, 349.5, 500.5);
1590 fHist = new TH2F("Gear", "Gear Ratio Re/Se",
1591 201, fMin.Az(), fMax.Az(), 61, 419.5, 570.5);
1592 fHist->SetXTitle("ZA [\\circ]");
1593 fHist->SetYTitle("Re/Se");
1594
1595 lout << "Starting Gear determination..." << endl;
1596
1597 ZdAz se0 = GetSePos();
1598 ZdAz re0 = GetRePosPdo();
1599
1600 while (fBackground==kBgdGear)
1601 {
1602 fZd1->ResetPosHasChanged();
1603 fZd2->ResetPosHasChanged();
1604 fAz->ResetPosHasChanged();
1605
1606 while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
1607 !fAz->PosHasChanged() && fBackground==kBgdGear)
1608 usleep(1);
1609
1610 ZdAz se = GetSePos();
1611 ZdAz re = GetRePosPdo();
1612
1613 ZdAz dse = se-se0;
1614 ZdAz dre = re-re0;
1615
1616 if (fabs(dse.Zd())*144>16384) // Each 2.5deg
1617 {
1618 se0.Zd(se.Zd());
1619 re0.Zd(re.Zd());
1620
1621 ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
1622 fHist->Fill(bend.Zd(), dre.Zd()/dse.Zd());
1623 }
1624
1625 if (fabs(dse.Az())*144>16384) // Each 2.5deg
1626 {
1627 se0.Az(se.Az());
1628 re0.Az(re.Az());
1629
1630 ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
1631 fHist->Fill(bend.Az(), dre.Az()/dse.Az());
1632
1633 cout << bend.Az() << ": " << dre.Az()/dse.Az() << endl;
1634 }
1635
1636 /*
1637 const Double_t pos[3] = {
1638 (fZd1->GetPos()+8192)%16384,
1639 (fZd2->GetPos()+8192)%16384,
1640 fAz->GetPos() };
1641
1642 //
1643 // Estimate Offset from the first ten positions
1644 //
1645 if (cnt++<10)
1646 {
1647 offset += pos[0]+pos[1];
1648 continue;
1649 }
1650 if (cnt==11)
1651 {
1652 offset /= 10;
1653 cnt++;
1654 }
1655
1656 Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
1657
1658 ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
1659 fHistTestSe->Fill(bend.Zd(), pos[0]+pos[1]-offset);
1660*/
1661 }
1662 lout << "Gear Test Stopped... displaying Histogram." << endl;
1663
1664 fBackground=kBgdGearDispl;
1665}
1666
1667void MCosy::TalkThread()
1668{
1669 /* ========== FIXME? =============
1670 if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
1671 return;
1672 */
1673
1674 if (fMac1 && fMac2)
1675 {
1676 fMac1->ReqPos();
1677 fMac2->ReqPos();
1678 }
1679
1680 InitSync();
1681
1682 /*** FOR DEMO MODE ***/
1683 if (!fZd1 || !fZd2 || !fAz)
1684 return;
1685 /*** FOR DEMO MODE ***/
1686
1687 //
1688 // Start the Network
1689 //
1690 while (1)
1691 {
1692 //
1693 // wait until a tracking session is started
1694 //
1695 while (fBackground==kBgdNone)
1696 usleep(1);
1697
1698 switch (fBackground)
1699 {
1700 case kBgdNone:
1701 continue;
1702
1703 case kBgdTracking:
1704 TalkThreadTracking();
1705 continue;
1706
1707 case kBgdSeTest:
1708 TalkThreadSeTest();
1709 continue;
1710
1711 case kBgdGear:
1712 TalkThreadGear();
1713 continue;
1714
1715 default:
1716 continue;
1717 }
1718 }
1719}
1720
1721Bool_t MCosy::HandleTimer(TTimer *t)
1722{
1723 //
1724 // Update Gui, foremer MTGui.
1725 //
1726 if (fZd1)
1727 fZd1->DisplayVal();
1728 if (fZd2)
1729 fZd2->DisplayVal();
1730 if (fAz)
1731 fAz->DisplayVal();
1732
1733 ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
1734 ZdAz bendist = fBending.CorrectBack(seist);
1735
1736 Byte_t avail = 0;
1737
1738 avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
1739 avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
1740 avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
1741 avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0;
1742 avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0;
1743 avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0;
1744
1745 if (HasError())
1746 SetStatus(MCosy::kError);
1747
1748 lout.UpdateGui();
1749
1750 fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError/kGearRatio2,
1751 fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail);
1752
1753 /*
1754 cout << (int)(fMac1->GetStatus()&Macs::kOutOfControl) << " ";
1755 cout << (int)(fMac2->GetStatus()&Macs::kOutOfControl) << " ";
1756 cout << (int)(fMac3->GetStatus()&Macs::kOutOfControl) << endl;
1757 */
1758
1759 const Bool_t trigger = fTriggerDisplay;
1760 fTriggerDisplay = kFALSE;
1761
1762 if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
1763 DisplayHistTestSe(!trigger);
1764
1765 if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
1766 DisplayHistGear(!trigger);
1767
1768 return kTRUE;
1769}
1770
1771void MCosy::DisplayHistTestSe(Bool_t del)
1772{
1773 lout << "Displaying histogram..." << endl;
1774
1775 TH2F &hist = *fHist;
1776
1777 if (del)
1778 {
1779 fHist = NULL;
1780 fBackground = kBgdNone;
1781 }
1782
1783 TCanvas *c=new TCanvas("c1", "", 1000, 1000);
1784 c->Divide(1,2);
1785
1786 c->cd(1);
1787 TH2 *h=(TH2*)hist.DrawCopy();
1788
1789 TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
1790 p->SetLineColor(kBlue);
1791 p->Draw("same");
1792 p->SetBit(kCanDelete);
1793
1794 c->cd(2);
1795
1796 TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
1797 hist.GetBinLowEdge(hist.GetNbinsX()+1));
1798 p2.SetXTitle("ZA [\\circ]");
1799 for (int i=0; i<hist.GetNbinsX(); i++)
1800 p2.SetBinError(i, p->GetBinError(i));
1801 p2.SetLineColor(kRed);
1802 p2.SetStats(0);
1803 p2.DrawCopy();
1804
1805 if (del)
1806 delete &hist;
1807}
1808
1809void MCosy::DisplayHistGear(Bool_t del)
1810{
1811 lout << "Displaying histogram..." << endl;
1812
1813 TH2F &hist = *fHist;
1814
1815 if (del)
1816 {
1817 fHist = NULL;
1818 fBackground = kBgdNone;
1819 }
1820
1821 TCanvas *c=new TCanvas("c1", "", 1000, 1000);
1822 c->Divide(1,2);
1823
1824 c->cd(1);
1825 TH2 *h=(TH2*)hist.DrawCopy();
1826
1827 TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
1828 p->SetLineColor(kBlue);
1829 p->Draw("same");
1830 p->SetBit(kCanDelete);
1831
1832 c->cd(2);
1833
1834 TH1F p2("spread", "Spread of the gear [16384/1500/4*U/U]", hist.GetNbinsX(), hist.GetBinLowEdge(1),
1835 hist.GetBinLowEdge(hist.GetNbinsX()+1));
1836 p2.SetXTitle("ZA [\\circ]");
1837 for (int i=0; i<hist.GetNbinsX(); i++)
1838 p2.SetBinError(i, p->GetBinError(i));
1839 p2.SetLineColor(kRed);
1840 p2.SetStats(0);
1841 p2.DrawCopy();
1842
1843 if (del)
1844 delete &hist;
1845}
1846
1847// --------------------------------------------------------------------------
1848//
1849// Start the work of the application:
1850//
1851// Start the Can-Network.
1852// Start the MCosy::TalkThread thread.
1853// turn on the gui update
1854//
1855void MCosy::Start()
1856{
1857 // Don't call this function twice!
1858 Network::Start();
1859
1860 ReadConfig();
1861
1862 lout << "- Starting TX Thread." << endl;
1863 fTTalk = new MTTalk(this);
1864
1865 lout << "- Starting GUI update." << endl;
1866 fUpdateGui->TurnOn();
1867}
1868
1869// --------------------------------------------------------------------------
1870//
1871// Start the work of the application:
1872//
1873// Turn of the gui update
1874// stop the MCosy::TalkThread thread.
1875// Stop the network
1876//
1877void MCosy::Stop()
1878{
1879 lout << "- Stopping GUI update." << endl;
1880 fUpdateGui->TurnOff();
1881 lout << "- GUI Update stopped." << endl;
1882
1883 delete fTTalk;
1884 lout << "- TX Thread stopped." << endl;
1885
1886 Network::Stop();
1887}
1888
1889// --------------------------------------------------------------------------
1890//
1891// Disable the synchronization by using a negative CAN Id for id2.
1892//
1893void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
1894 Int_t id4, Int_t id5, Int_t id6)
1895{
1896 //
1897 // Create Nodes
1898 //
1899 lout << "- Setting up network." << endl;
1900
1901 fMac1=new Macs(id1, "Mac/Az", lout);
1902 fMac2=new Macs(id3, "Mac/Zd", lout);
1903 if (id2>=0)
1904 fMac3=new Macs(id2, "Mac/Az-Sync", lout);
1905
1906 fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
1907 fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
1908 fAz =new ShaftEncoder(id6, "SE/Az", lout);
1909
1910 lout << "- Connecting devices to network." << endl;
1911
1912 //
1913 // Connect the devices to the network
1914 //
1915 SetNode(fMac1);
1916 SetNode(fMac2);
1917 if (id2>=0)
1918 SetNode(fMac3);
1919 SetNode(fZd1);
1920 SetNode(fZd2);
1921 SetNode(fAz);
1922
1923 //
1924 // Create Gui Event timer and Gui
1925 //
1926 lout << "- Initializing GUI Timer." << endl;
1927 fUpdateGui = new TTimer(this, 100); // 100ms
1928
1929 lout << "- Starting GUI." << endl;
1930 fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
1931}
1932
1933void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
1934{
1935 //
1936 // Create Nodes
1937 //
1938 lout << "- Setting up network." << endl;
1939
1940 fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
1941 fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
1942 fAz =new ShaftEncoder(id6, "SE/Az", lout);
1943
1944 lout << "- Connecting devices to network." << endl;
1945
1946 //
1947 // Connect the devices to the network
1948 //
1949 SetNode(fZd1);
1950 SetNode(fZd2);
1951 SetNode(fAz);
1952
1953 //
1954 // Create Gui Event timer and Gui
1955 //
1956 lout << "- Initializing GUI Timer." << endl;
1957 fUpdateGui = new TTimer(this, 100); // 100ms
1958
1959 lout << "- Starting GUI." << endl;
1960 fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
1961}
1962
1963void MCosy::ConstructorDemo()
1964{
1965 //
1966 // Create Nodes
1967 //
1968 lout << "- Setting up network." << endl;
1969
1970 //
1971 // Create Gui Event timer and Gui
1972 //
1973 lout << "- Initializing GUI Timer." << endl;
1974 fUpdateGui = new TTimer(this, 100); // 100ms
1975
1976 lout << "- Starting GUI." << endl;
1977 fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
1978}
1979
1980MCosy::MCosy(int mode, const char *dev, const int baud, MLog &out)
1981: Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone)
1982{
1983 TEnv env(".cosyrc");
1984 const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
1985 const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
1986 const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3
1987 const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4
1988 const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5
1989 const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6
1990
1991 lout << "- Program in ";
1992 switch (mode)
1993 {
1994 case 0:
1995 lout << "<<Stanard mode>>" << endl;
1996 fBending.Load("bending.txt");
1997 Constructor(id1, id2, id3, id4, id5, id6);
1998 break;
1999 case 1:
2000 lout << "<<SE mode>>" << endl;
2001 fBending.Load("bending.txt");
2002 ConstructorSE(id4, id5, id6);
2003 break;
2004 default:
2005 lout << "<<Demo mode>>" << endl;
2006 ConstructorDemo();
2007 }
2008
2009 lout.SetOutputGui(fWin->GetLog(), kTRUE);
2010
2011 fZd1->SetDisplay(fWin->GetLabel2());
2012 fZd2->SetDisplay(fWin->GetLabel3());
2013 fAz->SetDisplay(fWin->GetLabel1());
2014
2015 int i=0;
2016 char name[100];
2017 while (1)
2018 {
2019 sprintf(name, "tpoint/tpoint%03d.txt", i++);
2020 if (gSystem->AccessPathName(name, kFileExists))
2021 break;
2022 }
2023
2024 Timer time;
2025 time.Now();
2026
2027 cout << "TPoint File ********* " << name << " ********** " << endl;
2028
2029 tpout = new ofstream(name);
2030 *tpout << "Magic Model TPOINT data file" << endl;
2031 *tpout << ": ALTAZ" << endl;
2032 *tpout << "49 48 0 ";
2033 *tpout << time.Year() << " " << time.Month() << " " << time.Day() << " ";
2034 *tpout << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
2035 // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
2036}
2037
2038void MCosy::TerminateApp()
2039{
2040 cout << "MCosy::TerminateApp()" << endl;
2041/*
2042 Int_t rc;
2043 TGMessageBox msg(this, gClient->GetRoot(),
2044 "Information",
2045 "Cosy is shutting down the system - this may take wa while!",
2046 kMBIconExclamation,
2047 kMBOK, //kMBClose
2048 &rc, 0);
2049*/
2050
2051 lout.DisableOutputDevice(MLog::eGui);
2052 // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
2053 // lout.SetOutputGui(NULL, kFALSE);
2054
2055 gApplication->Terminate(0);
2056}
2057
2058MCosy::~MCosy()
2059{
2060 *tpout << "END" << endl;
2061 delete tpout;
2062
2063 cout << "Deleting GUI timer." << endl;
2064
2065 delete fUpdateGui;
2066
2067 cout << "Deleting Nodes." << endl;
2068
2069 delete fAz;
2070 delete fZd1;
2071 delete fZd2;
2072 delete fMac1;
2073 delete fMac2;
2074 if (fMac3)
2075 delete fMac3;
2076
2077 cout << "Deleting MGCosy." << endl;
2078
2079 lout.DisableOutputDevice(MLog::eGui);
2080
2081 delete fWin;
2082
2083 cout << "MGCosy destructed." << endl;
2084}
Note: See TracBrowser for help on using the repository browser.