source: trunk/MagicSoft/Cosy/MCosy.cc@ 774

Last change on this file since 774 was 738, checked in by tbretz, 24 years ago
*** empty log message ***
  • Property svn:executable set to *
File size: 18.7 KB
Line 
1#include "MCosy.h"
2
3#include <iomanip.h>
4#include <fstream.h>
5#include <iostream.h>
6
7#include <TROOT.h>
8#include <TSystem.h>
9#include <TApplication.h>
10
11#include "MGCosy.h"
12
13#include "macs.h"
14#include "timer.h"
15#include "slalib.h"
16#include "slamac.h"
17#include "shaftencoder.h"
18
19#include <sys/resource.h> // PRIO_PROCESS
20
21typedef struct tm tm_t;
22
23#define GEAR_RATIO_ALT 75.55 // 75.25 VERY IMPORTANT! unit=RE/SE
24#define GEAR_RATIO_AZ 179.8 // VERY IMPORTANT! unit=RE/SE
25
26const XY kGearRatio(GEAR_RATIO_ALT, GEAR_RATIO_AZ);
27const XY kGearRatio2(GEAR_RATIO_ALT*16384.0/360.0, GEAR_RATIO_AZ*16384.0/360.0);
28
29double Rad2SE(double rad)
30{
31 return 16384.0/D2PI*rad;
32}
33
34double Rad2ZdRE(double rad)
35{
36 return 16384.0/D2PI*rad*kGearRatio.X();
37}
38
39double Rad2AzRE(double rad)
40{
41 return 16384.0/D2PI*rad*kGearRatio.Y();
42}
43
44double Deg2ZdRE(double rad)
45{
46 return rad*kGearRatio2.X();
47}
48
49double Deg2AzRE(double rad)
50{
51 return rad*kGearRatio2.Y();
52}
53
54//double Rad2Deg(double rad)
55//{
56// return 360.0/D2PI*rad;
57//}
58
59ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
60{
61 // src [se]
62 // dst [rad]
63
64 // fAltMax = 70
65 // fAltMin = -105/110
66 // fAzMin = -355
67 // fAzMax = 355
68
69 ZdAz source = src * 360.0/16384.0;
70 ZdAz dest = dst * 360.0/D2PI;
71
72 if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
73 return dst;
74
75 const float fZdMin = -67;
76 const float fZdMax = 67;
77 const float fAzMin = -29;
78 const float fAzMax = 423;
79
80 //
81 // This corrects to target for the shortest distance, not for the fastest move!
82 //
83 ZdAz s = source-dest;
84
85 float min = s.Sqr();
86
87 //
88 // Is it enought to search inside one revolution?
89 //
90 ZdAz ret = dest;
91
92 for (int i=-5; i<5+1; i++)
93 {
94 const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);
95
96 //
97 // Range Check
98 //
99 if (p.Zd()<fZdMin || p.Zd()>fZdMax)
100 continue;
101
102 if (p.Az()<fAzMin || p.Az()>fAzMax)
103 continue;
104
105 //
106 // Calculate distance
107 //
108 s = source-p;
109
110 const float dist = s.Sqr();
111
112 if (dist > min)
113 continue;
114
115 //
116 // New shortest distance
117 //
118 ret = p;
119 min = dist;
120 }
121 return ret*(16384.0/360.0);
122}
123
124
125ZdAz MCosy::GetSePos()
126{
127 const int p0 = fAlt1->GetPos();
128 const int p1 = fAlt2->GetPos();
129 const int p2 = fAz->GetPos();
130
131 const int a0 = p0; //p0>8192?p0-16384:p0;
132 const int a1 = p1; //p1>8192?p1-16384:p1;
133 const int a2 = p2; //p2>8192?p2-16384:p2;
134
135 //
136 // interpolate shaft encoder positions
137 //
138 const float a = (float)(a0-a1)/2;
139
140 //
141 // calculate 'regelabweichung'
142 //
143 return ZdAz(a, a2);
144}
145
146ZdAz MCosy::GetRePos()
147{
148 return ZdAz(fMac2->GetPos(), fMac1->GetPos());
149}
150
151int MCosy::SetPosition(const ZdAz &dst) // [rad]
152{
153 // FIXME: CORRECT BY fOffset !!!!!!!!!!
154
155 //
156 // Calculate new target position (shortest distance to go)
157 //
158 const ZdAz src = GetSePos();
159 const ZdAz dest = CorrectTarget(src, dst);
160
161 cout << "Positioning to Target:" << endl;
162// cout << "Source Alt: " << src.Alt() << "se Az:" << src.Az() << "se -> Alt: " << srcd.Alt() << kDEG << " Az:" << srcd.Az() << kDEG << endl;
163// cout << "Shortest Dest Alt: " << dest.Alt() << "se Az:" << dest.Az() << "se -> Alt: " << sed.Alt() << kDEG << " Az:" << sed.Az() << kDEG << endl;
164
165 for (int i=0; i<10; i++)
166 {
167 //
168 // Get Shaft Encoder Positions
169 //
170 const ZdAz p=GetSePos();
171
172 //
173 // calculate control deviation and rounded cd
174 //
175 ZdAz rd = dest-p; // [se]
176
177 ZdAz cd = rd; // [se]
178 cd.Round();
179
180 //
181 // check if we reached the correct position already
182 //
183 if (!(int)cd.Zd() && !(int)cd.Az())
184 {
185 cout << "Positioning done with " << i << "manuvers." << endl;
186 return TRUE;
187 }
188
189 //
190 // change units from se to re
191 //
192 rd *= kGearRatio; // [re]
193
194 //
195 // Set velocities
196 //
197 const int vr = fMac1->GetVelRes();
198 const float maxvel = (i?0.1:0.9)*vr; // maxvel = 90%
199 const float maxacc = (i?0.1:0.5)*vr; // maxacc = 50%;
200
201 const float diff = i?1:fabs(rd.Ratio());
202
203 cout << "Salt/Saz: " << diff << endl;
204
205 if (diff <1)
206 {
207 fMac1->SetVelocity(maxvel);
208 fMac1->SetAcceleration(maxacc);
209 fMac1->SetDeceleration(maxacc);
210
211 fMac2->SetVelocity(maxvel*diff);
212 fMac2->SetAcceleration(maxacc*diff);
213 fMac2->SetDeceleration(maxacc*diff);
214 }
215 else
216 {
217 fMac1->SetVelocity(maxvel/diff);
218 fMac1->SetAcceleration(maxacc/diff);
219 fMac1->SetDeceleration(maxacc/diff);
220
221 fMac2->SetVelocity(maxvel);
222 fMac2->SetAcceleration(maxacc);
223 fMac2->SetDeceleration(maxacc);
224 }
225
226 rd.Round();
227
228 cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
229 cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
230 cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
231
232 //
233 // repositioning (relative)
234 //
235 if ((int)cd.Zd()) fMac2->StartRelPos(rd.Zd());
236 if ((int)cd.Az()) fMac1->StartRelPos(rd.Az());
237
238 cout << "Waiting for positioning..." << flush;
239
240 WaitForSdos();
241
242 cout << "SDO..." << flush;
243
244 while (fMac1->IsPositioning() || fMac2->IsPositioning())
245 {
246 if (StopWaitingForSDO())
247 return FALSE;
248
249 usleep(1);
250 }
251
252 cout << "done." << endl;
253 }
254
255 cout << "Positioning ERROR!" << endl;
256 return FALSE;
257}
258
259void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
260{
261 //
262 // Position to actual position
263 //
264 Timer t;
265 t.GetTime();
266
267 RaDec pm;
268 ZdAz dest = RaDec2ZdAz(t.GetMjd(), dst, pm);
269
270 if (!SetPosition(dest))
271 {
272 cout << "ERROR: Cannot start tracking, unable to reach requested position." << endl;
273 return;
274 }
275
276 if (StopWaitingForSDO())
277 return;
278
279 //
280 // calculate offset from present se position
281 //
282
283 const ZdAz sepos = GetSePos()*kGearRatio;
284
285 fMac1->ReqPos();
286 fMac2->ReqPos();
287
288 const ZdAz repos=GetRePos();
289
290 fOffset = sepos-repos;
291
292 cout << "Offset: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
293 cout << "Offset: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
294 cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
295
296 //
297 // Start revolution mode
298 //
299 fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
300 fMac2->SetDeceleration(0.90*fMac2->GetVelRes());
301 fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
302 fMac1->SetDeceleration(0.90*fMac1->GetVelRes());
303
304 fMac2->SetRpmMode(TRUE);
305 fMac1->SetRpmMode(TRUE);
306
307 /*-*/ int s = t.GetSecs();
308 cout << "Start tracking: Ra: " << Rad2Deg(dst.Ra()) << kDEG << " Dec: ";
309 cout << Rad2Deg(dst.Dec()) << kDEG << endl;
310
311 //
312 // Initialize Tracker (slalib or starguider)
313 //
314 fRaDec = dst;
315 fTracking = kTRUE;
316
317 //
318 // We want to reach the theoretical position exactly in about 0.5s
319 //
320 const float dt = 1; // 1 second
321 while (!StopWaitingForSDO())
322 {
323 //
324 // Request Real Position from Drive
325 //
326 Timer t;
327 t.GetTime();
328
329 //
330 // Request theoretical Position for a time in the future (To+dt) from CPU
331 //
332 dest = CorrectTarget(GetSePos(), RaDec2ZdAz(t.GetMjd()+dt/(60*60*24), dst, pm));
333
334 fMac2->RequestSDO(0x6004);
335 fMac1->RequestSDO(0x6004);
336 WaitForSdos();
337 if (StopWaitingForSDO())
338 {
339 lout << "Error 6004 happened" << endl;
340 SkipPendingSdos();
341 break;
342 }
343
344 //
345 // Copy fOffset to a local variable
346 //
347 ZdAz offset = fOffset;
348
349 //
350 // distance between (To+dt) and To [re]
351 // position time difference < 5usec
352 //
353 dest *= kGearRatio;
354 dest -= GetRePos() + offset;
355
356 //
357 // Velocity to go [re/min] to reach the right position at time t+dt
358 // correct for the duration of RaDec2AltAz
359 //
360 const ZdAz v = dest*60.0/(dt-(fMac2->GetTime()-t));
361
362 //
363 // calculate real velocity of future [re/min]
364 //
365 ZdAz vt = v/4;
366 vt.Round();
367
368 if (v.Zd()>.9*fMac1->GetVelRes() || v.Az()>.9*fMac2->GetVelRes())
369 {
370 cout << "Error: Tracking speed faster than possible maximum velocity." << endl;
371 break;
372 }
373
374 //
375 // Set theoretical velocity (as early after calculation as possible)
376 //
377 //
378 // Maybe we should attenuate the changes
379 //
380 fMac2->SendSDO(0x3006, 1, (LWORD_t)vt.Zd()); // SetRpmVelocity [re/min]
381 fMac1->SendSDO(0x3006, 1, (LWORD_t)vt.Az()); // SetRpmVelocity [re/min]
382 WaitForSdos();
383 if (StopWaitingForSDO())
384 {
385 lout << "Error 3006 happened" << endl;
386 SkipPendingSdos();
387 break;
388 }
389
390 //
391 // Now do 'unnecessary' things
392 // calculate control deviation - for the moment for convinience
393 //
394 if (fMac1->GetTime()-s > 1)
395 {
396 ZdAz dest0=CorrectTarget(GetSePos(),
397 RaDec2ZdAz(fMac2->GetMjd(), dst, pm));
398 dest0 *= kGearRatio;
399 dest0 -= GetRePos()+offset;
400 dest0.Round();
401 cout << "Control deviation: ";
402 cout << setw(4) << (int)fOffset.Zd() << " ";
403 cout << setw(4) << (int)fOffset.Az() << " re ";
404
405 cout << setw(4) << dest0.Zd() << " ";
406 cout << setw(4) << dest0.Az() << " re V: ";
407 cout << setw(4) << vt.Zd() << " ";
408 cout << setw(4) << vt.Az() << " re/min/4" << endl;
409 s = (int)fMac1->GetTime();
410 }
411
412 //
413 // Update speed as often as possible.
414 // make sure, that dt is around 10 times larger than the
415 // update time
416 //
417 // usleep(50000); // 0.05s
418 }
419
420 fTracking = kFALSE;
421
422 //
423 // Stop revolution mode
424 //
425 fMac2->SetRpmMode(FALSE);
426 fMac1->SetRpmMode(FALSE);
427
428 cout << "Tracking stopped." << endl;
429}
430
431ZdAz MCosy::RaDec2ZdAz(const double mjd, const RaDec &dst, const RaDec &pm)
432{
433 int status;
434
435 //
436 // calculate observers location (goe)
437 //
438 double fPhi, fElong;
439 slaDaf2r(51, 38, 48.0, &fPhi, &status);
440 slaDaf2r( 9, 56, 36.0, &fElong, &status);
441
442 // cout << "fPhi: 51ø38'48.0\" = " << 360.0/D2PI*fPhi << endl;
443 // cout << "fElong: 9ø56'36.0\" = " << 360.0/D2PI*fElong << endl;
444
445 //
446 // ----- calculate star independent parameters ----------
447 //
448 double fAmprms[21];
449 double fAoprms[14];
450 slaMappa(2000.0, mjd, fAmprms);
451 slaAoppa(mjd, 0, // mjd, UT1-UTC
452 fElong, fPhi, 148, // g”ttingen long, lat, height
453 0, 0, // polar motion x, y-coordinate (radians)
454 273.155, 1013.25, 0.5, // temp, pressure, humidity
455 0.2, 0.0065, // wavelength, tropo lapse rate
456 fAoprms);
457
458 //
459 // ---- Mean to apparent ----
460 //
461 double r=0, d=0;
462 slaMapqkz(dst.Ra(), dst.Dec(), (double*)fAmprms, &r, &d);
463 //
464 // Doesn't work - don't know why
465 //
466 // slaMapqk(dst.Ra(), dst.Dec(), pm.Ra(), pm.Dec(), // ra, dec (rad), r, d (rad)
467 // 0, 0, fAmprms, &r, &d);
468 //
469
470 //
471 // -- apparent to observed --
472 //
473 double r1=0; // ra
474 double d1=0; // dec
475 double h0=0; // ha
476
477 double az, zd;
478 slaAopqk (r, d, fAoprms,
479 &az, // observed azimuth (radians: N=0,E=90)
480 &zd, // observed zenith distance (radians)
481 &h0, // observed hour angle (radians)
482 &d1, // observed declination (radians)
483 &r1); // observed right ascension (radians)
484
485 return ZdAz(zd, az);
486}
487
488void *MCosy::Proc(int msg, void *mp)
489{
490 switch (msg)
491 {
492 case WM_STOP:
493 cout << "Stopping positioning." << endl;
494 fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
495 fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
496 cout << "Stoping" << endl;
497 fMac1->SetRpmMode(FALSE);
498 fMac2->SetRpmMode(FALSE);
499 cout << "Done." << endl;
500 while (fMac1->IsPositioning() || fMac2->IsPositioning())
501 usleep(1);
502 cout << "Positioning stopped." << endl;
503 return NULL;
504
505 case WM_PRESET:
506 cout << "WM_PRESET: START" << endl;
507 fAlt1->SetPreset();
508 fAlt2->SetPreset();
509 fAz->SetPreset();
510 cout << "WM_PRESET: DONE (return 0xaffe)" << endl;
511 return (void*)0xaffe;
512
513 case WM_POLARIS:
514 {
515 cout << "WM_POLARIS: START" << endl;
516 Timer t;
517 t.GetTime();
518
519 RaDec rd(37.94, 89.2644);
520 ZdAz za=MCosy::RaDec2ZdAz(t.GetMjd(), rd*D2PI/360.0)*16384.0/D2PI;
521
522 cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
523
524 ZdAz sepos = GetSePos();
525 cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
526
527 fAlt1->SetPreset(za.Zd());
528 fAlt2->SetPreset(-za.Zd());
529 fAz->SetPreset(za.Az());
530
531 cout << "WM_PRESET: DONE (return 0xaffe)" << endl;
532 }
533 return (void*)0xaffe;
534
535 case WM_POSITION:
536 cout << "WM_POSITION: START" << endl;
537 {
538 ZdAz dest = *((ZdAz*)mp);
539 SetPosition(dest*D2PI/360.0);
540 }
541 cout << "WM_POSITION: DONE (return 0x7777)" << endl;
542 return (void*)0x7777;
543
544 case WM_TRACK:
545 cout << "WM_TRACK: START" << endl;
546 {
547 RaDec dest = *((RaDec*)mp);
548 TrackPosition(dest*D2PI/360.0);
549 }
550 cout << "WM_TRACK: DONE (return 0x8888)" << endl;
551 return (void*)0x8888;
552 }
553 cout << "Unknown Msg" << endl;
554 return (void*)0xffffffff;
555}
556
557void MCosy::TalkThread()
558{
559 //
560 // Start the Network
561 //
562 Network::Start();
563 PostMsg(WM_PRESET, 0, 0);
564/*
565 cout << "PostMsg(WM_PRESET)" << endl;
566 void *rc =
567 cout << hex << "WM_PRESET: ret=" << rc << endl;
568
569 RaDec dest = RaDec(45.0, 30.0)*D2PI/360.0;
570
571 cout << "PostMsg(WM_TRACK)" << endl;
572 cout << sizeof(RaDec) << "==" << sizeof(dest) << endl;
573 rc=PostMsg(WM_TRACK, &dest, sizeof(dest));
574 cout << "DEST killed." << endl;
575*/
576 // AltAz dest = AltAz(45.0, 30.0);
577 // double ra, dec;
578 // slaDaf2r( 71, 0, 0, &ra, &status); // 0 WARNING: RANGE
579 // slaDaf2r( 89, 0, 0, &dec, &status); // 49
580 // cout << "Start tracking: Ra: " << Rad2Deg(ra) << kDEG << " Dec: " << Rad2Deg(dec) << kDEG << endl;
581
582 // dest = AltAz(-46.0, 210);
583 // SetPosition(dest);
584 setpriority(PRIO_PROCESS, 0, 10);
585
586 while (1)
587 {
588 //
589 // wait until a tracking session is started
590 //
591 while (!fTracking)
592 usleep(1);
593
594 RaDec pm;
595 ZdAz sollalt; // [se]
596 ZdAz sollaz; // [se]
597 ZdAz old;
598
599 ZdAz ist=fOffset/kGearRatio; // [se]
600 //
601 // only update fOffset while tracking
602 //
603 while (fTracking)
604 {
605 usleep(100000/*00*/); // 0.1s
606
607 //
608 // Make changes (eg wind) smoother - attenuation of control function
609 //
610
611 ZdAz offset(fOffset.Zd()*9.0/10.0+((ist.Zd()-sollalt.Zd())*kGearRatio.X())/10.0,
612 fOffset.Az()*9.0/10.0+((ist.Az()-sollaz.Az()) *kGearRatio.Y())/10.0);
613
614 fOffset = offset;
615 // fOffset.Zd(((offset.Zd()>1000)||(offset.Zd()<-1000))?0:offset.Zd());
616 // fOffset.Az(((offset.Az()>1000)||(offset.Az()<-1000))?0:offset.Az());
617
618 //
619 // get position, where we are
620 //
621 ist = GetSePos(); // [se]
622
623 //
624 // if the position didn't change continue
625 //
626 if ((int)ist.Zd() == (int)old.Zd() &&
627 (int)ist.Az() == (int)old.Az())
628 continue;
629
630 //
631 // if Alt Shaftencoder changed position
632 //
633 if ((int)ist.Zd() != (int)old.Zd())
634 {
635 //
636 // Get time from last shaftencoder position change
637 //
638 const double t = (fAlt1->GetMjd()+fAlt2->GetMjd())/2.0;
639
640 //
641 // calculate were we should be
642 //
643 sollalt = CorrectTarget(ist, RaDec2ZdAz(t, fRaDec, pm));
644
645 old.Zd(ist.Zd());
646 }
647
648 //
649 // if Alt Shaftencoder changed position
650 //
651 if ((int)ist.Az() != (int)old.Az())
652 {
653 //
654 // Get time from last shaftencoder position change
655 //
656 const double t = fAz->GetMjd();
657
658 //
659 // calculate were we should be
660 //
661 sollaz = CorrectTarget(ist, RaDec2ZdAz(t, fRaDec, pm));
662
663 old.Az(ist.Az());
664 }
665 }
666 }
667}
668
669void *MCosy::MapTalkThread(void *arg)
670{
671 pthread_detach(pthread_self());
672
673 MCosy *cosy = (MCosy*)arg;
674
675 cosy->TalkThread();
676
677 cosy->lout << "- Sending Thread done." << endl;
678
679 return NULL;
680}
681
682int MCosy::StopWaitingForSDO()
683{
684 return Break() || fMac1->HasError() || fMac2->HasError();
685}
686
687void MCosy::Start()
688{
689 if (fTxThrd)
690 {
691 cout << "Error: tx thread already started." << endl;
692 return;
693 }
694
695 lout << "- Starting sending Thread." << endl;
696
697 fTxThrd = new pthread_t;
698 pthread_create(fTxThrd, NULL, MapTalkThread, this);
699}
700
701void MCosy::Stop()
702{
703 if (!fTxThrd)
704 return;
705
706 pthread_cancel(*fTxThrd);
707
708 delete fTxThrd;
709 fTxThrd = NULL;
710
711 lout << "- Sending Thread stopped." << endl;
712
713 SkipPendingSdos();
714
715 Network::Stop();
716}
717
718MCosy::MCosy(const char *dev, const int baud, ostream &out)
719: Network(dev, baud, out), fTxThrd(NULL), fTracking(kFALSE)
720{
721 //
722 // Create Nodes
723 //
724 fMac1=new Macs(1, lout);
725 fMac2=new Macs(2, lout);
726 fAlt1=new ShaftEncoder(4, lout);
727 fAlt2=new ShaftEncoder(5, lout);
728 fAz =new ShaftEncoder(6, lout);
729
730 //
731 // Connect the devices to the network
732 //
733 SetNode(fMac1);
734 SetNode(fMac2);
735 SetNode(fAlt1);
736 SetNode(fAlt2);
737 SetNode(fAz);
738
739 MGCosy *fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
740
741 fAz->SetDisplay(fWin->GetLabel1());
742 fAlt1->SetDisplay(fWin->GetLabel2());
743 fAlt2->SetDisplay(fWin->GetLabel3());
744}
745
746void MCosy::TerminateApp()
747{
748 gSystem->ExitLoop();
749}
750
751MCosy::~MCosy()
752{
753 delete fAz;
754 delete fAlt2;
755 delete fAlt1;
756 delete fMac1;
757 delete fMac2;
758
759 delete fWin;
760}
Note: See TracBrowser for help on using the repository browser.