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

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