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

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