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

Last change on this file since 914 was 912, checked in by tbretz, 24 years ago
*** empty log message ***
File size: 11.4 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, ostream &out=cout)
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}
15
16Macs::~Macs()
17{
18}
19
20void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv)
21{
22 switch (idx)
23 {
24 case 0x100a:
25 lout << "- Mac using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
26 return;
27
28 case 0x2002:
29 cout << "Actual velocity: " << dec << val << endl;
30 fVel = val;
31 return;
32
33 case 0x6004:
34 if (subidx)
35 return;
36
37// lout << "Actual Position: " << dec << (signed long)val << endl;
38 fPos = (LWORDS_t)val;
39 fPosTime.SetTimer(tv);
40 return;
41/*
42 case 0x2001:
43 cout << "Axe Status: 0x" << hex << val << endl;
44 cout << " - Motor " << (val&0x01?"standing":"moving") << endl;
45 cout << " - Positioning " << (val&0x02?"active":"inactive") << endl;
46 cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl;
47 cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
48 cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl;
49 fPosActive = val&0x02;
50 fRpmActive = val&0x04;
51 return;
52 case 0x2003:
53 if (!subidx)
54 {
55 cout << "Input State: ";
56 for (int i=0; i<8; i++)
57 cout << (int)(val&(1<<i)?1:0);
58 cout <<endl;
59 return;
60 }
61 cout << "Input No." << subidx << (val?"hi":"lo") << endl;
62 return;
63 case 0x2004:
64 cout << "Status Value of Axis: 0x" << hex << val << endl;
65 cout << " - Attitude control: " << (val&0x800000?"off":"on") << endl;
66 cout << " - Movement done: " << (val&0x040000?"yes":"no") << endl;
67 cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
68 cout << " - Attitude control: " << (val&0x04?"off":"on") << endl;
69 cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl;
70 cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl;
71 cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl;
72 return;
73*/
74
75 case 0x6002:
76 lout << "- Velocity resolution #" << (int)GetId() << ": " << dec << val << " ticks/min" << endl;
77 fVelRes = val;
78 return;
79 }
80 cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
81 cout << ", val=0x"<<val<<endl;
82}
83
84void Macs::ReqVelRes()
85{
86 lout << "- Requesting velocity resolution (velres, 0x3007) of " << (int)GetId() << endl;
87 RequestSDO(0x6002);
88 WaitForSdo(0x6002);
89}
90
91void Macs::SetPDO1On(BYTE_t flag)
92{
93 lout << "- " << (flag?"Enable":"Disable") << " PDO1 of #" << (int)GetId() << endl;
94 SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
95 WaitForSdo(0x1800, 1);
96}
97
98void Macs::InitDevice(Network *net)
99{
100 NodeDrv::InitDevice(net);
101
102// SendSDO(0x4003, (LWORD_t)('E'<<24 | 'X'<<16 | 'I'<<8 'T'));
103// WaitForSdo(0x4003, 0);
104
105/*
106 lout << "- Requesting SDO 0x2002 (vel) of " << (int)GetId() << endl;
107 RequestSDO(0x2002);
108 WaitForSdo(0x2002);
109
110 lout << "- Requesting SDO 0x2003 of " << (int)GetId() << endl;
111 RequestSDO(0x2003);
112 WaitForSdo(0x2003);
113
114 lout << "- Requesting SDO 0x2004 of " << (int)GetId() << endl;
115 RequestSDO(0x2004);
116 WaitForSdo(0x2004);
117 */
118
119 lout << "- Requesting Mac Software Version of " << (int)GetId() << endl;
120 RequestSDO(0x100a);
121 WaitForSdo(0x100a);
122
123 SetRpmMode(FALSE);
124
125 ReqVelRes(); // Init fVelRes
126
127 lout << "- Motor on of " << (int)GetId() << endl;
128 SendSDO(0x3000, string('O', 'N'));
129 WaitForSdo(0x3000);
130
131
132// SetHome(250000);
133
134// lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
135// RequestSDO(0x2001);
136// WaitForSdo(0x2001);
137
138 SetPDO1On(FALSE); // this is a workaround for the Macs
139 SetPDO1On(TRUE);
140
141 SetNoWait(TRUE);
142}
143
144void Macs::StopMotor()
145{
146 //
147 // Stop the motor and switch off the position control unit
148 //
149 SendSDO(0x3000, string('S','T','O','P'));
150 WaitForSdo(0x3000);
151}
152
153void Macs::StopDevice()
154{
155 SetNoWait(FALSE);
156
157 //
158 // FIXME: This isn't called if the initialization isn't done completely!
159 //
160
161 SetRpmMode(FALSE);
162
163 SetPDO1On(FALSE);
164
165 lout << "- Motor off of " << (int)GetId() << endl;
166 SendSDO(0x3000, string('O', 'F', 'F'));
167 WaitForSdo(0x3000);
168
169 /*
170 lout << "- Stopping Program of " << (int)GetId() << endl;
171 SendSDO(0x4000, (LWORD_t)0xaffe);
172 WaitForSdo();
173 */
174}
175
176void Macs::ReqPos()
177{
178 lout << "- Requesting Position of #" << (int)GetId() << endl;
179 RequestSDO(0x6004);
180 WaitForSdo(0x6004);
181}
182
183void Macs::ReqVel()
184{
185 lout << "- Requesting Velocity of #" << (int)GetId() << endl;
186 RequestSDO(0x2002);
187 WaitForSdo(0x2002);
188}
189
190void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
191{
192 lout << "- Driving #" << (int)GetId() << " to home position, Offset=" << dec << pos << endl;
193 SendSDO(0x6003, 2, (LWORD_t)pos); // home
194 WaitForSdo(0x6003, 2);
195
196 // home also defines the zero point of the system
197 // maximum time allowd for home drive: 25.000ms
198 SendSDO(0x3001, string('h','o','m','e')); // home
199 WaitForSdo(0x3001, 0, maxtime*1000);
200 lout << "- Home position of #" << (int)GetId() << " reached. " << endl;
201
202 SendSDO(0x6003, 0, string('s','e','t')); // home
203 WaitForSdo(0x6003, 0);
204}
205
206void Macs::SetVelocity(LWORD_t vel)
207{
208 SendSDO(0x2002, vel); // velocity
209 WaitForSdo(0x2002);
210}
211
212void Macs::SetAcceleration(LWORD_t acc)
213{
214 SendSDO(0x2003, 0, acc); // acceleration
215 WaitForSdo(0x2003, 0);
216}
217
218void Macs::SetDeceleration(LWORD_t dec)
219{
220 SendSDO(0x2003, 1, dec); // acceleration
221 WaitForSdo(0x2003, 1);
222}
223
224void Macs::SetRpmMode(BYTE_t mode)
225{
226 //
227 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
228 //
229 SendSDO(0x3006, 0, mode ? string('S','T','R','T') : string('S','T','O','P'));
230 WaitForSdo(0x3006, 0);
231}
232
233void Macs::SetRpmVelocity(LWORDS_t cvel)
234{
235 SendSDO(0x3006, 1, (LWORD_t)cvel);
236 WaitForSdo(0x3006, 1);
237}
238
239void Macs::StartRelPos(LWORDS_t pos)
240{
241 SendSDO(0x6004, 1, (LWORD_t)pos);
242}
243
244void Macs::StartAbsPos(LWORDS_t pos)
245{
246 SendSDO(0x6004, 0, (LWORD_t)pos);
247}
248
249void Macs::SetNoWait(BYTE_t flag)
250{
251 lout << "- Setting NOWAIT " << (flag?"ON":"OFF") << " #" << (int)GetId() << endl;
252 SendSDO(0x3008, flag ? string('O', 'N') : string('O', 'F', 'F'));
253 WaitForSdo(0x3008);
254}
255
256void Macs::StartVelSync()
257{
258 //
259 // The syncronization mode is disabled by a 'MOTOR STOP'
260 // or by a positioning command (POSA, ...)
261 //
262 lout << "- Setting Vel Sync Mode #" << (int)GetId() << endl;
263 SendSDO(0x3007, 0, string('S', 'Y', 'N', 'C'));
264 WaitForSdo(0x3007, 0);
265}
266
267void Macs::StartPosSync()
268{
269 //
270 // The syncronization mode is disabled by a 'MOTOR STOP'
271 // or by a positioning command (POSA, ...)
272 //
273 lout << "- Setting Pos Sync Mode #" << (int)GetId() << endl;
274 SendSDO(0x3007, 1, string('S', 'Y', 'N', 'C'));
275 WaitForSdo(0x3007, 1);
276}
277/*
278void Macs::ReqAxEnd()
279{
280 RequestSDO(0x2001);
281 WaitForSdo(0x2001);
282}
283*/
284void Macs::SendMsg(BYTE_t data[6])
285{
286 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
287}
288
289void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
290 BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
291{
292 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
293}
294
295void Macs::HandlePDO1(BYTE_t *data, struct timeval *tv)
296{
297 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
298
299 fPosActive = data[3]&0x02;
300 fRpmActive = data[3]&0x04;
301
302 fPdoTime.SetTimer(tv);
303}
304
305void Macs::HandlePDO2(BYTE_t *data, struct timeval *tv)
306{
307 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
308 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
309
310 //
311 // errnum==0 gives a sudden information that something happened. Now the
312 // microcontroller is running inside its interrup procedure which
313 // stopped the normal program. The interrupt procedure should try to clear
314 // the error state of the hardware. This should never create a new error!
315 //
316 if (!errnum)
317 {
318 cout << "Mac #" << (int)GetId() << " reports Error occursion." << endl;
319 SetError(-1);
320 return;
321 }
322
323 //
324 // Now the error is handled by the hardware now it is the software part
325 // to react on it. The Error flag now is set to the correct value.
326 //
327 if (GetError()>0)
328 cout << "Mac #" << (int)GetId() << " WARNING! Error #" << GetError() << " unhandled by software." << endl;
329
330 SetError(errnum);
331
332 cout << "Mac #" << (int)GetId() << " reports: ";
333 switch (errnum)
334 {
335 case 6:
336 cout << "Home position not the first positioning command." << endl;
337 return;
338
339 case 8:
340 cout << "Control deviation overflow." << endl;
341 return;
342
343 case 9:
344 cout << "Zero index not found." << endl;
345 return;
346
347 case 11:
348 case 25:
349 switch (errinf)
350 {
351 case -1:
352 cout << "Negative";
353 break;
354 case 1:
355 cout << "Positive";
356 break;
357 default:
358 cout << "-unknown-";
359 }
360 switch (errnum)
361 {
362 case 11:
363 cout << " software endswitch activated." << endl;
364 break;
365 case 25:
366 cout << " hardware endswitch activated." << endl;
367 break;
368 }
369 return;
370
371 case 84:
372 cout << "Too many (>12) ON TIME calls." << endl;
373 return;
374
375 default:
376 cout << "Error Nr. " << errnum << ", " << errinf << endl;
377 }
378}
379
380void Macs::HandleError()
381{
382 //
383 // If there is no error we must not handle anything
384 //
385 if (!HasError())
386 return;
387
388 //
389 // If the program got into the: HandleError state before the hardware
390 // has finished handeling the error we have to wait for the hardware
391 // handeling the error
392 //
393 // FIXME: Timeout???
394 //
395 while (GetError()<0)
396 usleep(1);
397
398 //
399 // After this software and hardware should be in a state so that
400 // we can go on working 'as usual' Eg. Initialize a Display Update
401 //
402 cout << "Mac #" << (int)GetId() << " Handling Error #" << GetError() << endl;
403 switch (GetError())
404 {
405 case 6: // home
406 case 8: // control dev
407 case 9: // zero idx
408 case 84: // ON TIME
409 // Stop program?
410 return;
411
412 case 11: // software endswitch
413 case 25: // hardware endswitch
414 DelError();
415 return;
416 }
417}
418
419double Macs::GetTime()
420{
421 return fPosTime.Now();
422}
423
424double Macs::GetMjd()
425{
426 return fPosTime.CalcMjd();
427}
428
429double Macs::GetPdoTime()
430{
431 return fPdoTime.Now();
432}
433
434double Macs::GetPdoMjd()
435{
436 return fPdoTime.CalcMjd();
437}
438
439/* 0x2000 0 rw Maximum positioning error */
440/* 1 rw Negative Software Endswitch */
441/* 2 rw Positive Software Endswitch */
442void Macs::SetNegEndswitch(LWORDS_t val)
443{
444 SendSDO(0x2000, 1, (LWORD_t)val);
445 WaitForSdo(0x2000, 1);
446}
447
448void Macs::SetPosEndswitch(LWORDS_t val)
449{
450 SendSDO(0x2000, 2, (LWORD_t)val);
451 WaitForSdo(0x2000, 2);
452}
453
454void Macs::EnableEndswitches(bool neg, bool pos)
455{
456 SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
457 WaitForSdo(0x2000, 3);
458}
Note: See TracBrowser for help on using the repository browser.