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

Last change on this file since 8835 was 8835, checked in by tbretz, 18 years ago
*** empty log message ***
File size: 23.8 KB
Line 
1#include "macs.h"
2
3#include <sys/time.h> // timeval->tv_sec
4
5#include "network.h"
6#include "MLogManip.h"
7
8ClassImp(Macs);
9
10using namespace std;
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)
31 : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
32 fPos(0), fPdoPos(0), fPosActive(0), fRpmActive(0),
33 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, const timeval_t &tv)
45{
46 // cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx;
47 // cout << ", val=0x" << val << endl;
48 switch (idx)
49 {
50 case 0x1003:
51 // FIXME, see Init
52 if (subidx!=2)
53 return;
54 gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << dec << val << endl;
55 SetError(val);
56 return;
57
58 case 0x100a:
59 gLog << inf2 << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
60 fSoftVersion = val;
61 return;
62
63 case 0x100b:
64 // Do not display, this is used for CheckConnection
65 // lout << "Node ID: " << dec << val << endl;
66 return;
67
68 case 0x100c:
69 gLog << inf2 << "- " << GetNodeName() << ": Guard time:" << dec << val << endl;
70 return;
71
72 case 0x100d:
73 gLog << inf2 << "- " << GetNodeName() << ": Life time factor:" << dec << val << endl;
74 return;
75
76 case 0x2002:
77 gLog << inf2 << GetNodeName() << ": Current velocity: " << dec << val << endl;
78 fVel = val;
79 return;
80
81 case 0x6004:
82 if (subidx)
83 return;
84
85// lout << "Actual Position: " << dec << (signed long)val << endl;
86 fPos = (LWORDS_t)val;
87 fPosTime.Set(tv);
88 return;
89/*
90 case 0x2001:
91 cout << "Axe Status: 0x" << hex << val << endl;
92 cout << " - Motor " << (val&0x01?"standing":"moving") << endl;
93 cout << " - Positioning " << (val&0x02?"active":"inactive") << endl;
94 cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl;
95 cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
96 cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl;
97 fPosActive = val&0x02;
98 fRpmActive = val&0x04;
99 return;
100 case 0x2003:
101 if (!subidx)
102 {
103 cout << "Input State: ";
104 for (int i=0; i<8; i++)
105 cout << (int)(val&(1<<i)?1:0);
106 cout <<endl;
107 return;
108 }
109 cout << "Input No." << subidx << (val?"hi":"lo") << endl;
110 return;
111 case 0x2004:
112 cout << "Status Value of Axis: 0x" << hex << val << endl;
113 cout << " - Attitude control: " << (val&0x800000?"off":"on") << endl;
114 cout << " - Movement done: " << (val&0x040000?"yes":"no") << endl;
115 cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
116 cout << " - Attitude control: " << (val&0x04?"off":"on") << endl;
117 cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl;
118 cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl;
119 cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl;
120 return;
121*/
122
123 case 0x6002:
124 gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " ticks/min" << endl;
125 fVelRes = val;
126 return;
127
128 case 0x6501:
129 gLog << inf2 << "- " << GetNodeName() << ": Encoder resolution = " << dec << val << " ticks/min" << endl;
130 fRes = val;
131 return;
132 }
133
134 NodeDrv::HandleSDO(idx, subidx, val, tv);
135
136// cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
137// cout << ", val=0x"<<val<<endl;
138}
139
140void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
141{
142 // cout << "Node #" << dec << (int)GetId() << ": Sdo=" << hex << idx << "/" << (int)subidx << " set." << endl;
143
144 // If a real drive operation is requested from the MACS and
145 // the MACS is not correctly initialized the operation is
146 // rejected. (This is expecially harmfull if the NoWait state
147 // is set incorrectly)
148 if (data)
149 SetZombie();
150
151 switch (idx)
152 {
153 case 0x1000:
154 switch (subidx)
155 {
156 case 1:
157 //lout << ddev(MLog::eGui);
158 gLog << inf2 << "- " << GetNodeName() << ": State of node set." << endl;
159 //lout << edev(MLog::eGui);
160 return;
161 }
162 break;
163
164 case 0x100c:
165 switch (subidx)
166 {
167 case 0:
168 //lout << ddev(MLog::eGui);
169 gLog << inf2 << "- " << GetNodeName() << ": Guard time set." << endl;
170 //lout << edev(MLog::eGui);
171 return;
172 }
173 break;
174
175 case 0x100d:
176 switch (subidx)
177 {
178 case 0:
179 //lout << ddev(MLog::eGui);
180 gLog << inf2 << "- " << GetNodeName() << ": Life time factor set." << endl;
181 //lout << edev(MLog::eGui);
182 return;
183 }
184 break;
185
186 case 0x1800:
187 switch (subidx)
188 {
189 case 1:
190 //lout << ddev(MLog::eGui);
191 gLog << inf2 << "- " << GetNodeName() << ": Status of PDO1 set." << endl;
192 //lout << edev(MLog::eGui);
193 return;
194 }
195 break;
196
197 case 0x2002:
198 switch (subidx)
199 {
200 case 0:
201 //lout << ddev(MLog::eGui);
202 gLog << inf2 << "- " << GetNodeName() << ": Velocity set." << endl;
203 //lout << edev(MLog::eGui);
204 return;
205 }
206 break;
207
208 case 0x2003:
209 switch (subidx)
210 {
211 case 0:
212 //lout << ddev(MLog::eGui);
213 gLog << inf2 << "- " << GetNodeName() << ": Acceleration set." << endl;
214 //lout << edev(MLog::eGui);
215 return;
216 case 1:
217 //lout << ddev(MLog::eGui);
218 gLog << inf2 << "- " << GetNodeName() << ": Deceleration set." << endl;
219 //lout << edev(MLog::eGui);
220 return;
221 }
222 break;
223
224 case 0x3006:
225 switch (subidx)
226 {
227 case 0:
228 //lout << ddev(MLog::eGui);
229 gLog << inf2 << "- " << GetNodeName() << ": RPM mode switched." << endl;
230 //lout << edev(MLog::eGui);
231 return;
232
233 case 1:
234 /*
235 lout << ddev(MLog::eGui);
236 lout << "- Velocity set (" << GetNodeName() << ")" << endl;
237 lout << edev(MLog::eGui);
238 */
239 return;
240 }
241 break;
242
243 case 0x4000:
244 HandleNodeguard(tv);
245 return;
246
247 case 0x6000:
248 //lout << ddev(MLog::eGui);
249 gLog << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl;
250 //lout << edev(MLog::eGui);
251 return;
252
253 case 0x6002:
254 //lout << ddev(MLog::eGui);
255 gLog << inf2 << "- " << GetNodeName() << ": Velocitz resolution set." << endl;
256 //lout << edev(MLog::eGui);
257 return;
258
259 case 0x6003:
260 switch (subidx)
261 {
262 case 0:
263 //lout << ddev(MLog::eGui);
264 gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
265 //lout << edev(MLog::eGui);
266 return;
267
268 case 1:
269 //lout << ddev(MLog::eGui);
270 gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
271 //lout << edev(MLog::eGui);
272 return;
273 }
274 break;
275
276 case 0x6004:
277 switch (subidx)
278 {
279 case 0:
280 //lout << ddev(MLog::eGui);
281 gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
282 //lout << edev(MLog::eGui);
283 return;
284
285 case 1:
286 //lout << ddev(MLog::eGui);
287 gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
288 //lout << edev(MLog::eGui);
289 return;
290 }
291 break;
292
293
294 }
295 NodeDrv::HandleSDOOK(idx, subidx, data, tv);
296}
297
298void Macs::ReqVelRes()
299{
300 gLog << inf2 << "- " << GetNodeName() << ": Requesting velocity resolution (velres, 0x6002)." << endl;
301 RequestSDO(0x6002);
302 WaitForSdo(0x6002);
303}
304
305void Macs::ReqRes()
306{
307 gLog << inf2 << "- " << GetNodeName() << ": Requesting encoder resolution (res, 0x6501)." << endl;
308 RequestSDO(0x6501);
309 WaitForSdo(0x6501);
310}
311
312void Macs::SetPDO1On(BYTE_t flag)
313{
314 gLog << inf2 << "- " << GetNodeName() << ": " << (flag?"Enable":"Disable") << " PDO1." << endl;
315 SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
316 WaitForSdo(0x1800, 1);
317}
318
319void Macs::StartNode()
320{
321 //
322 // Switch node from pre-operational state to operational state
323 // (This is not CANOpen compatible)
324 // After this the MACS will react on real movement commands.
325 //
326 gLog << inf2 << "- " << GetNodeName() << ": Starting Node." << endl;
327 SendSDO(0x1000, 1, (LWORD_t)1);
328 WaitForSdo(0x1000, 1);
329}
330
331void Macs::CheckConnection()
332{
333 RequestSDO(0x100b);
334 WaitForSdo(0x100b);
335}
336
337void Macs::Init()
338{
339 //
340 // Request current error status (FIXME: is the first entry in the
341 // error list)
342 //
343 gLog << inf2 << "- " << GetNodeName() << ": Requesting Error[0]." << endl;
344 RequestSDO(0x1003, 2);
345 WaitForSdo(0x1003, 2);
346 if (HasError())
347 {
348 gLog << err << "Macs::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
349 SetZombie();
350 }
351 if (IsZombieNode())
352 return;
353
354 StopHostGuarding();
355 StopGuarding();
356
357 gLog << inf2 << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl;
358 RequestSDO(0x100a);
359 WaitForSdo(0x100a);
360 // FIXME! Not statically linked!
361 //if (fSoftVersion<0x00000044) // 00.68
362 if (fSoftVersion<0x00000045) // 00.69
363 {
364 gLog << err << GetNodeName() << " - Software Version too old!" << endl;
365 SetZombie();
366 return;
367 }
368
369 SetRpmMode(FALSE);
370
371 ReqRes(); // Init fRes
372 ReqVelRes(); // Init fVelRes
373
374 /* Should not be necessary anymore. This is done by the MACS itself.
375 lout << "- " << GetNodeName() << ": Motor on." << endl;
376 SendSDO(0x3000, string('o', 'n'));
377 WaitForSdo(0x3000);
378 */
379
380 SetPDO1On(FALSE); // this is a workaround for the Macs
381 SetPDO1On(TRUE);
382
383 //This is now standard in the MACS
384 //SetNoWait(TRUE);
385
386 //StartGuarding(400, 1, kFALSE); // Using PDO1 @ 100ms
387 //StartGuarding(250, 4);
388 //StartHostGuarding();
389
390 StartNode();
391}
392/*
393void Macs::StopMotor()
394{
395 //
396 // Stop the motor and switch off the position control unit
397 //
398 SendSDO(0x3000, string('s','t','o','p'));
399 WaitForSdo(0x3000);
400}
401*/
402void Macs::StopDevice()
403{
404 //EnableTimeout(kFALSE);
405
406 //No need to switch it off.
407 //SetNoWait(FALSE);
408
409 StopHostGuarding();
410 StopGuarding();
411
412 //
413 // FIXME: This isn't called if the initialization isn't done completely!
414 //
415
416 SetRpmMode(FALSE);
417
418 SetPDO1On(FALSE);
419
420 /*
421 lout << "- " << GetNodeName() << ": Motor off." << endl;
422 SendSDO(0x3000, string('o', 'f', 'f'));
423 WaitForSdo(0x3000);
424 */
425
426 /*
427 lout << "- Stopping Program of " << (int)GetId() << endl;
428 SendSDO(0x4000, (LWORD_t)0xaffe);
429 WaitForSdo();
430 */
431}
432
433void Macs::ReqPos()
434{
435 gLog << inf2 << "- " << GetNodeName() << ": Requesting Position." << endl;
436 RequestSDO(0x6004);
437 WaitForSdo(0x6004);
438}
439
440void Macs::ReqVel()
441{
442 gLog << inf2 << "- " << GetNodeName() << ": Requesting Velocity." << endl;
443 RequestSDO(0x2002);
444 WaitForSdo(0x2002);
445}
446/*
447void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
448{
449 StopHostGuarding();
450 StopGuarding();
451
452 lout << "- " << GetNodeName() << ": Driving to home position, Offset=" << dec << pos << endl;
453 SendSDO(0x6003, 2, (LWORD_t)pos); // home
454 WaitForSdo(0x6003, 2);
455
456 // home also defines the zero point of the system
457 // maximum time allowd for home drive: 25.000ms
458 SendSDO(0x3001, string('h','o','m','e')); // home
459 WaitForSdo(0x3001, 0, maxtime*1000);
460 lout << "- " << GetNodeName() << ": Home position reached. " << endl;
461
462 SendSDO(0x6003, 0, string('s','e','t')); // home
463 WaitForSdo(0x6003, 0);
464
465 StartGuarding();
466 StartHostGuarding();
467}
468*/
469void Macs::SetVelocity(LWORD_t vel)
470{
471 SendSDO(0x2002, vel); // velocity
472 WaitForSdo(0x2002, 0);
473}
474
475void Macs::SetAcceleration(LWORD_t acc)
476{
477 SendSDO(0x2003, 0, acc); // acceleration
478 WaitForSdo(0x2003, 0);
479}
480
481void Macs::SetDeceleration(LWORD_t dec)
482{
483 SendSDO(0x2003, 1, dec); // acceleration
484 WaitForSdo(0x2003, 1);
485}
486
487void Macs::SetRpmMode(BYTE_t mode)
488{
489 //
490 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
491 //
492 SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
493 WaitForSdo(0x3006, 0);
494}
495
496void Macs::SetRpmVelocity(LWORDS_t cvel)
497{
498 SendSDO(0x3006, 1, (LWORD_t)cvel);
499 WaitForSdo(0x3006, 1);
500}
501
502void Macs::StartRelPos(LWORDS_t pos)
503{
504 SendSDO(0x6004, 1, (LWORD_t)pos);
505}
506
507void Macs::StartAbsPos(LWORDS_t pos)
508{
509 SendSDO(0x6004, 0, (LWORD_t)pos);
510}
511
512/*
513void Macs::SetNoWait(BYTE_t flag)
514{
515 lout << "- " << GetNodeName() << ": Setting NOWAIT " << (flag?"ON":"OFF") << "." << endl;
516 SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f'));
517 WaitForSdo(0x3008);
518}
519*/
520
521void Macs::StartVelSync()
522{
523 //
524 // The syncronization mode is disabled by a 'MOTOR STOP'
525 // or by a positioning command (POSA, ...)
526 //
527 gLog << inf2 << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl;
528 SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
529 WaitForSdo(0x3007, 0);
530}
531
532void Macs::StartPosSync()
533{
534 //
535 // The syncronization mode is disabled by a 'MOTOR STOP'
536 // or by a positioning command (POSA, ...)
537 //
538 gLog << inf2 << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl;
539 SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
540 WaitForSdo(0x3007, 1);
541}
542/*
543void Macs::ReqAxEnd()
544{
545 RequestSDO(0x2001);
546 WaitForSdo(0x2001);
547}
548*/
549void Macs::SendMsg(BYTE_t data[6])
550{
551 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
552}
553
554void Macs::SendMsg(BYTE_t d0, BYTE_t d1, BYTE_t d2,
555 BYTE_t d3, BYTE_t d4, BYTE_t d5)
556{
557 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
558}
559
560void Macs::HandlePDO1(const BYTE_t *data, const timeval_t &tv)
561{
562 // FIXME!!!! Only 0x4000 should do this to be
563 // CanOpen conform
564 HandleNodeguard(tv);
565
566 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
567
568 // data[3]&0x01; // motor not moving
569 fPosActive = data[3]&kPosActive; // positioning active
570 fRpmActive = data[3]&kRpmActive; // RPM mode switched on
571 // data[3]&0x08; // - unused -
572 // data[3]&0x10; // - unused -
573 // data[3]&0x20; // - unused -
574 fInControl = data[3]&0x40; // motor uncontrolled
575 // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
576
577 fStatus = data[3];
578
579 fPdoTime.Set(tv);
580}
581
582void Macs::HandlePDO2(const BYTE_t *data, const timeval_t &tv)
583{
584 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
585 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
586
587 //
588 // errnum==0 gives a sudden information that something happened. Now the
589 // microcontroller is running inside its interrupt procedure which
590 // stopped the normal program. The interrupt procedure should try to clear
591 // the error state of the hardware. This should never create a new error!
592 //
593 if (!errnum)
594 {
595 gLog << err << "- " << GetNodeName() << ": reports Error occursion." << endl;
596 gLog << "Macs::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
597 SetZombie();
598 SetError(-1);
599 return;
600 }
601
602 //
603 // Now the error is handled by the hardware now it is the software part
604 // to react on it. The Error flag now is set to the correct value.
605 //
606 if (GetError()>0)
607 {
608 gLog << warn << GetNodeName() << ": WARNING! Error #" << GetError() << " unhandled (not cleared) by software." << endl;
609
610 //
611 // If the error is unhadled and/or not cleared, don't try it again.
612 //
613 if (GetError()==errnum)
614 return;
615 }
616
617 SetError(errnum);
618
619 gLog << err << GetNodeName() << " reports: ";
620 switch (errnum)
621 {
622 case 3:
623 gLog << "Axis does not existing." << endl;
624 return;
625 case 5:
626 gLog << "Error not cleared (while trying to move axis)" << endl;
627 return;
628 case 6:
629 //
630 // Report the error to the user. All possible movements should have
631 // been stopped anyhow. Now delete the error to prevent the system
632 // from reporting this error a thousands of times.
633 //
634 gLog << "Home position not the first positioning command." << endl;
635 SetError(0);
636 return;
637 case 8:
638 gLog << "Control deviation overflow." << endl;
639 return;
640 case 9:
641 gLog << "Zero index not found." << endl;
642 return;
643 case 10:
644 gLog << "Unknown command, syntax error." << endl;
645 gLog << "Please recompile and reload program." << endl;
646 return;
647 case 11:
648 case 25:
649 switch (errinf)
650 {
651 case -1:
652 gLog << "Negative";
653 break;
654 case 1:
655 gLog << "Positive";
656 break;
657 default:
658 gLog << "-unknown-";
659 }
660 switch (errnum)
661 {
662 case 11:
663 gLog << " software";
664 break;
665 case 25:
666 gLog << " hardware";
667 break;
668 }
669 gLog << " endswitch activated." << endl;
670 return;
671 case 12:
672 gLog << "Wrong parameter number used in SET command." << endl;
673 return;
674 case 14:
675 gLog << " Too many LOOP calls." << endl;
676 return;
677 case 16:
678 gLog << "Parameter in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl;
679 gLog << "Please use APOSS to 'Reset' the MACS and reload the parameters." << endl;
680 return;
681 case 17:
682 gLog << "Program in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl;
683 gLog << "Please use APOSS to delete all Programs restore the programs." << endl;
684 return;
685 case 18:
686 gLog << "Reset by CPU (reset called by Watch-dog cause of CPU halted)" << endl;
687 gLog << "Possible reasons: short under-/overvoltage or shortcut." << endl;
688 return;
689 case 19:
690 gLog << "User break (autostart program stopped by user)" << endl;
691 return;
692 case 51:
693 gLog << "Too many (>=10) GOSUB calls." << endl;
694 return;
695 case 52:
696 gLog << "Too many RETURN calls." << endl;
697 return;
698 case 62:
699 gLog << "Error veryfiing EEPROM after access (Try again savaing parameters or program)" << endl;
700 return;
701 case 70:
702 gLog << "Error in DIM call (call to DIM doesn't fit existing DIM call)" << endl;
703 return;
704 case 71:
705 gLog << "Array out of bound." << endl;
706 return;
707 case 79:
708 gLog << "Timeout waiting for index (WAITNDX)." << endl;
709 return;
710 case 84:
711 gLog << "Too many (>12) ON TIME calls." << endl;
712 return;
713 case 87:
714 gLog << "Out of memory for variables - Check APOSS predifined number of" << endl;
715 gLog << "variables and try deleting the array by doing a 'Reset' from APOSS." << endl;
716 return;
717 case 89:
718 gLog << "CAN I/O error (REOPEN=" << dec << errinf << " " << (errinf==0?"OK":"ERR") << ")" << endl;
719 return;
720
721 case 100:
722 //lout << "Connection timed out." << endl;
723 //EnableTimeout(false);
724 return;
725
726 default:
727 gLog << "Error Nr. " << errnum << ", " << errinf << endl;
728 }
729}
730
731void Macs::HandlePDO3(const BYTE_t *data, const timeval_t &tv)
732{
733 // 3 5 7 9
734 // 1100 1010 1110 1001
735 if (fStatusPdo3 == data[3])
736 return;
737
738 MTime time;
739 time.Now();
740
741 gLog << inf << time << ": " << GetNodeName() << " - PDO3 = ";
742 const Bool_t ready = data[3]&0x01;
743 const Bool_t fuse = data[3]&0x02;
744 const Bool_t emcy = data[3]&0x04;
745 const Bool_t vltg = data[3]&0x08;
746 const Bool_t mode = data[3]&0x10;
747 const Bool_t rf = data[3]&0x20;
748 const Bool_t brake = data[3]&0x40;
749 if (ready) gLog << "DKC-Ready ";
750 if (fuse) gLog << "FuseOk ";
751 if (emcy) gLog << "EmcyOk ";
752 if (vltg) gLog << "OvervoltOk ";
753 if (mode) gLog << "SwitchToManualMode ";
754 if (rf) gLog << "RF ";
755 if (brake) gLog << "BrakeOpen ";
756 gLog << endl;
757
758 fStatusPdo3 = data[3];
759}
760
761// FIXME? Handling of fIsZombie?
762void Macs::HandleError()
763{
764 //
765 // If there is no error we must not handle anything
766 //
767 if (!HasError())
768 return;
769
770 //
771 // If the program got into the: HandleError state before the hardware
772 // has finished handeling the error we have to wait for the hardware
773 // handeling the error
774 //
775 // FIXME: Timeout???
776 //
777// while (GetError()<0)
778// usleep(1);
779
780 //
781 // After this software and hardware should be in a state so that
782 // we can go on working 'as usual' Eg. Initialize a Display Update
783 //
784 gLog << inf << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
785 switch (GetError())
786 {
787 case 6: // home
788 case 8: // control dev
789 case 9: // zero idx
790 case 84: // ON TIME
791 gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
792 return;
793
794 case 11: // software endswitch
795 case 25: // hardware endswitch
796 gLog << "- " << GetNodeName() << ": Cannot handle error 'Endswitch!'" << endl;
797 return;
798
799 case 100: // timeout (movement has been stopped, so we can go on)
800 DelError();
801 return;
802/*
803 case 101:
804 //lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
805 DelError();
806 return;
807 */
808 default:
809 gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
810
811 }
812}
813
814/* 0x2000 0 rw Maximum positioning error */
815/* 1 rw Negative Software Endswitch */
816/* 2 rw Positive Software Endswitch */
817void Macs::SetNegEndswitch(LWORDS_t val)
818{
819 SendSDO(0x2000, 1, (LWORD_t)val);
820 WaitForSdo(0x2000, 1);
821}
822
823void Macs::SetPosEndswitch(LWORDS_t val)
824{
825 SendSDO(0x2000, 2, (LWORD_t)val);
826 WaitForSdo(0x2000, 2);
827}
828
829void Macs::EnableEndswitches(bool neg, bool pos)
830{
831 SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
832 WaitForSdo(0x2000, 3);
833}
834
835void Macs::SendNodeguard()
836{
837 SendSDO(0x4000, 0, (LWORD_t)0, false);
838}
839
840// --------------------------------------------------------------------------
841//
842// This starts the host guarding. The host guarding is only available
843// if the node guarding is running. The host guarding works with the
844// guardtime and the lifetimefactor from the nodeguarding.
845//
846void Macs::StartHostGuarding()
847{
848 SendSDO(0x100c, 0, (LWORD_t)GetGuardTime());
849 WaitForSdo(0x100c);
850
851 SendSDO(0x100d, 0, (LWORD_t)GetLifeTimeFactor());
852 WaitForSdo(0x100d);
853
854 gLog << inf2 << "- " << GetNodeName() << ": Hostguarding started (" << dec;
855 gLog << GetLifeTimeFactor() << "*" << GetGuardTime() << "ms)" << endl;
856}
857
858// --------------------------------------------------------------------------
859//
860// Stop the host guarding.
861//
862void Macs::StopHostGuarding()
863{
864 SendSDO(0x100c, 0, (LWORD_t)0);
865 WaitForSdo(0x100c);
866
867 SendSDO(0x100d, 0, (LWORD_t)0);
868 WaitForSdo(0x100d);
869
870 gLog << inf2 << "- " << GetNodeName() << ": Hostguarding stopped." << endl;
871}
872
Note: See TracBrowser for help on using the repository browser.