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

Last change on this file since 808 was 808, checked in by tbretz, 25 years ago
*** empty log message ***
File size: 10.9 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)
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 SendSDO(0x3001, string('h','o','m','e')); // home
198 WaitForSdo(0x3001);
199 lout << "- Home position of #" << (int)GetId() << " reached. " << endl;
200
201 SendSDO(0x6003, 0, string('s','e','t')); // home
202 WaitForSdo(0x6003, 0);
203}
204
205void Macs::SetVelocity(LWORD_t vel)
206{
207 SendSDO(0x2002, vel); // velocity
208 WaitForSdo(0x2002);
209}
210
211void Macs::SetAcceleration(LWORD_t acc)
212{
213 SendSDO(0x2003, 0, acc); // acceleration
214 WaitForSdo(0x2003, 0);
215}
216
217void Macs::SetDeceleration(LWORD_t dec)
218{
219 SendSDO(0x2003, 1, dec); // acceleration
220 WaitForSdo(0x2003, 1);
221}
222
223void Macs::SetRpmMode(BYTE_t mode)
224{
225 //
226 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
227 //
228 SendSDO(0x3006, 0, mode ? string('S','T','R','T') : string('S','T','O','P'));
229 WaitForSdo(0x3006, 0);
230}
231
232void Macs::SetRpmVelocity(LWORDS_t cvel)
233{
234 SendSDO(0x3006, 1, (LWORD_t)cvel);
235 WaitForSdo(0x3006, 1);
236}
237
238void Macs::StartRelPos(LWORDS_t pos)
239{
240 SendSDO(0x6004, 1, (LWORD_t)pos);
241}
242
243void Macs::StartAbsPos(LWORDS_t pos)
244{
245 SendSDO(0x6004, 0, (LWORD_t)pos);
246}
247
248void Macs::SetNoWait(BYTE_t flag)
249{
250 lout << "- Setting NOWAIT " << (flag?"ON":"OFF") << " #" << (int)GetId() << endl;
251 SendSDO(0x3008, flag ? string('O', 'N') : string('O', 'F', 'F'));
252 WaitForSdo(0x3008);
253}
254
255void Macs::SetSyncMode()
256{
257 lout << "- Setting Sync Mode #" << (int)GetId() << endl;
258 SendSDO(0x3007, string('S', 'Y', 'N', 'C'));
259 WaitForSdo(0x3007);
260}
261/*
262void Macs::ReqAxEnd()
263{
264 RequestSDO(0x2001);
265 WaitForSdo(0x2001);
266}
267*/
268void Macs::SendMsg(BYTE_t data[6])
269{
270 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
271}
272
273void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
274 BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
275{
276 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
277}
278
279void Macs::HandlePDO1(BYTE_t *data, struct timeval *tv)
280{
281 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
282
283 fPosActive = data[3]&0x02;
284 fRpmActive = data[3]&0x04;
285
286 fPdoTime.SetTimer(tv);
287}
288
289void Macs::HandlePDO2(BYTE_t *data, struct timeval *tv)
290{
291 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
292 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
293
294 //
295 // errnum==0 gives a sudden information that something happened. Now the
296 // microcontroller is running inside its interrup procedure which
297 // stopped the normal program. The interrupt procedure should try to clear
298 // the error state of the hardware. This should never create a new error!
299 //
300 if (!errnum)
301 {
302 cout << "Mac #" << (int)GetId() << " reports Error occursion." << endl;
303 SetError(-1);
304 return;
305 }
306
307 //
308 // Now the error is handled by the hardware now it is the software part
309 // to react on it. The Error flag now is set to the correct value.
310 //
311 if (GetError()>0)
312 cout << "Mac #" << (int)GetId() << " WARNING! Error #" << GetError() << " unhandled by software." << endl;
313
314 SetError(errnum);
315
316 cout << "Mac #" << (int)GetId() << " reports: ";
317 switch (errnum)
318 {
319 case 6:
320 cout << "Home position not the first positioning command." << endl;
321 return;
322
323 case 8:
324 cout << "Control deviation overflow." << endl;
325 return;
326
327 case 9:
328 cout << "Zero index not found." << endl;
329 return;
330
331 case 11:
332 case 25:
333 switch (errinf)
334 {
335 case -1:
336 cout << "Negative";
337 break;
338 case 1:
339 cout << "Positive";
340 break;
341 default:
342 cout << "-unknown-";
343 }
344 switch (errnum)
345 {
346 case 11:
347 cout << " software endswitch activated." << endl;
348 break;
349 case 25:
350 cout << " hardware endswitch activated." << endl;
351 break;
352 }
353 return;
354
355 case 84:
356 cout << "Too many (>12) ON TIME calls." << endl;
357 return;
358
359 default:
360 cout << "Error Nr. " << errnum << ", " << errinf << endl;
361 }
362}
363
364void Macs::HandleError()
365{
366 //
367 // If there is no error we must not handle anything
368 //
369 if (!HasError())
370 return;
371
372 //
373 // If the program got into the: HandleError state before the hardware
374 // has finished handeling the error we have to wait for the hardware
375 // handeling the error
376 //
377 // FIXME: Timeout???
378 //
379 while (GetError()<0)
380 usleep(1);
381
382 //
383 // After this software and hardware should be in a state so that
384 // we can go on working 'as usual' Eg. Initialize a Display Update
385 //
386 cout << "Mac #" << (int)GetId() << " Handling Error #" << GetError() << endl;
387 switch (GetError())
388 {
389 case 6: // home
390 case 8: // control dev
391 case 9: // zero idx
392 case 84: // ON TIME
393 // Stop program?
394 return;
395
396 case 11: // software endswitch
397 case 25: // hardware endswitch
398 DelError();
399 return;
400 }
401}
402
403double Macs::GetTime()
404{
405 return fPosTime.GetTime();
406}
407
408double Macs::GetMjd()
409{
410 return fPosTime.GetMjd();
411}
412
413double Macs::GetPdoTime()
414{
415 return fPdoTime.GetTime();
416}
417
418double Macs::GetPdoMjd()
419{
420 return fPdoTime.GetMjd();
421}
422
423/* 0x2000 0 rw Maximum positioning error */
424/* 1 rw Negative Software Endswitch */
425/* 2 rw Positive Software Endswitch */
426void Macs::SetNegEndswitch(LWORDS_t val)
427{
428 SendSDO(0x2000, 1, (LWORD_t)val);
429 WaitForSdo(0x2000, 1);
430}
431
432void Macs::SetPosEndswitch(LWORDS_t val)
433{
434 SendSDO(0x2000, 2, (LWORD_t)val);
435 WaitForSdo(0x2000, 2);
436}
437
438void Macs::EnableEndswitches(bool neg, bool pos)
439{
440 SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
441 WaitForSdo(0x2000, 3);
442}
Note: See TracBrowser for help on using the repository browser.