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

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