source: trunk/MagicSoft/Cosy/devdrv/dkc.cc@ 9439

Last change on this file since 9439 was 9439, checked in by tbretz, 16 years ago
*** empty log message ***
File size: 20.1 KB
Line 
1#include "dkc.h"
2
3#include <sys/time.h> // timeval->tv_sec
4
5#include <TGLabel.h>
6
7#include "network.h"
8
9#include "MLogManip.h"
10
11#include "MString.h"
12
13ClassImp(Dkc);
14
15using namespace std;
16
17#define EXPERT
18
19Dkc::Dkc(const BYTE_t nodeid, const char *name)
20 : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
21 fPdoPos1(0), fPdoPos2(0), fPosActive(0), fRpmActive(0),
22 fStatus(0), fStatusDKC(0), fStatusPdo3(0xff), fArmed(false),
23 fReport(NULL),fLabel(NULL),fUpdPos(0)
24{
25}
26
27TString Dkc::EvalStatusDKC(UInt_t stat) const
28{
29 switch (stat)
30 {
31 case 0: return "offline";
32 case 0xa000:
33 case 0xa001:
34 case 0xa002:
35 case 0xa003: return MString::Format("Communication phase %d", stat&0xf);
36 case 0xa010: return "Drive HALT";
37 case 0xa012: return "Control and power section ready for operation";
38 case 0xa013: return "Ready for power on";
39 case 0xa100: return "Drive in Torque mode";
40 case 0xa101: return "Drive in Velocity mode";
41 case 0xa102: return "Position control mode with encoder 1";
42 case 0xa103: return "Position control mode with encoder 2";
43 case 0xa104: return "Position control mode with encoder 1, lagless";
44 case 0xa105: return "Position control mode with encoder 2, lagless";
45 case 0xa106: return "Drive controlled interpolated positioning with encoder 1";
46 case 0xa107: return "Drive controlled interpolated positioning with encoder 2";
47 case 0xa108: return "Drive controlled interpolated positioning with encoder 1, lagless";
48 case 0xa109: return "Drive controlled interpolated positioning with encoder 2, lagless";
49 //case 0xa146: return "Drive controlled interpolated relative positioning with encoder 1";
50 //case 0xa147: return "Drive controlled interpolated relative positioning with encoder 2";
51 //case 0xa148: return "Drive controlled interpolated relative positioning lagless with encoder 1";
52 //case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2";
53 case 0xa150: return "Drive controlled positioning with encoder 1";
54 case 0xa151: return "Drive controlled positioning with encoder 1, lagless";
55 case 0xa152: return "Drive controlled positioning with encoder 2";
56 case 0xa153: return "Drive controlled positioning with encoder 2, lagless";
57 case 0xa208: return "Jog mode positive";
58 case 0xa218: return "Jog mode negative";
59 case 0xa400: return "Automatic drive check and adjustment";
60 case 0xa401: return "Drive decelerating to standstill";
61 case 0xa800: return "Unknown operation mode";
62 case 0xc217: return "Motor encoder reading error";
63 case 0xc218: return "Shaft encoder reading error";
64 case 0xc220: return "Motor encoder initialization error";
65 case 0xc221: return "Shaft encoder initialization error";
66 case 0xc300: return "Command: set absolute measure";
67 case 0xc400: return "Switching to parameter mode";
68 case 0xc401: return "Drive active, switching mode not allowed";
69 case 0xc500: return "Error reset";
70 case 0xc600: return "Drive controlled homi*ng procedure ";
71 case 0xe225: return "Motor overload";
72 case 0xe249: return "Positioning command velocity exceeds limit bipolar";
73 case 0xe250: return "Drive overtemp warning";
74 case 0xe251: return "Motor overtemp warning";
75 case 0xe252: return "Bleeder overtemp warning";
76 case 0xe257: return "Continous current limit active";
77 case 0xe259: return "Command velocity limit active";
78 case 0xe264: return "Target position out of numerical range";
79 case 0xe829: return "Positive position limit exceeded";
80 case 0xe830: return "Negative position limit exceeded";
81 case 0xe831: return "Position limit reached during jog";
82 case 0xe834: return "Emergency-Stop";
83 case 0xe842: return "Both end-switches activated";
84 case 0xe843: return "Positive end-switch activated";
85 case 0xe844: return "Negative end-switch activated";
86 case 0xf218: return "Amplifier overtemp shutdown";
87 case 0xf219: return "Motor overtemp shutdown";
88 case 0xf220: return "Bleeder overload shutdown";
89 case 0xf221: return "Motor temperature surveillance defective";
90 case 0xf224: return "Maximum breaking time exceeded";
91 case 0xf228: return "Excessive control deviation";
92 case 0xf250: return "Overflow of target position preset memory";
93 case 0xf269: return "Error during release of the motor holding brake";
94 case 0xf276: return "Absolute encoder out of allowed window";
95 case 0xf409: return "Bus error on Profibus interface";
96 case 0xf434: return "Emergency-Stop";
97 case 0xf629: return "Positive position limit exceeded";
98 case 0xf630: return "Negative position limit exceeded";
99 case 0xf634: return "Emergency-Stop";
100 case 0xf643: return "Positive end-switch activated";
101 case 0xf644: return "Negative end-switch activated";
102 case 0xf870: return "24V DC error";
103 case 0xf878: return "Velocity loop error";
104 }
105 return "unknown";
106}
107
108TString Dkc::GetStatus(LWORD_t val) const
109{
110 const Int_t errnum = val&0xffff;
111 const Int_t errinf = val>>16;
112
113 if (errnum!=0xff)
114 return "";
115
116 // DKC offline. This is a fatal error
117 if (errinf==0)
118 return "offline.";
119
120 TString str;
121
122 const Int_t type = errinf&0xf000;
123 switch (type)
124 {
125 case 0xf000: str += "ERROR"; break;
126 case 0xe000: str += "WARNING"; break;
127 case 0xa000: str += "Status"; break;
128 case 0xc000:
129 case 0xd000: str += "Message"; break;
130 default: str += "Unknown"; break;
131 }
132
133 str += " (";
134 str += MString::Format("%X", errinf);
135 str += "): ";
136 str += EvalStatusDKC(errinf);
137 str += (type==0xf000 || type==0xe000 ? "!" : ".");
138
139 return str;
140}
141
142Bool_t Dkc::PrintStatus(LWORD_t val) const
143{
144 const Int_t errnum = val&0xffff;
145 const Int_t errinf = val>>16;
146
147 if (errnum!=0xff)
148 return errnum==0;
149
150 gLog << all << MTime(-1) << ": " << GetNodeName() << " " << GetStatus(val) << endl;
151
152 // errinf==0: DKC offline. This is a fatal error
153 return errinf==0 ? kFALSE : (errinf&0xf000)!=0xf000;
154}
155
156void Dkc::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv)
157{
158 // cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx;
159 // cout << ", val=0x" << val << endl;
160 switch (idx)
161 {
162 case 0x1000:
163 gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl;
164 fArmed = val==1;
165 return;
166
167 case 0x1003:
168 // FIXME, see Init
169 gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << hex << val << dec << endl;
170 CheckErrorDKC(val);
171 return;
172
173 case 0x100a:
174 gLog << inf2 << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
175 fSoftVersion = val;
176 return;
177
178 case 0x100b:
179 // Do not display, this is used for CheckConnection
180 // lout << "Node ID: " << dec << val << endl;
181 return;
182
183 case 0x2002:
184 gLog << inf2 << GetNodeName() << ": Current velocity: " << dec << val << endl;
185 fVel = val;
186 return;
187
188 case 0x6004:
189 fPdoPos1 = (LWORDS_t)val;
190 fPdoTime1.Set(tv);
191 fHasChangedPos1 = true;
192 return;
193
194 case 0x6005:
195 fPdoPos2 = (LWORDS_t)val;
196 fPdoTime2.Set(tv);
197 fHasChangedPos2 = true;
198 return;
199
200 case 0x6002:
201 gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " (1rpm)" << endl;
202 fVelRes = val;
203 return;
204
205 case 0x6003:
206 gLog << inf2 << "- " << GetNodeName() << ": Maximum velocity = " << dec << val << " (100%)" << endl;
207 fVelMax = val;
208 return;
209
210 case 0x6501:
211 gLog << inf2 << "- " << GetNodeName() << ": Position resolution = " << dec << val << " ticks/revolution" << endl;
212 fPosRes = val;
213 return;
214 }
215
216 NodeDrv::HandleSDO(idx, subidx, val, tv);
217
218 // cout << "Dkc: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
219 // cout << ", val=0x"<<val<<endl;
220}
221
222void Dkc::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
223{
224 // cout << "Node #" << dec << (int)GetId() << ": Sdo=" << hex << idx << "/" << (int)subidx << " set." << endl;
225
226 // If a real drive operation is requested from the MACS and
227 // the MACS is not correctly initialized the operation is
228 // rejected. (This is expecially harmfull if the NoWait state
229 // is set incorrectly)
230 if (data)
231 SetZombie();
232
233 switch (idx)
234 {
235 case 0x1000:
236 gLog << inf2 << "- " << GetNodeName() << ": State of node set." << endl;
237 return;
238
239 case 0x1001:
240 gLog << inf2 << "- " << GetNodeName() << ": PDOs requested." << endl;
241 return;
242
243 case 0x2002:
244 gLog << inf2 << "- " << GetNodeName() << ": Velocity set." << endl;
245 return;
246
247 case 0x2003:
248 gLog << inf2 << "- " << GetNodeName() << ": Acceleration set." << endl;
249 return;
250
251 case 0x3006:
252 gLog << inf2 << "- " << GetNodeName() << ": RPM mode switched." << endl;
253 return;
254
255 case 0x3007:
256 //gLog << inf2 << "- Velocity set (" << GetNodeName() << ")" << endl;
257 return;
258
259 case 0x4000:
260 HandleNodeguard(tv);
261 return;
262
263 case 0x6000:
264 gLog << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl;
265 return;
266
267 case 0x6002:
268 gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution set." << endl;
269 return;
270
271 case 0x6004:
272 gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
273 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
274 return;
275
276 case 0x6005:
277 gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
278 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
279 return;
280 }
281
282 NodeDrv::HandleSDOOK(idx, subidx, data, tv);
283}
284
285void Dkc::ReqVelRes()
286{
287 gLog << inf2 << "- " << GetNodeName() << ": Requesting velocity resolution (velres, 0x6002)." << endl;
288 RequestSDO(0x6002);
289 WaitForSdo(0x6002);
290}
291
292void Dkc::ReqVelMax()
293{
294 gLog << inf2 << "- " << GetNodeName() << ": Requesting maximum velocity (velmax, 0x6003)." << endl;
295 RequestSDO(0x6003);
296 WaitForSdo(0x6003);
297}
298
299void Dkc::ReqPosRes()
300{
301 gLog << inf2 << "- " << GetNodeName() << ": Requesting position resolution (posres, 0x6501)." << endl;
302 RequestSDO(0x6501);
303 WaitForSdo(0x6501);
304}
305
306void Dkc::Arm()
307{
308 gLog << inf2 << "- " << GetNodeName() << ": Arming node." << endl;
309 SendSDO(0x1000, (LWORD_t)1);
310 WaitForSdo(0x1000);
311}
312
313void Dkc::Disarm()
314{
315 gLog << inf2 << "- " << GetNodeName() << ": Disarming Node." << endl;
316 SendSDO(0x1000, (LWORD_t)0);
317 WaitForSdo(0x1000);
318}
319
320void Dkc::CheckConnection()
321{
322 RequestSDO(0x100b);
323 WaitForSdo(0x100b);
324}
325
326void Dkc::Init()
327{
328 //
329 // Request current error status (FIXME: is the first entry in the
330 // error list)
331 //
332 gLog << inf2 << "- " << GetNodeName() << ": Requesting Error[0]." << endl;
333 RequestSDO(0x1003);
334 WaitForSdo(0x1003);
335
336 /*
337 if (HasError())
338 {
339 gLog << err << "Dkc::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
340 SetZombie();
341 }
342 if (IsZombieNode())
343 return;
344 */
345
346 SetRpmMode(FALSE);
347
348 ReqPosRes(); // Init fVelRes
349 ReqVelRes(); // Init fVelResVelRes
350 ReqVelMax(); // Init fVelMax
351
352 // Request to send all PDOs at least once
353 ReqPDOs();
354
355#ifdef EXPERT
356 Arm();
357#endif
358
359 gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl;
360 RequestSDO(0x1000);
361 WaitForSdo(0x1000);
362}
363
364void Dkc::StopDevice()
365{
366 //
367 // FIXME: This isn't called if the initialization isn't done completely!
368 //
369
370 SetRpmMode(FALSE);
371}
372
373void Dkc::ReqPDOs()
374{
375 gLog << inf2 << "- " << GetNodeName() << ": Requesting all PDOs." << endl;
376
377 SendSDO(0x1001, (LWORD_t)1);
378 WaitForSdo(0x1001);
379}
380
381void Dkc::ReqPos1()
382{
383 gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 1." << endl;
384 RequestSDO(0x6004);
385 WaitForSdo(0x6004);
386}
387
388void Dkc::ReqPos2()
389{
390 gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 2." << endl;
391 RequestSDO(0x6005);
392 WaitForSdo(0x6005);
393}
394
395void Dkc::ReqVel()
396{
397 gLog << inf2 << "- " << GetNodeName() << ": Requesting Velocity." << endl;
398 RequestSDO(0x2002);
399 WaitForSdo(0x2002);
400}
401
402void Dkc::SetVelocity(LWORD_t vel)
403{
404 gLog << dbg << "- Setting velocity to: " << vel << endl;
405 SendSDO(0x2002, vel); // velocity
406 WaitForSdo(0x2002);
407}
408
409void Dkc::SetVelocityRel(Double_t vel)
410{
411 gLog << dbg << "- Setting velocity to: " << vel*100 << "%" << endl;
412 SendSDO(0x2002, (LWORD_t)(vel*fVelMax+0.5)); // velocity
413 WaitForSdo(0x2002);
414}
415
416void Dkc::SetAcceleration(LWORD_t acc)
417{
418 gLog << dbg << "- Setting acceleration to: " << acc << endl;
419 SendSDO(0x2003, acc); // acceleration
420 WaitForSdo(0x2003);
421}
422
423void Dkc::SetRpmMode(BYTE_t mode)
424{
425 //
426 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
427 //
428 SendSDO(0x3006, mode ? string('s','t','r','t') : string('s','t','o','p'));
429 WaitForSdo(0x3006);
430}
431
432void Dkc::SetRpmVelocity(LWORDS_t cvel)
433{
434 SendSDO(0x3007, (LWORD_t)cvel);
435 WaitForSdo(0x3007);
436}
437
438void Dkc::StartRelPos(LWORDS_t pos)
439{
440 if (!fArmed)
441 {
442 gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
443 SetZombie();
444 return;
445 }
446
447 gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl;
448 SendSDO(0x6005, (LWORD_t)pos);
449 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
450}
451
452void Dkc::StartAbsPos(LWORDS_t pos)
453{
454 if (!fArmed)
455 {
456 gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
457 SetZombie();
458 return;
459 }
460
461 gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (LWORDS_t)pos << " ticks." << endl;
462 SendSDO(0x6004, (LWORD_t)pos);
463 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
464}
465
466void Dkc::StartAbsPosRev(Double_t pos)
467{
468 if (!fArmed)
469 {
470 gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl;
471 SetZombie();
472 return;
473 }
474
475 const LWORD_t p = (LWORD_t)(pos*fPosRes+.5);
476
477 gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << p << " ticks." << endl;
478 SendSDO(0x6004, p);
479 fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
480}
481
482void Dkc::SendMsg(BYTE_t data[6])
483{
484 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
485}
486
487void Dkc::SendMsg(BYTE_t d0, BYTE_t d1, BYTE_t d2,
488 BYTE_t d3, BYTE_t d4, BYTE_t d5)
489{
490 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
491}
492
493void Dkc::HandlePDO1(const BYTE_t *data, const timeval_t &tv)
494{
495 // FIXME!!!! Only 0x4000 should do this to be
496 // CanOpen conform
497 //HandleNodeguard(tv);
498
499 fPdoPos1 = (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0];
500 fPdoTime1.Set(tv);
501 fHasChangedPos1 = true;
502
503 fPdoPos2 = (data[7]<<24) | (data[6]<<16) | (data[5]<<8) | data[4];
504 fPdoTime2.Set(tv);
505 fHasChangedPos2 = true;
506
507 if (fReport)
508 {
509 fReport->Lock("ShaftEncoder::HandlePDOType0");
510 *fReport << "FEEDBACK " << (int)GetId() << " " << fPdoTime1 << " PDO0 " << GetNodeName() << " " << fPdoPos1 << " " << fPdoPos2 << endl;
511 fReport->UnLock("ShaftEncoder::HandlePDOType0");
512 }
513}
514
515void Dkc::HandlePDO3(const BYTE_t *data, const timeval_t &tv)
516{
517 fArmed = data[2]&0x01;
518
519 if (fStatus!=data[3])
520 {
521 // data[3]&0x01; // motor not moving
522 fPosActive = data[3]&0x02; // positioning active
523 fRpmActive = data[3]&0x04; // RPM mode switched on
524 // data[3]&0x08; // - unused -
525 // data[3]&0x10; // - unused -
526 // data[3]&0x20; // - unused -
527 //fInControl = data[3]&0x40; // motor uncontrolled
528 // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
529
530 fStatus = data[3];
531 }
532
533 if (fStatusPdo3!=data[0])
534 {
535 gLog << inf << MTime(-1) << ": " << GetNodeName() << " - PDO1 = ";
536 const Bool_t ready = data[0]&0x01;
537 const Bool_t fuse = data[0]&0x02;
538 const Bool_t emcy = data[0]&0x04;
539 const Bool_t vltg = data[0]&0x08;
540 const Bool_t mode = data[0]&0x10;
541 const Bool_t rf = data[0]&0x20;
542 const Bool_t brake = data[0]&0x40;
543 const Bool_t power = data[0]&0x80;
544 if (ready) gLog << "DKC-Ready ";
545 if (fuse) gLog << "FuseOk ";
546 if (emcy) gLog << "EmcyOk ";
547 if (vltg) gLog << "OvervoltOk ";
548 if (mode) gLog << "SwitchToManualMode ";
549 if (rf) gLog << "RF ";
550 if (brake) gLog << "BrakeOpen ";
551 if (power) gLog << "PowerOn ";
552 gLog << endl;
553
554 fStatusPdo3 = data[0];
555 }
556}
557
558void Dkc::CheckErrorDKC(LWORD_t val)
559{
560 fStatusDKC = val;
561
562 const Bool_t rc = PrintStatus(val);
563 SetError(rc ? 0 : val);
564 if (!rc)
565 SetZombie();
566}
567
568void Dkc::HandlePDO2(const BYTE_t *data, const timeval_t &tv)
569{
570 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
571 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
572
573 // Check if the DKC changed its status message
574 if (errnum==0xff && (errinf&0xf000)<=0xe000)
575 {
576 CheckErrorDKC(errnum, errinf);
577 return;
578 }
579
580 // Check if MACS report error occursion.
581 // errnum==0 gives a sudden information that something happened. Now the
582 // microcontroller is running inside its interrupt procedure which
583 // stopped the normal program. The interrupt procedure should try to clear
584 // the error state of the hardware. This should never create a new error!
585 //
586 if (!errnum)
587 {
588 gLog << err << "- " << GetNodeName() << ": reports Error occursion." << endl;
589 gLog << "Dkc::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
590 SetZombie();
591 SetError(-1);
592 return;
593 }
594
595 //
596 // Now the error is handled by the hardware now it is the software part
597 // to react on it. The Error flag now is set to the correct value.
598 //
599 if (GetError()>0)
600 {
601 gLog << warn << GetNodeName() << ": WARNING! Previous error #" << GetError() << " unhandled (not cleared) by software." << endl;
602
603 //
604 // If the error is unhadled and/or not cleared, don't try it again.
605 //
606 if (GetError()==errnum)
607 return;
608 }
609
610 SetError(errnum);
611
612 gLog << err << GetNodeName() << " reports: ";
613 switch (errnum)
614 {
615 case 0xff:
616 EvalStatus(errnum, errinf);
617 return;
618
619 default:
620 gLog << "Error Nr. " << dec << errnum << ", " << errinf << endl;
621 }
622}
623
624// FIXME? Handling of fIsZombie?
625void Dkc::HandleError()
626{
627 //
628 // If there is no error we must not handle anything
629 //
630 if (!HasError())
631 return;
632
633 //
634 // If the program got into the: HandleError state before the hardware
635 // has finished handeling the error we have to wait for the hardware
636 // handeling the error
637 //
638 // FIXME: Timeout???
639 //
640// while (GetError()<0)
641// usleep(1);
642
643 //
644 // After this software and hardware should be in a state so that
645 // we can go on working 'as usual' Eg. Initialize a Display Update
646 //
647 gLog << inf << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
648 switch (GetError())
649 {
650 case 0xff:
651 gLog << err << "DKC error! Go and check what is going on!" << endl;
652 return;
653
654 default:
655 gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
656
657 }
658}
659
660void Dkc::DisplayVal()
661{
662 const LWORDS_t pos = GetPdoPos2();
663 if (IsZombieNode())
664 {
665 if (fLabel)
666 fLabel->SetText(new TGString(""));
667 fUpdPos = ~pos;
668 return;
669 }
670
671 char text[21]="";
672 if (pos!=fUpdPos && fLabel)
673 {
674 sprintf(text, "%ld", pos);
675 fLabel->SetText(new TGString(text));
676 fUpdPos = pos;
677 }
678}
Note: See TracBrowser for help on using the repository browser.