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

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