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

Last change on this file since 973 was 926, checked in by tbretz, 24 years ago
*** empty log message ***
File size: 15.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
10Macs::Macs(const BYTE_t nodeid, const char *name, MLog &out)
11 : NodeDrv(nodeid, name, out), fMacId(2*nodeid+1),
12 fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0),
13 fPosActive(0), fRpmActive(0)
14{
15 fTimeout = new TTimer(this, 100, kFALSE); // 100ms
16}
17
18Macs::~Macs()
19{
20 fTimerOn = kFALSE;
21 delete fTimeout;
22}
23
24void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv)
25{
26 switch (idx)
27 {
28 case 0x100a:
29 lout << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
30 return;
31
32 case 0x2002:
33 cout << GetNodeName() << ": Actual velocity: " << dec << val << endl;
34 fVel = val;
35 return;
36
37 case 0x4000:
38 switch (subidx)
39 {
40 case 1:
41 cout << GetNodeName() << ": Timeout timer is " << (val?"en":"dis") << "abled." << endl;
42 return;
43 case 2:
44 cout << GetNodeName() << ": Actual timeout time: " << dec << val << "ms" << endl;
45 return;
46 }
47 break;
48
49 case 0x6004:
50 if (subidx)
51 return;
52
53// lout << "Actual Position: " << dec << (signed long)val << endl;
54 fPos = (LWORDS_t)val;
55 fPosTime.SetTimer(tv);
56 return;
57/*
58 case 0x2001:
59 cout << "Axe Status: 0x" << hex << val << endl;
60 cout << " - Motor " << (val&0x01?"standing":"moving") << endl;
61 cout << " - Positioning " << (val&0x02?"active":"inactive") << endl;
62 cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl;
63 cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
64 cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl;
65 fPosActive = val&0x02;
66 fRpmActive = val&0x04;
67 return;
68 case 0x2003:
69 if (!subidx)
70 {
71 cout << "Input State: ";
72 for (int i=0; i<8; i++)
73 cout << (int)(val&(1<<i)?1:0);
74 cout <<endl;
75 return;
76 }
77 cout << "Input No." << subidx << (val?"hi":"lo") << endl;
78 return;
79 case 0x2004:
80 cout << "Status Value of Axis: 0x" << hex << val << endl;
81 cout << " - Attitude control: " << (val&0x800000?"off":"on") << endl;
82 cout << " - Movement done: " << (val&0x040000?"yes":"no") << endl;
83 cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
84 cout << " - Attitude control: " << (val&0x04?"off":"on") << endl;
85 cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl;
86 cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl;
87 cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl;
88 return;
89*/
90
91 case 0x6002:
92 lout << "- Velocity resolution " << GetNodeName() << ": " << dec << val << " ticks/min" << endl;
93 fVelRes = val;
94 return;
95 }
96 cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
97 cout << ", val=0x"<<val<<endl;
98}
99
100void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx)
101{
102 if (idx==0x4000 && subidx==0 && fTimerOn)
103 {
104 ResetTimeout();
105 return;
106 }
107 NodeDrv::HandleSDOOK(idx, subidx);
108}
109
110
111void Macs::ReqVelRes()
112{
113 lout << "- Requesting velocity resolution (velres, 0x6002) of " << GetNodeName() << endl;
114 RequestSDO(0x6002);
115 WaitForSdo(0x6002);
116}
117
118void Macs::SetPDO1On(BYTE_t flag)
119{
120 lout << "- " << (flag?"Enable":"Disable") << " PDO1 of " << GetNodeName() << endl;
121 SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
122 WaitForSdo(0x1800, 1);
123}
124
125void Macs::InitDevice(Network *net)
126{
127 NodeDrv::InitDevice(net);
128
129// SendSDO(0x4003, (LWORD_t)('E'<<24 | 'X'<<16 | 'I'<<8 'T'));
130// WaitForSdo(0x4003, 0);
131
132/*
133 lout << "- Requesting SDO 0x2002 (vel) of " << (int)GetId() << endl;
134 RequestSDO(0x2002);
135 WaitForSdo(0x2002);
136
137 lout << "- Requesting SDO 0x2003 of " << (int)GetId() << endl;
138 RequestSDO(0x2003);
139 WaitForSdo(0x2003);
140
141 lout << "- Requesting SDO 0x2004 of " << (int)GetId() << endl;
142 RequestSDO(0x2004);
143 WaitForSdo(0x2004);
144 */
145 EnableTimeout(kFALSE);
146
147 lout << "- Requesting Mac Software Version of " << GetNodeName() << endl;
148 RequestSDO(0x100a);
149 WaitForSdo(0x100a);
150
151 SetRpmMode(FALSE);
152
153 ReqVelRes(); // Init fVelRes
154
155 lout << "- Motor on of " << GetNodeName() << endl;
156 SendSDO(0x3000, string('o', 'n'));
157 WaitForSdo(0x3000);
158
159
160// SetHome(250000);
161
162// lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
163// RequestSDO(0x2001);
164// WaitForSdo(0x2001);
165
166 SetPDO1On(FALSE); // this is a workaround for the Macs
167 SetPDO1On(TRUE);
168
169 SetNoWait(TRUE);
170}
171
172void Macs::StopMotor()
173{
174 //
175 // Stop the motor and switch off the position control unit
176 //
177 SendSDO(0x3000, string('s','t','o','p'));
178 WaitForSdo(0x3000);
179}
180
181void Macs::StopDevice()
182{
183 EnableTimeout(kFALSE);
184
185 SetNoWait(FALSE);
186
187 //
188 // FIXME: This isn't called if the initialization isn't done completely!
189 //
190
191 SetRpmMode(FALSE);
192
193 SetPDO1On(FALSE);
194
195 lout << "- Motor off of " << GetNodeName() << endl;
196 SendSDO(0x3000, string('o', 'f', 'f'));
197 WaitForSdo(0x3000);
198
199 /*
200 lout << "- Stopping Program of " << (int)GetId() << endl;
201 SendSDO(0x4000, (LWORD_t)0xaffe);
202 WaitForSdo();
203 */
204}
205
206void Macs::ReqPos()
207{
208 lout << "- Requesting Position of " << GetNodeName() << endl;
209 RequestSDO(0x6004);
210 WaitForSdo(0x6004);
211}
212
213void Macs::ReqVel()
214{
215 lout << "- Requesting Velocity of " << GetNodeName() << endl;
216 RequestSDO(0x2002);
217 WaitForSdo(0x2002);
218}
219
220void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
221{
222 lout << "- Driving " << GetNodeName() << " to home position, Offset=" << dec << pos << endl;
223 SendSDO(0x6003, 2, (LWORD_t)pos); // home
224 WaitForSdo(0x6003, 2);
225
226 // home also defines the zero point of the system
227 // maximum time allowd for home drive: 25.000ms
228 SendSDO(0x3001, string('h','o','m','e')); // home
229 WaitForSdo(0x3001, 0, maxtime*1000);
230 lout << "- Home position of " << GetNodeName() << " reached. " << endl;
231
232 SendSDO(0x6003, 0, string('s','e','t')); // home
233 WaitForSdo(0x6003, 0);
234}
235
236void Macs::SetVelocity(LWORD_t vel)
237{
238 SendSDO(0x2002, vel); // velocity
239 WaitForSdo(0x2002);
240}
241
242void Macs::SetAcceleration(LWORD_t acc)
243{
244 SendSDO(0x2003, 0, acc); // acceleration
245 WaitForSdo(0x2003, 0);
246}
247
248void Macs::SetDeceleration(LWORD_t dec)
249{
250 SendSDO(0x2003, 1, dec); // acceleration
251 WaitForSdo(0x2003, 1);
252}
253
254void Macs::SetRpmMode(BYTE_t mode)
255{
256 //
257 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
258 //
259 SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
260 WaitForSdo(0x3006, 0);
261}
262
263void Macs::SetRpmVelocity(LWORDS_t cvel)
264{
265 SendSDO(0x3006, 1, (LWORD_t)cvel);
266 WaitForSdo(0x3006, 1);
267}
268
269void Macs::StartRelPos(LWORDS_t pos)
270{
271 SendSDO(0x6004, 1, (LWORD_t)pos);
272}
273
274void Macs::StartAbsPos(LWORDS_t pos)
275{
276 SendSDO(0x6004, 0, (LWORD_t)pos);
277}
278
279void Macs::SetNoWait(BYTE_t flag)
280{
281 lout << "- Setting NOWAIT " << (flag?"ON ":"OFF ") << GetNodeName() << endl;
282 SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f'));
283 WaitForSdo(0x3008);
284}
285
286void Macs::StartVelSync()
287{
288 //
289 // The syncronization mode is disabled by a 'MOTOR STOP'
290 // or by a positioning command (POSA, ...)
291 //
292 lout << "- Setting Vel Sync Mode of " << GetNodeName() << endl;
293 SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
294 WaitForSdo(0x3007, 0);
295}
296
297void Macs::StartPosSync()
298{
299 //
300 // The syncronization mode is disabled by a 'MOTOR STOP'
301 // or by a positioning command (POSA, ...)
302 //
303 lout << "- Setting Pos Sync Mode of " << GetNodeName() << endl;
304 SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
305 WaitForSdo(0x3007, 1);
306}
307/*
308void Macs::ReqAxEnd()
309{
310 RequestSDO(0x2001);
311 WaitForSdo(0x2001);
312}
313*/
314void Macs::SendMsg(BYTE_t data[6])
315{
316 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
317}
318
319void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
320 BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
321{
322 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
323}
324
325void Macs::HandlePDO1(BYTE_t *data, struct timeval *tv)
326{
327 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
328
329 fPosActive = data[3]&0x02;
330 fRpmActive = data[3]&0x04;
331
332 fPdoTime.SetTimer(tv);
333}
334
335void Macs::HandlePDO2(BYTE_t *data, struct timeval *tv)
336{
337 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
338 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
339
340 //
341 // errnum==0 gives a sudden information that something happened. Now the
342 // microcontroller is running inside its interrup procedure which
343 // stopped the normal program. The interrupt procedure should try to clear
344 // the error state of the hardware. This should never create a new error!
345 //
346 if (!errnum)
347 {
348 cout << "Mac " << GetNodeName() << " reports Error occursion." << endl;
349 SetError(-1);
350 return;
351 }
352
353 //
354 // Now the error is handled by the hardware now it is the software part
355 // to react on it. The Error flag now is set to the correct value.
356 //
357 if (GetError()>0)
358 cout << "Mac " << GetNodeName() << " WARNING! Error #" << GetError() << " unhandled by software." << endl;
359
360 SetError(errnum);
361
362 cout << "Mac " << GetNodeName() << " reports: ";
363 switch (errnum)
364 {
365 case 6:
366 cout << "Home position not the first positioning command." << endl;
367 return;
368
369 case 8:
370 cout << "Control deviation overflow." << endl;
371 return;
372
373 case 9:
374 cout << "Zero index not found." << endl;
375 return;
376
377 case 11:
378 case 25:
379 switch (errinf)
380 {
381 case -1:
382 cout << "Negative";
383 break;
384 case 1:
385 cout << "Positive";
386 break;
387 default:
388 cout << "-unknown-";
389 }
390 switch (errnum)
391 {
392 case 11:
393 cout << " software endswitch activated." << endl;
394 break;
395 case 25:
396 cout << " hardware endswitch activated." << endl;
397 break;
398 }
399 return;
400
401 case 84:
402 cout << "Too many (>12) ON TIME calls." << endl;
403 return;
404
405 case 100:
406 cout << "Connection timed out." << endl;
407 return;
408
409 default:
410 cout << "Error Nr. " << errnum << ", " << errinf << endl;
411 }
412}
413
414void Macs::HandleError()
415{
416 //
417 // If there is no error we must not handle anything
418 //
419 if (!HasError())
420 return;
421
422 //
423 // If the program got into the: HandleError state before the hardware
424 // has finished handeling the error we have to wait for the hardware
425 // handeling the error
426 //
427 // FIXME: Timeout???
428 //
429 while (GetError()<0)
430 usleep(1);
431
432 //
433 // After this software and hardware should be in a state so that
434 // we can go on working 'as usual' Eg. Initialize a Display Update
435 //
436 cout << "Mac " << GetNodeName() << " Handling Error #" << GetError() << endl;
437 switch (GetError())
438 {
439 case 6: // home
440 case 8: // control dev
441 case 9: // zero idx
442 case 84: // ON TIME
443 // Stop program?
444 return;
445
446 case 11: // software endswitch
447 case 25: // hardware endswitch
448 case 100: // timeout (movement has been stopped, so we can go on)
449 DelError();
450 return;
451
452 case 101:
453 lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
454 DelError();
455 return;
456 }
457}
458
459double Macs::GetTime()
460{
461 return fPosTime.Now();
462}
463
464double Macs::GetMjd()
465{
466 return fPosTime.CalcMjd();
467}
468
469double Macs::GetPdoTime()
470{
471 return fPdoTime.Now();
472}
473
474double Macs::GetPdoMjd()
475{
476 return fPdoTime.CalcMjd();
477}
478
479/* 0x2000 0 rw Maximum positioning error */
480/* 1 rw Negative Software Endswitch */
481/* 2 rw Positive Software Endswitch */
482void Macs::SetNegEndswitch(LWORDS_t val)
483{
484 SendSDO(0x2000, 1, (LWORD_t)val);
485 WaitForSdo(0x2000, 1);
486}
487
488void Macs::SetPosEndswitch(LWORDS_t val)
489{
490 SendSDO(0x2000, 2, (LWORD_t)val);
491 WaitForSdo(0x2000, 2);
492}
493
494void Macs::EnableEndswitches(bool neg, bool pos)
495{
496 SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
497 WaitForSdo(0x2000, 3);
498}
499
500void Macs::ReqTimeoutTime()
501{
502 RequestSDO(0x4000, 2);
503 WaitForSdo(0x4000, 2);
504}
505
506void Macs::EnableTimeout(bool enable, LWORDS_t ms)
507{
508 if (!enable)
509 {
510 SendSDO(0x4000, 1, string('o', 'f', 'f'));
511 WaitForSdo(0x4000, 1);
512
513 fTimeout->Stop(); //kTRUE);
514
515 fTimerOn = kFALSE;
516 }
517 else
518 {
519 if (ms>0)
520 SetTimeoutTime(ms);
521
522 ResetTimeout();
523
524 fTimerOn = kTRUE;
525 fTimeout->Start(fGuardTime, kFALSE); //kTRUE);
526
527 SendSDO(0x4000, 1, string('o', 'n'));
528 WaitForSdo(0x4000, 1);
529 }
530 lout << "- Timeout timer of " << GetNodeName() << " turned "
531 << (enable?"on.":"off.") << endl;
532}
533
534Bool_t Macs::HandleTimer(TTimer *t)
535{
536 /*
537 Fons:
538 -----
539
540 timers never trigger at the same time or when in a TTimer::Notify.
541 Little explanation:
542
543 - there are two types of timers synchronous and a-synchronous.
544 - synchronous timers are only handled via the ROOT eventloop
545 (see TUnixSystem::DispatchOneEvent()). If there are no mouse/keyboard
546 events then the synchronous timer queue is checked. So if the processing
547 of a mouse/keyboard event takes a long time synchronous timers are not
548 called for a while. To prevent this from happening one can call in long
549 procedures gSystem->ProcessEvents(). The system schedules only the
550 next timer in the queue when the current one's Notify() has finished.
551 - a-synchronous timers are triggered via SIGALARM, i.e. the program is
552 interupted and execution jumps to the Notify() function. When the
553 notify is finished the next a-sync timer is scheduled and the system
554 resumes from the place where it was initially interrupted. One of the
555 things to remember when using a-sync timers is don't make any graphics
556 calls in them. X11 is not re-entrant and it might be that the SIGALARM
557 signal interrupted the system while being in X11. A-sync timers are best
558 used to set flags that you can test at a convenient and controlled
559 time.
560 */
561
562 //
563 // FIXME! Use NMT!
564 //
565 SendSDO(0x4000, 0, (LWORD_t)0, false);
566
567 Timer time;
568 if (time.Now() > fTimeoutTime)
569 {
570 lout << ddev(MLog::eGui);
571 lout << "Warning: " << GetNodeName() << " didn't respond in timeout window." << endl;
572 lout << edev(MLog::eGui);
573 SetError(101);
574 }
575
576
577 //WaitForSdo(0x4000, 0, kDontWait);
578 // if (fTimerOn)
579 // fTimeout->Start(fGuardTime, kTRUE);
580
581 return kTRUE;
582}
583
584void Macs::ResetTimeout()
585{
586 Timer time;
587 fTimeoutTime = time.Now() + 2.*fGuardTime/1000.; //[usec]
588}
589
590void Macs::SetTimeoutTime(LWORD_t ms)
591{
592 fGuardTime = ms/2; // FIXME: Is '/2' the best choose?
593
594 SendSDO(0x4000, 2, ms);
595 WaitForSdo(0x4000, 2);
596}
597
Note: See TracBrowser for help on using the repository browser.