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

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