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

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