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

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