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

Last change on this file since 770 was 732, checked in by tbretz, 24 years ago
*** empty log message ***
File size: 8.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), fError(FALSE)
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*/
120 lout << "- Requesting Mac Software Version of " << (int)GetId() << endl;
121 RequestSDO(0x100a);
122 WaitForSdo(0x100a);
123 //
124 SetRpmMode(FALSE);
125
126 ReqVelRes(); // Init fVelRes
127
128 lout << "- Motor on of " << (int)GetId() << endl;
129 SendSDO(0x3000, string('O', 'N'));
130 WaitForSdo(0x3000);
131
132
133 SetHome(250000);
134
135// lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
136// RequestSDO(0x2001);
137// WaitForSdo(0x2001);
138
139 SetPDO1On(FALSE); // this is a workaround for the Macs
140 SetPDO1On(TRUE);
141
142 SetNoWait(TRUE);
143}
144
145void Macs::StopMotor()
146{
147 //
148 // Stop the motor and switch off the position control unit
149 //
150 SendSDO(0x3000, string('S','T','O','P'));
151 WaitForSdo(0x3000);
152}
153
154void Macs::StopDevice()
155{
156 SetNoWait(FALSE);
157
158 //
159 // FIXME: This isn't called if the initialization isn't done completely!
160 //
161
162 SetRpmMode(FALSE);
163
164 SetPDO1On(FALSE);
165
166 lout << "- Motor off of " << (int)GetId() << endl;
167 SendSDO(0x3000, string('O', 'F', 'F'));
168 WaitForSdo(0x3000);
169
170 /*
171 lout << "- Stopping Program of " << (int)GetId() << endl;
172 SendSDO(0x4000, (LWORD_t)0xaffe);
173 WaitForSdo();
174 */
175}
176
177void Macs::ReqPos()
178{
179 lout << "- Requesting Position of #" << (int)GetId() << endl;
180 RequestSDO(0x6004);
181 WaitForSdo(0x6004);
182}
183
184void Macs::ReqVel()
185{
186 lout << "- Requesting Velocity of #" << (int)GetId() << endl;
187 RequestSDO(0x2002);
188 WaitForSdo(0x2002);
189}
190
191void Macs::SetHome(LWORDS_t pos)
192{
193 lout << "- Driving #" << (int)GetId() << " to home position, Offset=" << dec << pos << endl;
194 SendSDO(0x6003, 2, (LWORD_t)pos); // home
195 WaitForSdo(0x6003, 2);
196
197 SendSDO(0x3001, string('h','o','m','e')); // home
198 WaitForSdo(0x3001);
199 lout << "- Home position of #" << (int)GetId() << " reached. " << endl;
200}
201
202void Macs::SetVelocity(LWORD_t vel)
203{
204 SendSDO(0x2002, vel); // velocity
205 WaitForSdo(0x2002);
206}
207
208void Macs::SetAcceleration(LWORD_t acc)
209{
210 SendSDO(0x2003, 0, acc); // acceleration
211 WaitForSdo(0x2003, 0);
212}
213
214void Macs::SetDeceleration(LWORD_t dec)
215{
216 SendSDO(0x2003, 1, dec); // acceleration
217 WaitForSdo(0x2003, 1);
218}
219
220void Macs::SetRpmMode(BYTE_t mode)
221{
222 //
223 // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
224 //
225
226 SendSDO(0x3006, 0, mode ? string('S','T','R','T') : string('S','T','O','P'));
227 WaitForSdo(0x3006, 0);
228}
229
230void Macs::SetRpmVelocity(LWORDS_t cvel)
231{
232 SendSDO(0x3006, 1, (LWORD_t)cvel);
233 WaitForSdo(0x3006, 1);
234}
235
236void Macs::StartRelPos(LWORDS_t pos)
237{
238 SendSDO(0x6004, 1, (LWORD_t)pos);
239}
240
241void Macs::StartAbsPos(LWORDS_t pos)
242{
243 SendSDO(0x6004, 0, (LWORD_t)pos);
244}
245
246void Macs::SetNoWait(BYTE_t flag)
247{
248 lout << "- Setting NOWAIT " << (flag?"ON":"OFF") << " #" << (int)GetId() << endl;
249 SendSDO(0x3008, flag ? string('O', 'N') : string('O', 'F', 'F'));
250 WaitForSdo(0x3008);
251}
252/*
253void Macs::ReqAxEnd()
254{
255 RequestSDO(0x2001);
256 WaitForSdo(0x2001);
257}
258*/
259void Macs::SendMsg(BYTE_t data[6])
260{
261 GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
262}
263
264void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
265 BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
266{
267 GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
268}
269
270void Macs::HandlePDO1(BYTE_t *data, struct timeval *tv)
271{
272 fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
273
274 fPosActive = data[3]&0x02;
275 fRpmActive = data[3]&0x04;
276
277 fPdoTime.SetTimer(tv);
278}
279
280void Macs::HandlePDO2(BYTE_t *data, struct timeval *tv)
281{
282 LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
283 LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
284
285 if (!errnum)
286 {
287 cout << "Mac #" << (int)GetId() << " reports Error occursion." << endl;
288 fError = TRUE;
289 return;
290 }
291
292 cout << "Mac #" << (int)GetId() << " reports: ";
293 switch (errnum)
294 {
295 case 6:
296 cout << "Home position not the first positioning command." << endl;
297 return;
298
299 case 8:
300 cout << "Control deviation overflow." << endl;
301 return;
302
303 case 9:
304 cout << "Zero index not found." << endl;
305 return;
306
307 case 25:
308 switch (errinf)
309 {
310 case 1:
311 cout << "Positive";
312 break;
313 case 2:
314 cout << "Negative";
315 break;
316 default:
317 cout << "-unknown-";
318 }
319 cout << " endswitch activated." << endl;
320 fError = FALSE;
321 return;
322
323 case 84:
324 cout << "Too many (>12) ON TIME calls." << endl;
325 return;
326
327 default:
328 cout << "Error Nr. " << errnum << ", " << errinf << endl;
329 }
330}
331
332double Macs::GetTime()
333{
334 return fPosTime.GetTime();
335}
336
337double Macs::GetMjd()
338{
339 return fPosTime.GetMjd();
340}
341
342double Macs::GetPdoTime()
343{
344 return fPdoTime.GetTime();
345}
346
347double Macs::GetPdoMjd()
348{
349 return fPdoTime.GetMjd();
350}
351
Note: See TracBrowser for help on using the repository browser.