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

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