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

Last change on this file since 1958 was 1953, checked in by tbretz, 23 years ago
*** empty log message ***
File size: 18.4 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), fStatusPdo3(0xff)
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 0x1003:
49 // FIXME, see Init
50 if (subidx!=2)
51 return;
52 lout << "- " << GetNodeName() << ": Error[0]=" << dec << val << endl;
53 SetError(val);
54 return;
55
56 case 0x100a:
57 lout << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
58 fSoftVersion = val;
59 return;
60
61 case 0x100b:
62 // Do not display, this is used for CheckConnection
63 // lout << "Node ID: " << dec << val << endl;
64 return;
65
66 case 0x2002:
67 cout << GetNodeName() << ": Current velocity: " << dec << val << endl;
68 fVel = val;
69 return;
70
71 case 0x6004:
72 if (subidx)
73 return;
74
75// lout << "Actual Position: " << dec << (signed long)val << endl;
76 fPos = (LWORDS_t)val;
77 fPosTime.SetTimer(tv);
78 return;
79/*
80 case 0x2001:
81 cout << "Axe Status: 0x" << hex << val << endl;
82 cout << " - Motor " << (val&0x01?"standing":"moving") << endl;
83 cout << " - Positioning " << (val&0x02?"active":"inactive") << endl;
84 cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl;
85 cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
86 cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl;
87 fPosActive = val&0x02;
88 fRpmActive = val&0x04;
89 return;
90 case 0x2003:
91 if (!subidx)
92 {
93 cout << "Input State: ";
94 for (int i=0; i<8; i++)
95 cout << (int)(val&(1<<i)?1:0);
96 cout <<endl;
97 return;
98 }
99 cout << "Input No." << subidx << (val?"hi":"lo") << endl;
100 return;
101 case 0x2004:
102 cout << "Status Value of Axis: 0x" << hex << val << endl;
103 cout << " - Attitude control: " << (val&0x800000?"off":"on") << endl;
104 cout << " - Movement done: " << (val&0x040000?"yes":"no") << endl;
105 cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
106 cout << " - Attitude control: " << (val&0x04?"off":"on") << endl;
107 cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl;
108 cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl;
109 cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl;
110 return;
111*/
112
113 case 0x6002:
114 lout << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " ticks/min" << endl;
115 fVelRes = val;
116 return;
117
118 case 0x6501:
119 lout << "- " << GetNodeName() << ": Encoder resolution = " << dec << val << " ticks/min" << endl;
120 fRes = val;
121 return;
122 }
123 cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
124 cout << ", val=0x"<<val<<endl;
125}
126
127void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
128{
129 switch (idx)
130 {
131 case 0x2002:
132 switch (subidx)
133 {
134 case 0:
135 //lout << ddev(MLog::eGui);
136 lout << "- " << GetNodeName() << ": Velocity set." << endl;
137 //lout << edev(MLog::eGui);
138 return;
139 }
140 break;
141
142 case 0x2003:
143 switch (subidx)
144 {
145 case 0:
146 //lout << ddev(MLog::eGui);
147 lout << "- " << GetNodeName() << ": Acceleration set." << endl;
148 //lout << edev(MLog::eGui);
149 return;
150 case 1:
151 //lout << ddev(MLog::eGui);
152 lout << "- " << GetNodeName() << ": Decceleration set." << endl;
153 //lout << edev(MLog::eGui);
154 return;
155 }
156 break;
157
158 case 0x3006:
159 switch (subidx)
160 {
161 case 0:
162 //lout << ddev(MLog::eGui);
163 lout << "- " << GetNodeName() << ": RPM mode switched." << endl;
164 //lout << edev(MLog::eGui);
165 return;
166
167 case 1:
168 /*
169 lout << ddev(MLog::eGui);
170 lout << "- Velocity set (" << GetNodeName() << ")" << endl;
171 lout << edev(MLog::eGui);
172 */
173 return;
174 }
175 break;
176
177 case 0x4000:
178 HandleNodeguard(tv);
179 return;
180
181 case 0x6004:
182 switch (subidx)
183 {
184 case 0:
185 //lout << ddev(MLog::eGui);
186 lout << "- " << GetNodeName() << ": Absolute positioning started." << endl;
187 //lout << edev(MLog::eGui);
188 return;
189
190 case 1:
191 //lout << ddev(MLog::eGui);
192 lout << "- " << GetNodeName() << ": Relative positioning started." << endl;
193 //lout << edev(MLog::eGui);
194 return;
195 }
196 break;
197
198
199 }
200 NodeDrv::HandleSDOOK(idx, subidx, tv);
201}
202
203
204void Macs::ReqVelRes()
205{
206 lout << "- " << GetNodeName() << ": Requesting velocity resolution (velres, 0x6002)." << endl;
207 RequestSDO(0x6002);
208 WaitForSdo(0x6002);
209}
210
211void Macs::ReqRes()
212{
213 lout << "- " << GetNodeName() << ": Requesting encoder resolution (res, 0x6501)." << endl;
214 RequestSDO(0x6501);
215 WaitForSdo(0x6501);
216}
217
218void Macs::SetPDO1On(BYTE_t flag)
219{
220 lout << "- " << GetNodeName() << ": " << (flag?"Enable":"Disable") << " PDO1." << endl;
221 SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
222 WaitForSdo(0x1800, 1);
223}
224
225void Macs::CheckConnection()
226{
227 RequestSDO(0x100b);
228 WaitForSdo(0x100b);
229}
230
231void Macs::Init()
232{
233 //
234 // Request current error status (FIXME: is the first entry in the
235 // error list)
236 //
237 lout << "- " << GetNodeName() << ": Requesting Error[0]." << endl;
238 RequestSDO(0x1003, 2);
239 WaitForSdo(0x1003, 2);
240 if (HasError())
241 {
242 lout << "Macs::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
243 SetZombie();
244 }
245
246 StopHostGuarding();
247 StopGuarding();
248
249 usleep(2000*GetGuardTime());
250
251 lout << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl;
252 RequestSDO(0x100a);
253 WaitForSdo(0x100a);
254 if (IsZombieNode())
255 return;
256
257 // FIXME! Not statically linked!
258 if (fSoftVersion<0x00000042) // 00.66
259 {
260 lout << GetNodeName() << " - Software Version too old!" << endl;
261 SetZombie();
262 return;
263 }
264
265 SetRpmMode(FALSE);
266
267 ReqRes(); // Init fRes
268 ReqVelRes(); // Init fVelRes
269
270 lout << "- " << GetNodeName() << ": Motor on." << endl;
271 SendSDO(0x3000, string('o', 'n'));
272 WaitForSdo(0x3000);
273
274// SetHome(250000);
275
276// lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
277// RequestSDO(0x2001);
278// WaitForSdo(0x2001);
279
280 SetPDO1On(FALSE); // this is a workaround for the Macs
281 SetPDO1On(TRUE);
282
283 SetNoWait(TRUE);
284
285 // StartGuarding(500, 2);
286 // StartHostGuarding();
287}
288
289void Macs::StopMotor()
290{
291 //
292 // Stop the motor and switch off the position control unit
293 //
294 SendSDO(0x3000, string('s','t','o','p'));
295 WaitForSdo(0x3000);
296}
297
298void Macs::StopDevice()
299{
300 //EnableTimeout(kFALSE);
301
302 SetNoWait(FALSE);
303
304 //
305 // FIXME: This isn't called if the initialization isn't done completely!
306 //
307
308 SetRpmMode(FALSE);
309
310 SetPDO1On(FALSE);
311
312 /*
313 lout << "- " << GetNodeName() << ": Motor off." << endl;
314 SendSDO(0x3000, string('o', 'f', 'f'));
315 WaitForSdo(0x3000);
316 */
317
318 /*
319 lout << "- Stopping Program of " << (int)GetId() << endl;
320 SendSDO(0x4000, (LWORD_t)0xaffe);
321 WaitForSdo();
322 */
323}
324
325void Macs::ReqPos()
326{
327 lout << "- " << GetNodeName() << ": Requesting Position." << endl;
328 RequestSDO(0x6004);
329 WaitForSdo(0x6004);
330}
331
332void Macs::ReqVel()
333{
334 lout << "- " << GetNodeName() << ": Requesting Velocity." << endl;
335 RequestSDO(0x2002);
336 WaitForSdo(0x2002);
337}
338
339void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
340{
341 StopHostGuarding();
342 StopGuarding();
343
344 lout << "- " << GetNodeName() << ": Driving to home position, Offset=" << dec << pos << endl;
345 SendSDO(0x6003, 2, (LWORD_t)pos); // home
346 WaitForSdo(0x6003, 2);
347
348 // home also defines the zero point of the system
349 // maximum time allowd for home drive: 25.000ms
350 SendSDO(0x3001, string('h','o','m','e')); // home
351 WaitForSdo(0x3001, 0, maxtime*1000);
352 lout << "- " << GetNodeName() << ": Home position reached. " << endl;
353
354 SendSDO(0x6003, 0, string('s','e','t')); // home
355 WaitForSdo(0x6003, 0);
356
357 StartGuarding();
358 StartHostGuarding();
359}
360
361void Macs::SetVelocity(LWORD_t vel)
362{
363 SendSDO(0x2002, vel); // velocity
364 WaitForSdo(0x2002, 0);
365}
366
367void Macs::SetAcceleration(LWORD_t acc)
368{
369 SendSDO(0x2003, 0, acc); // acceleration
370 WaitForSdo(0x2003, 0);
371}
372
373void Macs::SetDeceleration(LWORD_t dec)
374{
375 SendSDO(0x2003, 1, dec); // acceleration
376 WaitForSdo(0x2003, 1);
377}
378
379void Macs::SetRpmMode(BYTE_t mode)
380{
381 //
382 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
383 //
384 SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
385 WaitForSdo(0x3006, 0);
386}
387
388void Macs::SetRpmVelocity(LWORDS_t cvel)
389{
390 SendSDO(0x3006, 1, (LWORD_t)cvel);
391 WaitForSdo(0x3006, 1);
392}
393
394void Macs::StartRelPos(LWORDS_t pos)
395{
396 SendSDO(0x6004, 1, (LWORD_t)pos);
397}
398
399void Macs::StartAbsPos(LWORDS_t pos)
400{
401 SendSDO(0x6004, 0, (LWORD_t)pos);
402}
403
404void Macs::SetNoWait(BYTE_t flag)
405{
406 lout << "- " << GetNodeName() << ": Setting NOWAIT " << (flag?"ON":"OFF") << "." << endl;
407 SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f'));
408 WaitForSdo(0x3008);
409}
410
411void Macs::StartVelSync()
412{
413 //
414 // The syncronization mode is disabled by a 'MOTOR STOP'
415 // or by a positioning command (POSA, ...)
416 //
417 lout << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl;
418 SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
419 WaitForSdo(0x3007, 0);
420}
421
422void Macs::StartPosSync()
423{
424 //
425 // The syncronization mode is disabled by a 'MOTOR STOP'
426 // or by a positioning command (POSA, ...)
427 //
428 lout << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl;
429 SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
430 WaitForSdo(0x3007, 1);
431}
432/*
433void Macs::ReqAxEnd()
434{
435 RequestSDO(0x2001);
436 WaitForSdo(0x2001);
437}
438*/
439void Macs::SendMsg(BYTE_t data[6])
440{
441 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
442}
443
444void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
445 BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
446{
447 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
448}
449
450void Macs::HandlePDO1(BYTE_t *data, timeval_t *tv)
451{
452 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
453
454 // data[3]&0x01; // motor not moving
455 fPosActive = data[3]&kPosActive; // positioning active
456 fRpmActive = data[3]&kRpmActive; // RPM mode switched on
457 // data[3]&0x08; // - unused -
458 // data[3]&0x10; // - unused -
459 // data[3]&0x20; // - unused -
460 fInControl = data[3]&0x40; // motor uncontrolled
461 // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
462
463 fStatus = data[3];
464
465 fPdoTime.SetTimer(tv);
466}
467
468void Macs::HandlePDO2(BYTE_t *data, timeval_t *tv)
469{
470 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
471 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
472
473 //
474 // errnum==0 gives a sudden information that something happened. Now the
475 // microcontroller is running inside its interrupt procedure which
476 // stopped the normal program. The interrupt procedure should try to clear
477 // the error state of the hardware. This should never create a new error!
478 //
479 if (!errnum)
480 {
481 lout << "- " << GetNodeName() << ": reports Error occursion." << endl;
482 lout << "Macs::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
483 SetZombie();
484 SetError(-1);
485 return;
486 }
487
488 //
489 // Now the error is handled by the hardware now it is the software part
490 // to react on it. The Error flag now is set to the correct value.
491 //
492 if (GetError()>0)
493 {
494 lout << GetNodeName() << ": WARNING! Error #" << GetError() << " unhandled (not cleared) by software." << endl;
495
496 //
497 // If the error is unhadled and/or not cleared, don't try it again.
498 //
499 if (GetError()==errnum)
500 return;
501 }
502
503 SetError(errnum);
504
505 lout << GetNodeName() << " reports: ";
506 switch (errnum)
507 {
508 case 6:
509 //
510 // Report the error to the user. All possible movements should have
511 // been stopped anyhow. Now delete the error to prevent the system
512 // from reporting this error a thousands of times.
513 //
514 lout << "Home position not the first positioning command." << endl;
515 SetError(0);
516 return;
517
518 case 8:
519 lout << "Control deviation overflow." << endl;
520 return;
521
522 case 9:
523 lout << "Zero index not found." << endl;
524 return;
525
526 case 11:
527 case 25:
528 switch (errinf)
529 {
530 case -1:
531 lout << "Negative";
532 break;
533 case 1:
534 lout << "Positive";
535 break;
536 default:
537 lout << "-unknown-";
538 }
539 switch (errnum)
540 {
541 case 11:
542 lout << " software endswitch activated." << endl;
543 break;
544 case 25:
545 lout << " hardware endswitch activated." << endl;
546 break;
547 }
548 return;
549
550 case 71:
551 lout << "Array out of bound." << endl;
552 return;
553
554 case 84:
555 lout << "Too many (>12) ON TIME calls." << endl;
556 return;
557
558 case 100:
559 //lout << "Connection timed out." << endl;
560 //EnableTimeout(false);
561 return;
562
563 default:
564 lout << "Error Nr. " << errnum << ", " << errinf << endl;
565 }
566}
567
568void Macs::HandlePDO3(BYTE_t *data, timeval_t *tv)
569{
570 // 3 5 7 9
571 // 1100 1010 1110 1001
572 if (fStatusPdo3 == data[3])
573 return;
574
575 lout << GetNodeName() << ": Status PDO3 = ";
576 const Bool_t ready = data[3]&0x01;
577 const Bool_t fuse = data[3]&0x02;
578 const Bool_t emcy = data[3]&0x04;
579 const Bool_t vltg = data[3]&0x08;
580 const Bool_t mode = data[3]&0x10;
581 const Bool_t rf = data[3]&0x20;
582 const Bool_t brake = data[3]&0x40;
583 if (ready) lout << "DKC-Ready ";
584 if (fuse) lout << "FuseOk ";
585 if (emcy) lout << "EmcyOk ";
586 if (vltg) lout << "OvervoltOk ";
587 if (mode) lout << "SwitchToManualMode ";
588 if (rf) lout << "RF ";
589 if (brake) lout << "BrakeOpen ";
590 lout << endl;
591
592 fStatusPdo3 = data[3];
593}
594
595// FIXME? Handling of fIsZombie?
596void Macs::HandleError()
597{
598 //
599 // If there is no error we must not handle anything
600 //
601 if (!HasError())
602 return;
603
604 //
605 // If the program got into the: HandleError state before the hardware
606 // has finished handeling the error we have to wait for the hardware
607 // handeling the error
608 //
609 // FIXME: Timeout???
610 //
611// while (GetError()<0)
612// usleep(1);
613
614 //
615 // After this software and hardware should be in a state so that
616 // we can go on working 'as usual' Eg. Initialize a Display Update
617 //
618 cout << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
619 switch (GetError())
620 {
621 case 6: // home
622 case 8: // control dev
623 case 9: // zero idx
624 case 84: // ON TIME
625 lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
626 return;
627
628 case 11: // software endswitch
629 case 25: // hardware endswitch
630 lout << "- " << GetNodeName() << ": Cannot handle error 'Endswitch!'" << endl;
631 return;
632
633 case 100: // timeout (movement has been stopped, so we can go on)
634 DelError();
635 return;
636/*
637 case 101:
638 //lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
639 DelError();
640 return;
641 */
642 default:
643 lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
644
645 }
646}
647
648double Macs::GetTime()
649{
650 return fPosTime.Now();
651}
652
653double Macs::GetMjd()
654{
655 return fPosTime.GetMjd();
656}
657
658double Macs::GetPdoTime()
659{
660 return fPdoTime.Now();
661}
662
663double Macs::GetPdoMjd()
664{
665 return fPdoTime.GetMjd();
666}
667
668/* 0x2000 0 rw Maximum positioning error */
669/* 1 rw Negative Software Endswitch */
670/* 2 rw Positive Software Endswitch */
671void Macs::SetNegEndswitch(LWORDS_t val)
672{
673 SendSDO(0x2000, 1, (LWORD_t)val);
674 WaitForSdo(0x2000, 1);
675}
676
677void Macs::SetPosEndswitch(LWORDS_t val)
678{
679 SendSDO(0x2000, 2, (LWORD_t)val);
680 WaitForSdo(0x2000, 2);
681}
682
683void Macs::EnableEndswitches(bool neg, bool pos)
684{
685 SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
686 WaitForSdo(0x2000, 3);
687}
688
689void Macs::SendNodeguard()
690{
691 SendSDO(0x4000, 0, (LWORD_t)0, false);
692}
693
694// --------------------------------------------------------------------------
695//
696// This starts the host guarding. The host guarding is only available
697// if the node guarding is running. The host guarding works with the
698// guardtime and the lifetimefactor from the nodeguarding.
699//
700void Macs::StartHostGuarding()
701{
702 SendSDO(0x100c, 0, (LWORD_t)GetGuardTime());
703 WaitForSdo(0x100c);
704
705 SendSDO(0x100d, 0, (LWORD_t)GetLifeTimeFactor());
706 WaitForSdo(0x100d);
707
708 lout << "- " << GetNodeName() << ": Hostguarding started (" << dec;
709 lout << GetLifeTimeFactor() << "*" << GetGuardTime() << "ms)" << endl;
710}
711
712// --------------------------------------------------------------------------
713//
714// Stop the host guarding.
715//
716void Macs::StopHostGuarding()
717{
718 SendSDO(0x100c, 0, (LWORD_t)0);
719 WaitForSdo(0x100c);
720
721 SendSDO(0x100d, 0, (LWORD_t)0);
722 WaitForSdo(0x100d);
723
724 lout << "- " << GetNodeName() << ": Hostguarding stopped." << endl;
725}
726
Note: See TracBrowser for help on using the repository browser.