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

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