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

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