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

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