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

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