source: trunk/MagicSoft/Cosy/devdrv/macs.cc@ 1683

Last change on this file since 1683 was 1275, checked in by tbretz, 24 years ago
*** empty log message ***
File size: 18.2 KB
Line 
1#include "macs.h"
2
3#include <iostream.h>
4#include <sys/time.h> // timeval->tv_sec
5
6#include "timer.h"
7#include "network.h"
8#include "MLogManip.h"
9
10ClassImp(Macs);
11
12/*
13 ---------------------------
14 For test purposes
15 ---------------------------
16
17class MyTimer : public TTimer
18{
19public:
20 MyTimer(TObject *obj, Long_t ms, Bool_t mode) : TTimer(obj, ms, mode) {}
21 Bool_t Notify()
22 {
23 cout << "Notify" << endl;
24 TTimer::Notify();
25 return kTRUE;
26 }
27};
28*/
29
30Macs::Macs(const BYTE_t nodeid, const char *name, MLog &out)
31 : NodeDrv(nodeid, name, out), fMacId(2*nodeid+1),
32 fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0),
33 fPosActive(0), fRpmActive(0)
34{
35 fTimeout = new TTimer(this, 100, kFALSE); // 100ms, asynchronous
36}
37
38Macs::~Macs()
39{
40 fTimerOn = kFALSE;
41 delete fTimeout;
42}
43
44void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
45{
46 switch (idx)
47 {
48 case 0x100a:
49 lout << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
50 return;
51
52 case 0x2002:
53 cout << GetNodeName() << ": Actual velocity: " << dec << val << endl;
54 fVel = val;
55 return;
56
57 case 0x4000:
58 switch (subidx)
59 {
60 case 1:
61 cout << GetNodeName() << ": Timeout timer is " << (val?"en":"dis") << "abled." << endl;
62 return;
63 case 2:
64 cout << GetNodeName() << ": Actual timeout time: " << dec << val << "ms" << endl;
65 return;
66 }
67 break;
68
69 case 0x6004:
70 if (subidx)
71 return;
72
73// lout << "Actual Position: " << dec << (signed long)val << endl;
74 fPos = (LWORDS_t)val;
75 fPosTime.SetTimer(tv);
76 return;
77/*
78 case 0x2001:
79 cout << "Axe Status: 0x" << hex << val << endl;
80 cout << " - Motor " << (val&0x01?"standing":"moving") << endl;
81 cout << " - Positioning " << (val&0x02?"active":"inactive") << endl;
82 cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl;
83 cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
84 cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl;
85 fPosActive = val&0x02;
86 fRpmActive = val&0x04;
87 return;
88 case 0x2003:
89 if (!subidx)
90 {
91 cout << "Input State: ";
92 for (int i=0; i<8; i++)
93 cout << (int)(val&(1<<i)?1:0);
94 cout <<endl;
95 return;
96 }
97 cout << "Input No." << subidx << (val?"hi":"lo") << endl;
98 return;
99 case 0x2004:
100 cout << "Status Value of Axis: 0x" << hex << val << endl;
101 cout << " - Attitude control: " << (val&0x800000?"off":"on") << endl;
102 cout << " - Movement done: " << (val&0x040000?"yes":"no") << endl;
103 cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
104 cout << " - Attitude control: " << (val&0x04?"off":"on") << endl;
105 cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl;
106 cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl;
107 cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl;
108 return;
109*/
110
111 case 0x6002:
112 lout << "- Velocity resolution " << GetNodeName() << ": " << dec << val << " ticks/min" << endl;
113 fVelRes = val;
114 return;
115 }
116 cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
117 cout << ", val=0x"<<val<<endl;
118}
119
120void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx)
121{
122 switch (idx)
123 {
124 case 0x2002:
125 switch (subidx)
126 {
127 case 0:
128 lout << ddev(MLog::eGui);
129 lout << "- Velocity set (" << GetNodeName() << ")" << endl;
130 lout << edev(MLog::eGui);
131 return;
132 }
133 break;
134 case 0x2003:
135 switch (subidx)
136 {
137 case 0:
138 lout << ddev(MLog::eGui);
139 lout << "- Acceleration set (" << GetNodeName() << ")" << endl;
140 lout << edev(MLog::eGui);
141 return;
142 case 1:
143 lout << ddev(MLog::eGui);
144 lout << "- Deceleration set (" << GetNodeName() << ")" << endl;
145 lout << edev(MLog::eGui);
146 return;
147 }
148 break;
149 case 0x3006:
150 switch (subidx)
151 {
152 case 0:
153 lout << ddev(MLog::eGui);
154 lout << "- RPM mode switched (" << GetNodeName() << ")" << endl;
155 lout << edev(MLog::eGui);
156 return;
157
158 case 1:
159 /*
160 lout << ddev(MLog::eGui);
161 lout << "- Velocity set (" << GetNodeName() << ")" << endl;
162 lout << edev(MLog::eGui);
163 */
164 return;
165 }
166 break;
167 case 0x4000:
168 if (subidx==0 && fTimerOn)
169 {
170 ResetTimeout();
171 return;
172 }
173 break;
174 case 0x6004:
175 switch (subidx)
176 {
177 case 0:
178 lout << ddev(MLog::eGui);
179 lout << "- Relative positioning started (" << GetNodeName() << ")" << endl;
180 lout << edev(MLog::eGui);
181 return;
182
183 case 1:
184 lout << ddev(MLog::eGui);
185 lout << "- Absolute positioning started (" << GetNodeName() << ")" << endl;
186 lout << edev(MLog::eGui);
187 return;
188 }
189 break;
190
191
192 }
193 NodeDrv::HandleSDOOK(idx, subidx);
194}
195
196
197void Macs::ReqVelRes()
198{
199 lout << "- Requesting velocity resolution (velres, 0x6002) of " << GetNodeName() << endl;
200 RequestSDO(0x6002);
201 WaitForSdo(0x6002);
202}
203
204void Macs::SetPDO1On(BYTE_t flag)
205{
206 lout << "- " << (flag?"Enable":"Disable") << " PDO1 of " << GetNodeName() << endl;
207 SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
208 WaitForSdo(0x1800, 1);
209}
210
211void Macs::InitDevice(Network *net)
212{
213 NodeDrv::InitDevice(net);
214
215// SendSDO(0x4003, (LWORD_t)('E'<<24 | 'X'<<16 | 'I'<<8 'T'));
216// WaitForSdo(0x4003, 0);
217
218/*
219 lout << "- Requesting SDO 0x2002 (vel) of " << (int)GetId() << endl;
220 RequestSDO(0x2002);
221 WaitForSdo(0x2002);
222
223 lout << "- Requesting SDO 0x2003 of " << (int)GetId() << endl;
224 RequestSDO(0x2003);
225 WaitForSdo(0x2003);
226
227 lout << "- Requesting SDO 0x2004 of " << (int)GetId() << endl;
228 RequestSDO(0x2004);
229 WaitForSdo(0x2004);
230 */
231 EnableTimeout(kFALSE);
232
233 lout << "- Requesting Mac Software Version of " << GetNodeName() << endl;
234 RequestSDO(0x100a);
235 WaitForSdo(0x100a);
236
237 SetRpmMode(FALSE);
238
239 ReqVelRes(); // Init fVelRes
240
241 lout << "- Motor on of " << GetNodeName() << endl;
242 SendSDO(0x3000, string('o', 'n'));
243 WaitForSdo(0x3000);
244
245
246// SetHome(250000);
247
248// lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
249// RequestSDO(0x2001);
250// WaitForSdo(0x2001);
251
252 SetPDO1On(FALSE); // this is a workaround for the Macs
253 SetPDO1On(TRUE);
254
255 SetNoWait(TRUE);
256}
257
258void Macs::StopMotor()
259{
260 //
261 // Stop the motor and switch off the position control unit
262 //
263 SendSDO(0x3000, string('s','t','o','p'));
264 WaitForSdo(0x3000);
265}
266
267void Macs::StopDevice()
268{
269 EnableTimeout(kFALSE);
270
271 SetNoWait(FALSE);
272
273 //
274 // FIXME: This isn't called if the initialization isn't done completely!
275 //
276
277 SetRpmMode(FALSE);
278
279 SetPDO1On(FALSE);
280
281 lout << "- Motor off of " << GetNodeName() << endl;
282 SendSDO(0x3000, string('o', 'f', 'f'));
283 WaitForSdo(0x3000);
284
285 /*
286 lout << "- Stopping Program of " << (int)GetId() << endl;
287 SendSDO(0x4000, (LWORD_t)0xaffe);
288 WaitForSdo();
289 */
290}
291
292void Macs::ReqPos()
293{
294 lout << "- Requesting Position of " << GetNodeName() << endl;
295 RequestSDO(0x6004);
296 WaitForSdo(0x6004);
297}
298
299void Macs::ReqVel()
300{
301 lout << "- Requesting Velocity of " << GetNodeName() << endl;
302 RequestSDO(0x2002);
303 WaitForSdo(0x2002);
304}
305
306void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
307{
308 lout << "- Driving " << GetNodeName() << " to home position, Offset=" << dec << pos << endl;
309 SendSDO(0x6003, 2, (LWORD_t)pos); // home
310 WaitForSdo(0x6003, 2);
311
312 // home also defines the zero point of the system
313 // maximum time allowd for home drive: 25.000ms
314 SendSDO(0x3001, string('h','o','m','e')); // home
315 WaitForSdo(0x3001, 0, maxtime*1000);
316 lout << "- Home position of " << GetNodeName() << " reached. " << endl;
317
318 SendSDO(0x6003, 0, string('s','e','t')); // home
319 WaitForSdo(0x6003, 0);
320}
321
322void Macs::SetVelocity(LWORD_t vel)
323{
324 SendSDO(0x2002, vel); // velocity
325 WaitForSdo(0x2002);
326}
327
328void Macs::SetAcceleration(LWORD_t acc)
329{
330 SendSDO(0x2003, 0, acc); // acceleration
331 WaitForSdo(0x2003, 0);
332}
333
334void Macs::SetDeceleration(LWORD_t dec)
335{
336 SendSDO(0x2003, 1, dec); // acceleration
337 WaitForSdo(0x2003, 1);
338}
339
340void Macs::SetRpmMode(BYTE_t mode)
341{
342 //
343 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
344 //
345 SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
346 WaitForSdo(0x3006, 0);
347}
348
349void Macs::SetRpmVelocity(LWORDS_t cvel)
350{
351 SendSDO(0x3006, 1, (LWORD_t)cvel);
352 WaitForSdo(0x3006, 1);
353}
354
355void Macs::StartRelPos(LWORDS_t pos)
356{
357 SendSDO(0x6004, 1, (LWORD_t)pos);
358}
359
360void Macs::StartAbsPos(LWORDS_t pos)
361{
362 SendSDO(0x6004, 0, (LWORD_t)pos);
363}
364
365void Macs::SetNoWait(BYTE_t flag)
366{
367 lout << "- Setting NOWAIT " << (flag?"ON ":"OFF ") << GetNodeName() << endl;
368 SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f'));
369 WaitForSdo(0x3008);
370}
371
372void Macs::StartVelSync()
373{
374 //
375 // The syncronization mode is disabled by a 'MOTOR STOP'
376 // or by a positioning command (POSA, ...)
377 //
378 lout << "- Setting Vel Sync Mode of " << GetNodeName() << endl;
379 SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
380 WaitForSdo(0x3007, 0);
381}
382
383void Macs::StartPosSync()
384{
385 //
386 // The syncronization mode is disabled by a 'MOTOR STOP'
387 // or by a positioning command (POSA, ...)
388 //
389 lout << "- Setting Pos Sync Mode of " << GetNodeName() << endl;
390 SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
391 WaitForSdo(0x3007, 1);
392}
393/*
394void Macs::ReqAxEnd()
395{
396 RequestSDO(0x2001);
397 WaitForSdo(0x2001);
398}
399*/
400void Macs::SendMsg(BYTE_t data[6])
401{
402 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
403}
404
405void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
406 BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
407{
408 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
409}
410
411void Macs::HandlePDO1(BYTE_t *data, timeval_t *tv)
412{
413 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
414
415 fPosActive = data[3]&0x02;
416 fRpmActive = data[3]&0x04;
417
418 fPdoTime.SetTimer(tv);
419}
420
421void Macs::HandlePDO2(BYTE_t *data, timeval_t *tv)
422{
423 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
424 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
425
426 //
427 // errnum==0 gives a sudden information that something happened. Now the
428 // microcontroller is running inside its interrup procedure which
429 // stopped the normal program. The interrupt procedure should try to clear
430 // the error state of the hardware. This should never create a new error!
431 //
432 if (!errnum)
433 {
434 lout << "Mac " << GetNodeName() << " reports Error occursion." << endl;
435 SetError(-1);
436 return;
437 }
438
439 //
440 // Now the error is handled by the hardware now it is the software part
441 // to react on it. The Error flag now is set to the correct value.
442 //
443 if (GetError()>0)
444 lout << GetNodeName() << " WARNING! Error #" << GetError() << " unhandled by software." << endl;
445
446 SetError(errnum);
447
448 lout << GetNodeName() << " reports: ";
449 switch (errnum)
450 {
451 case 6:
452 lout << "Home position not the first positioning command." << endl;
453 return;
454
455 case 8:
456 lout << "Control deviation overflow." << endl;
457 return;
458
459 case 9:
460 lout << "Zero index not found." << endl;
461 return;
462
463 case 11:
464 case 25:
465 switch (errinf)
466 {
467 case -1:
468 lout << "Negative";
469 break;
470 case 1:
471 lout << "Positive";
472 break;
473 default:
474 lout << "-unknown-";
475 }
476 switch (errnum)
477 {
478 case 11:
479 lout << " software endswitch activated." << endl;
480 break;
481 case 25:
482 lout << " hardware endswitch activated." << endl;
483 break;
484 }
485 return;
486
487 case 84:
488 lout << "Too many (>12) ON TIME calls." << endl;
489 return;
490
491 case 100:
492 lout << "Connection timed out." << endl;
493 return;
494
495 default:
496 lout << "Error Nr. " << errnum << ", " << errinf << endl;
497 }
498}
499
500void Macs::HandleError()
501{
502 //
503 // If there is no error we must not handle anything
504 //
505 if (!HasError())
506 return;
507
508 //
509 // If the program got into the: HandleError state before the hardware
510 // has finished handeling the error we have to wait for the hardware
511 // handeling the error
512 //
513 // FIXME: Timeout???
514 //
515 while (GetError()<0)
516 usleep(1);
517
518 //
519 // After this software and hardware should be in a state so that
520 // we can go on working 'as usual' Eg. Initialize a Display Update
521 //
522 cout << GetNodeName() << " Handling Error #" << GetError() << endl;
523 switch (GetError())
524 {
525 case 6: // home
526 case 8: // control dev
527 case 9: // zero idx
528 case 84: // ON TIME
529 // Stop program?
530 return;
531
532 case 11: // software endswitch
533 case 25: // hardware endswitch
534 case 100: // timeout (movement has been stopped, so we can go on)
535 DelError();
536 return;
537
538 case 101:
539 lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
540 DelError();
541 return;
542 }
543}
544
545double Macs::GetTime()
546{
547 return fPosTime.Now();
548}
549
550double Macs::GetMjd()
551{
552 return fPosTime.CalcMjd();
553}
554
555double Macs::GetPdoTime()
556{
557 return fPdoTime.Now();
558}
559
560double Macs::GetPdoMjd()
561{
562 return fPdoTime.CalcMjd();
563}
564
565/* 0x2000 0 rw Maximum positioning error */
566/* 1 rw Negative Software Endswitch */
567/* 2 rw Positive Software Endswitch */
568void Macs::SetNegEndswitch(LWORDS_t val)
569{
570 SendSDO(0x2000, 1, (LWORD_t)val);
571 WaitForSdo(0x2000, 1);
572}
573
574void Macs::SetPosEndswitch(LWORDS_t val)
575{
576 SendSDO(0x2000, 2, (LWORD_t)val);
577 WaitForSdo(0x2000, 2);
578}
579
580void Macs::EnableEndswitches(bool neg, bool pos)
581{
582 SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
583 WaitForSdo(0x2000, 3);
584}
585
586void Macs::ReqTimeoutTime()
587{
588 RequestSDO(0x4000, 2);
589 WaitForSdo(0x4000, 2);
590}
591
592void Macs::EnableTimeout(bool enable, LWORDS_t ms)
593{
594 if (!enable)
595 {
596 SendSDO(0x4000, 1, string('o', 'f', 'f'));
597 WaitForSdo(0x4000, 1);
598
599 fTimeout->Stop(); //kTRUE);
600
601 fTimerOn = kFALSE;
602 }
603 else
604 {
605 if (ms>0)
606 SetTimeoutTime(ms);
607
608 ResetTimeout();
609
610 fTimerOn = kTRUE;
611 fTimeout->Start(fGuardTime/*/3*2*/, kTRUE); //kFALSE); //TRUE);
612 //
613 // Start with kFALSE would be a continous timer, but this
614 // timer seems to stop it's activity at some stage without
615 // any reason
616 //
617
618// SendSDO(0x4000, 1, string('o', 'n'));
619// WaitForSdo(0x4000, 1);
620 }
621 lout << "- Timeout timer of " << GetNodeName() << " turned "
622 << (enable?"on.":"off.") << endl;
623}
624
625Bool_t Macs::HandleTimer(TTimer *t)
626{
627 /*
628 Fons:
629 -----
630
631 timers never trigger at the same time or when in a TTimer::Notify.
632 Little explanation:
633
634 - there are two types of timers synchronous and a-synchronous.
635 - synchronous timers are only handled via the ROOT eventloop
636 (see TUnixSystem::DispatchOneEvent()). If there are no mouse/keyboard
637 events then the synchronous timer queue is checked. So if the processing
638 of a mouse/keyboard event takes a long time synchronous timers are not
639 called for a while. To prevent this from happening one can call in long
640 procedures gSystem->ProcessEvents(). The system schedules only the
641 next timer in the queue when the current one's Notify() has finished.
642 - a-synchronous timers are triggered via SIGALARM, i.e. the program is
643 interupted and execution jumps to the Notify() function. When the
644 notify is finished the next a-sync timer is scheduled and the system
645 resumes from the place where it was initially interrupted. One of the
646 things to remember when using a-sync timers is don't make any graphics
647 calls in them. X11 is not re-entrant and it might be that the SIGALARM
648 signal interrupted the system while being in X11. A-sync timers are best
649 used to set flags that you can test at a convenient and controlled
650 time.
651 */
652
653 //
654 // FIXME! Use NMT!
655 //
656 // --- lout << ddev(MLog::eGui);
657 // --- lout << "Send 0x4000: " << GetNodeName() << endl;
658
659 SendSDO(0x4000, 0, (LWORD_t)0, false);
660
661 // --- lout << "Done 0x4000: " << GetNodeName() << endl;
662
663 Timer time;
664
665 // --- lout << "dT " << GetNodeName() << ": " << dec <<(int)((fTimeoutTime-time.Now())*1000) << endl;
666 // --- lout << edev(MLog::eGui);
667
668 if (time.Now() > fTimeoutTime+0.5)
669 {
670 lout << ddev(MLog::eGui);
671 lout << "Warning: " << GetNodeName() << " didn't respond in timeout window." << endl;
672 lout << edev(MLog::eGui);
673 SetError(101);
674 }
675
676 //WaitForSdo(0x4000, 0, kDontWait);
677 //
678 // Would not be necessary if I would Start the timer with
679 // kFALSE. This would be a continous timer, but this
680 // timer seems to stop it's activity at some stage without
681 // any reason
682 //
683 if (fTimerOn)
684 fTimeout->Start(fGuardTime/*/3*2*/, kTRUE);
685
686 return kTRUE;
687}
688
689void Macs::ResetTimeout()
690{
691 Timer time;
692
693 // --- lout << ddev(MLog::eGui);
694 // --- lout << "Reset " << GetNodeName() << ": " << dec << (int)((fTimeoutTime-time.Now())*1000) << endl;
695 // --- lout << edev(MLog::eGui);
696
697 fTimeoutTime = time.Now() + 3.*fGuardTime/1000.;
698}
699
700void Macs::SetTimeoutTime(LWORD_t ms)
701{
702 // FIXME: Is '/2' the best choose?
703 fGuardTime = ms/3; // how often do we send/request the handshake
704
705 SendSDO(0x4000, 2, ms); // How often do we check for the handshake
706 WaitForSdo(0x4000, 2);
707}
708
Note: See TracBrowser for help on using the repository browser.