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

Last change on this file since 1778 was 1758, checked in by tbretz, 23 years ago
*** empty log message ***
File size: 17.2 KB
Line 
1#include "macs.h"
2
3#include <iostream.h>
4#include <sys/time.h> // timeval->tv_sec
5
6#include "timer.h"
7#include "network.h"
8#include "MLogManip.h"
9
10ClassImp(Macs);
11
12/*
13 ---------------------------
14 For test purposes
15 ---------------------------
16
17class MyTimer : public TTimer
18{
19public:
20 MyTimer(TObject *obj, Long_t ms, Bool_t mode) : TTimer(obj, ms, mode) {}
21 Bool_t Notify()
22 {
23 cout << "Notify" << endl;
24 TTimer::Notify();
25 return kTRUE;
26 }
27};
28*/
29
30Macs::Macs(const BYTE_t nodeid, const char *name, MLog &out)
31 : NodeDrv(nodeid, name, out), fMacId(2*nodeid+1),
32 fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0),
33 fPosActive(0), fRpmActive(0)
34{
35// fTimeout = new TTimer(this, 100); //, kFALSE); // 100ms, asynchronous
36}
37
38Macs::~Macs()
39{
40 //fTimerOn = kFALSE;
41 // delete fTimeout;
42}
43
44void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
45{
46 switch (idx)
47 {
48 case 0x100a:
49 lout << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
50 fSoftVersion = val;
51 return;
52
53 case 0x100b:
54 // Do not display, this is used for CheckConnection
55 // lout << "Node ID: " << dec << val << endl;
56 return;
57
58 case 0x2002:
59 cout << GetNodeName() << ": Actual velocity: " << dec << val << endl;
60 fVel = val;
61 return;
62
63 case 0x6004:
64 if (subidx)
65 return;
66
67// lout << "Actual Position: " << dec << (signed long)val << endl;
68 fPos = (LWORDS_t)val;
69 fPosTime.SetTimer(tv);
70 return;
71/*
72 case 0x2001:
73 cout << "Axe Status: 0x" << hex << val << endl;
74 cout << " - Motor " << (val&0x01?"standing":"moving") << endl;
75 cout << " - Positioning " << (val&0x02?"active":"inactive") << endl;
76 cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl;
77 cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
78 cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl;
79 fPosActive = val&0x02;
80 fRpmActive = val&0x04;
81 return;
82 case 0x2003:
83 if (!subidx)
84 {
85 cout << "Input State: ";
86 for (int i=0; i<8; i++)
87 cout << (int)(val&(1<<i)?1:0);
88 cout <<endl;
89 return;
90 }
91 cout << "Input No." << subidx << (val?"hi":"lo") << endl;
92 return;
93 case 0x2004:
94 cout << "Status Value of Axis: 0x" << hex << val << endl;
95 cout << " - Attitude control: " << (val&0x800000?"off":"on") << endl;
96 cout << " - Movement done: " << (val&0x040000?"yes":"no") << endl;
97 cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
98 cout << " - Attitude control: " << (val&0x04?"off":"on") << endl;
99 cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl;
100 cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl;
101 cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl;
102 return;
103*/
104
105 case 0x6002:
106 lout << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " ticks/min" << endl;
107 fVelRes = val;
108 return;
109
110 case 0x6501:
111 lout << "- " << GetNodeName() << ": Encoder resolution = " << dec << val << " ticks/min" << endl;
112 fRes = val;
113 return;
114 }
115 cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
116 cout << ", val=0x"<<val<<endl;
117}
118
119void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
120{
121 switch (idx)
122 {
123 case 0x2002:
124 switch (subidx)
125 {
126 case 0:
127 //lout << ddev(MLog::eGui);
128 lout << "- " << GetNodeName() << ": Velocity set." << endl;
129 //lout << edev(MLog::eGui);
130 return;
131 }
132 break;
133
134 case 0x2003:
135 switch (subidx)
136 {
137 case 0:
138 //lout << ddev(MLog::eGui);
139 lout << "- " << GetNodeName() << ": Acceleration set." << endl;
140 //lout << edev(MLog::eGui);
141 return;
142 case 1:
143 //lout << ddev(MLog::eGui);
144 lout << "- " << GetNodeName() << ": Decceleration set." << endl;
145 //lout << edev(MLog::eGui);
146 return;
147 }
148 break;
149
150 case 0x3006:
151 switch (subidx)
152 {
153 case 0:
154 //lout << ddev(MLog::eGui);
155 lout << "- " << GetNodeName() << ": RPM mode switched." << endl;
156 //lout << edev(MLog::eGui);
157 return;
158
159 case 1:
160 /*
161 lout << ddev(MLog::eGui);
162 lout << "- Velocity set (" << GetNodeName() << ")" << endl;
163 lout << edev(MLog::eGui);
164 */
165 return;
166 }
167 break;
168
169 case 0x4000:
170 HandleNodeguard(tv);
171 return;
172
173 case 0x6004:
174 switch (subidx)
175 {
176 case 0:
177 //lout << ddev(MLog::eGui);
178 lout << "- " << GetNodeName() << ": Absolute positioning started." << endl;
179 //lout << edev(MLog::eGui);
180 return;
181
182 case 1:
183 //lout << ddev(MLog::eGui);
184 lout << "- " << GetNodeName() << ": Relative positioning started." << endl;
185 //lout << edev(MLog::eGui);
186 return;
187 }
188 break;
189
190
191 }
192 NodeDrv::HandleSDOOK(idx, subidx, tv);
193}
194
195
196void Macs::ReqVelRes()
197{
198 lout << "- " << GetNodeName() << ": Requesting velocity resolution (velres, 0x6002)." << endl;
199 RequestSDO(0x6002);
200 WaitForSdo(0x6002);
201}
202
203void Macs::ReqRes()
204{
205 lout << "- " << GetNodeName() << ": Requesting encoder resolution (res, 0x6501)." << endl;
206 RequestSDO(0x6501);
207 WaitForSdo(0x6501);
208}
209
210void Macs::SetPDO1On(BYTE_t flag)
211{
212 lout << "- " << GetNodeName() << ": " << (flag?"Enable":"Disable") << " PDO1." << endl;
213 SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
214 WaitForSdo(0x1800, 1);
215}
216
217void Macs::CheckConnection()
218{
219 RequestSDO(0x100b);
220 WaitForSdo(0x100b);
221}
222
223
224void Macs::Init()
225{
226 if (HasError())
227 {
228 lout << "Macs::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
229 SetZombie();
230 }
231
232 StopHostGuarding();
233 StopGuarding();
234
235 usleep(2000*GetGuardTime());
236
237 lout << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl;
238 RequestSDO(0x100a);
239 WaitForSdo(0x100a);
240
241 if (IsZombieNode())
242 {
243 lout << GetNodeName() << " - InitDevice failed!" << endl;
244 return;
245 }
246
247 // FIXME! Not statically linked!
248 if (fSoftVersion<0x00000042) // 00.66
249 {
250 lout << GetNodeName() << " - Software Version too old!" << endl;
251 SetZombie();
252 return;
253 }
254
255 SetRpmMode(FALSE);
256
257 ReqRes(); // Init fRes
258 ReqVelRes(); // Init fVelRes
259
260 lout << "- " << GetNodeName() << ": Motor on." << endl;
261 SendSDO(0x3000, string('o', 'n'));
262 WaitForSdo(0x3000);
263
264// SetHome(250000);
265
266// lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
267// RequestSDO(0x2001);
268// WaitForSdo(0x2001);
269
270 SetPDO1On(FALSE); // this is a workaround for the Macs
271 SetPDO1On(TRUE);
272
273 SetNoWait(TRUE);
274
275 StartGuarding(100, 2);
276 StartHostGuarding();
277}
278
279void Macs::StopMotor()
280{
281 //
282 // Stop the motor and switch off the position control unit
283 //
284 SendSDO(0x3000, string('s','t','o','p'));
285 WaitForSdo(0x3000);
286}
287
288void Macs::StopDevice()
289{
290 //EnableTimeout(kFALSE);
291
292 SetNoWait(FALSE);
293
294 //
295 // FIXME: This isn't called if the initialization isn't done completely!
296 //
297
298 SetRpmMode(FALSE);
299
300 SetPDO1On(FALSE);
301
302 lout << "- " << GetNodeName() << ": Motor off." << endl;
303 SendSDO(0x3000, string('o', 'f', 'f'));
304 WaitForSdo(0x3000);
305
306 /*
307 lout << "- Stopping Program of " << (int)GetId() << endl;
308 SendSDO(0x4000, (LWORD_t)0xaffe);
309 WaitForSdo();
310 */
311}
312
313void Macs::ReqPos()
314{
315 lout << "- " << GetNodeName() << ": Requesting Position." << endl;
316 RequestSDO(0x6004);
317 WaitForSdo(0x6004);
318}
319
320void Macs::ReqVel()
321{
322 lout << "- " << GetNodeName() << ": Requesting Velocity." << endl;
323 RequestSDO(0x2002);
324 WaitForSdo(0x2002);
325}
326
327void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
328{
329 StopHostGuarding();
330 StopGuarding();
331
332 lout << "- " << GetNodeName() << ": Driving to home position, Offset=" << dec << pos << endl;
333 SendSDO(0x6003, 2, (LWORD_t)pos); // home
334 WaitForSdo(0x6003, 2);
335
336 // home also defines the zero point of the system
337 // maximum time allowd for home drive: 25.000ms
338 SendSDO(0x3001, string('h','o','m','e')); // home
339 WaitForSdo(0x3001, 0, maxtime*1000);
340 lout << "- " << GetNodeName() << ": Home position reached. " << endl;
341
342 SendSDO(0x6003, 0, string('s','e','t')); // home
343 WaitForSdo(0x6003, 0);
344
345 StartGuarding();
346 StartHostGuarding();
347}
348
349void Macs::SetVelocity(LWORD_t vel)
350{
351 SendSDO(0x2002, vel); // velocity
352 WaitForSdo(0x2002, 0);
353}
354
355void Macs::SetAcceleration(LWORD_t acc)
356{
357 SendSDO(0x2003, 0, acc); // acceleration
358 WaitForSdo(0x2003, 0);
359}
360
361void Macs::SetDeceleration(LWORD_t dec)
362{
363 SendSDO(0x2003, 1, dec); // acceleration
364 WaitForSdo(0x2003, 1);
365}
366
367void Macs::SetRpmMode(BYTE_t mode)
368{
369 //
370 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
371 //
372 SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
373 WaitForSdo(0x3006, 0);
374}
375
376void Macs::SetRpmVelocity(LWORDS_t cvel)
377{
378 SendSDO(0x3006, 1, (LWORD_t)cvel);
379 WaitForSdo(0x3006, 1);
380}
381
382void Macs::StartRelPos(LWORDS_t pos)
383{
384 SendSDO(0x6004, 1, (LWORD_t)pos);
385}
386
387void Macs::StartAbsPos(LWORDS_t pos)
388{
389 SendSDO(0x6004, 0, (LWORD_t)pos);
390}
391
392void Macs::SetNoWait(BYTE_t flag)
393{
394 lout << "- " << GetNodeName() << ": Setting NOWAIT " << (flag?"ON":"OFF") << "." << endl;
395 SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f'));
396 WaitForSdo(0x3008);
397}
398
399void Macs::StartVelSync()
400{
401 //
402 // The syncronization mode is disabled by a 'MOTOR STOP'
403 // or by a positioning command (POSA, ...)
404 //
405 lout << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl;
406 SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
407 WaitForSdo(0x3007, 0);
408}
409
410void Macs::StartPosSync()
411{
412 //
413 // The syncronization mode is disabled by a 'MOTOR STOP'
414 // or by a positioning command (POSA, ...)
415 //
416 lout << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl;
417 SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
418 WaitForSdo(0x3007, 1);
419}
420/*
421void Macs::ReqAxEnd()
422{
423 RequestSDO(0x2001);
424 WaitForSdo(0x2001);
425}
426*/
427void Macs::SendMsg(BYTE_t data[6])
428{
429 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
430}
431
432void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
433 BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
434{
435 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
436}
437
438void Macs::HandlePDO1(BYTE_t *data, timeval_t *tv)
439{
440 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
441
442 // data[3]&0x01; // motor not moving
443 fPosActive = data[3]&kPosActive; // positioning active
444 fRpmActive = data[3]&kRpmActive; // RPM mode switched on
445 // data[3]&0x08; // - unused -
446 // data[3]&0x10; // - unused -
447 // data[3]&0x20; // - unused -
448 fInControl = data[3]&0x40; // motor uncontrolled
449 // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
450
451 fStatus = data[3];
452
453 fPdoTime.SetTimer(tv);
454}
455
456void Macs::HandlePDO2(BYTE_t *data, timeval_t *tv)
457{
458 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
459 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
460
461 //
462 // errnum==0 gives a sudden information that something happened. Now the
463 // microcontroller is running inside its interrupt procedure which
464 // stopped the normal program. The interrupt procedure should try to clear
465 // the error state of the hardware. This should never create a new error!
466 //
467 if (!errnum)
468 {
469 lout << "- " << GetNodeName() << ": reports Error occursion." << endl;
470 lout << "Macs::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
471 SetZombie();
472 SetError(-1);
473 return;
474 }
475
476 //
477 // Now the error is handled by the hardware now it is the software part
478 // to react on it. The Error flag now is set to the correct value.
479 //
480 if (GetError()>0)
481 {
482 lout << GetNodeName() << ": WARNING! Error #" << GetError() << " unhandled (not cleared) by software." << endl;
483
484 //
485 // If the error is unhadled and/or not cleared, don't try it again.
486 //
487 if (GetError()==errnum)
488 return;
489 }
490
491 SetError(errnum);
492
493 lout << GetNodeName() << " reports: ";
494 switch (errnum)
495 {
496 case 6:
497 //
498 // Report the error to the user. All possible movements should have
499 // been stopped anyhow. Now delete the error to prevent the system
500 // from reporting this error a thousands of times.
501 //
502 lout << "Home position not the first positioning command." << endl;
503 SetError(0);
504 return;
505
506 case 8:
507 lout << "Control deviation overflow." << endl;
508 return;
509
510 case 9:
511 lout << "Zero index not found." << endl;
512 return;
513
514 case 11:
515 case 25:
516 switch (errinf)
517 {
518 case -1:
519 lout << "Negative";
520 break;
521 case 1:
522 lout << "Positive";
523 break;
524 default:
525 lout << "-unknown-";
526 }
527 switch (errnum)
528 {
529 case 11:
530 lout << " software endswitch activated." << endl;
531 break;
532 case 25:
533 lout << " hardware endswitch activated." << endl;
534 break;
535 }
536 return;
537
538 case 84:
539 lout << "Too many (>12) ON TIME calls." << endl;
540 return;
541
542 case 100:
543 //lout << "Connection timed out." << endl;
544 //EnableTimeout(false);
545 return;
546
547 default:
548 lout << "Error Nr. " << errnum << ", " << errinf << endl;
549 }
550}
551
552// FIXME? Handling of fIsZombie?
553void Macs::HandleError()
554{
555 //
556 // If there is no error we must not handle anything
557 //
558 if (!HasError())
559 return;
560
561 //
562 // If the program got into the: HandleError state before the hardware
563 // has finished handeling the error we have to wait for the hardware
564 // handeling the error
565 //
566 // FIXME: Timeout???
567 //
568// while (GetError()<0)
569// usleep(1);
570
571 //
572 // After this software and hardware should be in a state so that
573 // we can go on working 'as usual' Eg. Initialize a Display Update
574 //
575 cout << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
576 switch (GetError())
577 {
578 case 6: // home
579 case 8: // control dev
580 case 9: // zero idx
581 case 84: // ON TIME
582 lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
583 return;
584
585 case 11: // software endswitch
586 case 25: // hardware endswitch
587 lout << "- " << GetNodeName() << ": Cannot handle error 'Endswitch!'" << endl;
588 return;
589
590 case 100: // timeout (movement has been stopped, so we can go on)
591 DelError();
592 return;
593/*
594 case 101:
595 //lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
596 DelError();
597 return;
598 */
599 default:
600 lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
601
602 }
603}
604
605double Macs::GetTime()
606{
607 return fPosTime.Now();
608}
609
610double Macs::GetMjd()
611{
612 return fPosTime.GetMjd();
613}
614
615double Macs::GetPdoTime()
616{
617 return fPdoTime.Now();
618}
619
620double Macs::GetPdoMjd()
621{
622 return fPdoTime.GetMjd();
623}
624
625/* 0x2000 0 rw Maximum positioning error */
626/* 1 rw Negative Software Endswitch */
627/* 2 rw Positive Software Endswitch */
628void Macs::SetNegEndswitch(LWORDS_t val)
629{
630 SendSDO(0x2000, 1, (LWORD_t)val);
631 WaitForSdo(0x2000, 1);
632}
633
634void Macs::SetPosEndswitch(LWORDS_t val)
635{
636 SendSDO(0x2000, 2, (LWORD_t)val);
637 WaitForSdo(0x2000, 2);
638}
639
640void Macs::EnableEndswitches(bool neg, bool pos)
641{
642 SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
643 WaitForSdo(0x2000, 3);
644}
645
646void Macs::SendNodeguard()
647{
648 SendSDO(0x4000, 0, (LWORD_t)0, false);
649}
650
651// --------------------------------------------------------------------------
652//
653// This starts the host guarding. The host guarding is only available
654// if the node guarding is running. The host guarding works with the
655// guardtime and the lifetimefactor from the nodeguarding.
656//
657void Macs::StartHostGuarding()
658{
659 SendSDO(0x100c, 0, (LWORD_t)GetGuardTime());
660 WaitForSdo(0x100c);
661
662 SendSDO(0x100d, 0, (LWORD_t)GetLifeTimeFactor());
663 WaitForSdo(0x100d);
664
665 lout << "- " << GetNodeName() << ": Hostguarding started (" << dec;
666 lout << GetLifeTimeFactor() << "*" << GetGuardTime() << "ms)" << endl;
667}
668
669// --------------------------------------------------------------------------
670//
671// Stop the host guarding.
672//
673void Macs::StopHostGuarding()
674{
675 SendSDO(0x100c, 0, (LWORD_t)0);
676 WaitForSdo(0x100c);
677
678 SendSDO(0x100d, 0, (LWORD_t)0);
679 WaitForSdo(0x100d);
680
681 lout << "- " << GetNodeName() << ": Hostguarding stopped." << endl;
682}
683
Note: See TracBrowser for help on using the repository browser.