1 | #include "macs.h"
|
---|
2 |
|
---|
3 | #include <sys/time.h> // timeval->tv_sec
|
---|
4 |
|
---|
5 | #include "network.h"
|
---|
6 | #include "MLogManip.h"
|
---|
7 |
|
---|
8 | ClassImp(Macs);
|
---|
9 |
|
---|
10 | using namespace std;
|
---|
11 |
|
---|
12 | /*
|
---|
13 | ---------------------------
|
---|
14 | For test purposes
|
---|
15 | ---------------------------
|
---|
16 |
|
---|
17 | class MyTimer : public TTimer
|
---|
18 | {
|
---|
19 | public:
|
---|
20 | MyTimer(TObject *obj, Long_t ms, Bool_t mode) : TTimer(obj, ms, mode) {}
|
---|
21 | Bool_t Notify()
|
---|
22 | {
|
---|
23 | cout << "Notify" << endl;
|
---|
24 | TTimer::Notify();
|
---|
25 | return kTRUE;
|
---|
26 | }
|
---|
27 | };
|
---|
28 | */
|
---|
29 |
|
---|
30 | Macs::Macs(const BYTE_t nodeid, const char *name)
|
---|
31 | : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
|
---|
32 | fPos(0), fPdoPos(0), fPosActive(0), fRpmActive(0),
|
---|
33 | fStatusPdo3(0xff)
|
---|
34 | {
|
---|
35 | // fTimeout = new TTimer(this, 100); //, kFALSE); // 100ms, asynchronous
|
---|
36 | }
|
---|
37 |
|
---|
38 | Macs::~Macs()
|
---|
39 | {
|
---|
40 | //fTimerOn = kFALSE;
|
---|
41 | // delete fTimeout;
|
---|
42 | }
|
---|
43 |
|
---|
44 | void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv)
|
---|
45 | {
|
---|
46 | // cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx;
|
---|
47 | // cout << ", val=0x" << val << endl;
|
---|
48 | switch (idx)
|
---|
49 | {
|
---|
50 | case 0x1003:
|
---|
51 | // FIXME, see Init
|
---|
52 | if (subidx!=2)
|
---|
53 | return;
|
---|
54 | gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << dec << val << endl;
|
---|
55 | SetError(val);
|
---|
56 | return;
|
---|
57 |
|
---|
58 | case 0x100a:
|
---|
59 | gLog << inf2 << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
|
---|
60 | fSoftVersion = val;
|
---|
61 | return;
|
---|
62 |
|
---|
63 | case 0x100b:
|
---|
64 | // Do not display, this is used for CheckConnection
|
---|
65 | // lout << "Node ID: " << dec << val << endl;
|
---|
66 | return;
|
---|
67 |
|
---|
68 | case 0x100c:
|
---|
69 | gLog << inf2 << "- " << GetNodeName() << ": Guard time:" << dec << val << endl;
|
---|
70 | return;
|
---|
71 |
|
---|
72 | case 0x100d:
|
---|
73 | gLog << inf2 << "- " << GetNodeName() << ": Life time factor:" << dec << val << endl;
|
---|
74 | return;
|
---|
75 |
|
---|
76 | case 0x2002:
|
---|
77 | gLog << inf2 << GetNodeName() << ": Current velocity: " << dec << val << endl;
|
---|
78 | fVel = val;
|
---|
79 | return;
|
---|
80 |
|
---|
81 | case 0x6004:
|
---|
82 | if (subidx)
|
---|
83 | return;
|
---|
84 |
|
---|
85 | // lout << "Actual Position: " << dec << (signed long)val << endl;
|
---|
86 | fPos = (LWORDS_t)val;
|
---|
87 | fPosTime.Set(tv);
|
---|
88 | return;
|
---|
89 | /*
|
---|
90 | case 0x2001:
|
---|
91 | cout << "Axe Status: 0x" << hex << val << endl;
|
---|
92 | cout << " - Motor " << (val&0x01?"standing":"moving") << endl;
|
---|
93 | cout << " - Positioning " << (val&0x02?"active":"inactive") << endl;
|
---|
94 | cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl;
|
---|
95 | cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
|
---|
96 | cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl;
|
---|
97 | fPosActive = val&0x02;
|
---|
98 | fRpmActive = val&0x04;
|
---|
99 | return;
|
---|
100 | case 0x2003:
|
---|
101 | if (!subidx)
|
---|
102 | {
|
---|
103 | cout << "Input State: ";
|
---|
104 | for (int i=0; i<8; i++)
|
---|
105 | cout << (int)(val&(1<<i)?1:0);
|
---|
106 | cout <<endl;
|
---|
107 | return;
|
---|
108 | }
|
---|
109 | cout << "Input No." << subidx << (val?"hi":"lo") << endl;
|
---|
110 | return;
|
---|
111 | case 0x2004:
|
---|
112 | cout << "Status Value of Axis: 0x" << hex << val << endl;
|
---|
113 | cout << " - Attitude control: " << (val&0x800000?"off":"on") << endl;
|
---|
114 | cout << " - Movement done: " << (val&0x040000?"yes":"no") << endl;
|
---|
115 | cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
|
---|
116 | cout << " - Attitude control: " << (val&0x04?"off":"on") << endl;
|
---|
117 | cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl;
|
---|
118 | cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl;
|
---|
119 | cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl;
|
---|
120 | return;
|
---|
121 | */
|
---|
122 |
|
---|
123 | case 0x6002:
|
---|
124 | gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " ticks/min" << endl;
|
---|
125 | fVelRes = val;
|
---|
126 | return;
|
---|
127 |
|
---|
128 | case 0x6501:
|
---|
129 | gLog << inf2 << "- " << GetNodeName() << ": Encoder resolution = " << dec << val << " ticks/min" << endl;
|
---|
130 | fRes = val;
|
---|
131 | return;
|
---|
132 | }
|
---|
133 |
|
---|
134 | NodeDrv::HandleSDO(idx, subidx, val, tv);
|
---|
135 |
|
---|
136 | // cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
|
---|
137 | // cout << ", val=0x"<<val<<endl;
|
---|
138 | }
|
---|
139 |
|
---|
140 | void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
|
---|
141 | {
|
---|
142 | // cout << "Node #" << dec << (int)GetId() << ": Sdo=" << hex << idx << "/" << (int)subidx << " set." << endl;
|
---|
143 |
|
---|
144 | // If a real drive operation is requested from the MACS and
|
---|
145 | // the MACS is not correctly initialized the operation is
|
---|
146 | // rejected. (This is expecially harmfull if the NoWait state
|
---|
147 | // is set incorrectly)
|
---|
148 | if (data)
|
---|
149 | SetZombie();
|
---|
150 |
|
---|
151 | switch (idx)
|
---|
152 | {
|
---|
153 | case 0x1000:
|
---|
154 | switch (subidx)
|
---|
155 | {
|
---|
156 | case 1:
|
---|
157 | //lout << ddev(MLog::eGui);
|
---|
158 | gLog << inf2 << "- " << GetNodeName() << ": State of node set." << endl;
|
---|
159 | //lout << edev(MLog::eGui);
|
---|
160 | return;
|
---|
161 | }
|
---|
162 | break;
|
---|
163 |
|
---|
164 | case 0x100c:
|
---|
165 | switch (subidx)
|
---|
166 | {
|
---|
167 | case 0:
|
---|
168 | //lout << ddev(MLog::eGui);
|
---|
169 | gLog << inf2 << "- " << GetNodeName() << ": Guard time set." << endl;
|
---|
170 | //lout << edev(MLog::eGui);
|
---|
171 | return;
|
---|
172 | }
|
---|
173 | break;
|
---|
174 |
|
---|
175 | case 0x100d:
|
---|
176 | switch (subidx)
|
---|
177 | {
|
---|
178 | case 0:
|
---|
179 | //lout << ddev(MLog::eGui);
|
---|
180 | gLog << inf2 << "- " << GetNodeName() << ": Life time factor set." << endl;
|
---|
181 | //lout << edev(MLog::eGui);
|
---|
182 | return;
|
---|
183 | }
|
---|
184 | break;
|
---|
185 |
|
---|
186 | case 0x1800:
|
---|
187 | switch (subidx)
|
---|
188 | {
|
---|
189 | case 1:
|
---|
190 | //lout << ddev(MLog::eGui);
|
---|
191 | gLog << inf2 << "- " << GetNodeName() << ": Status of PDO1 set." << endl;
|
---|
192 | //lout << edev(MLog::eGui);
|
---|
193 | return;
|
---|
194 | }
|
---|
195 | break;
|
---|
196 |
|
---|
197 | case 0x2002:
|
---|
198 | switch (subidx)
|
---|
199 | {
|
---|
200 | case 0:
|
---|
201 | //lout << ddev(MLog::eGui);
|
---|
202 | gLog << inf2 << "- " << GetNodeName() << ": Velocity set." << endl;
|
---|
203 | //lout << edev(MLog::eGui);
|
---|
204 | return;
|
---|
205 | }
|
---|
206 | break;
|
---|
207 |
|
---|
208 | case 0x2003:
|
---|
209 | switch (subidx)
|
---|
210 | {
|
---|
211 | case 0:
|
---|
212 | //lout << ddev(MLog::eGui);
|
---|
213 | gLog << inf2 << "- " << GetNodeName() << ": Acceleration set." << endl;
|
---|
214 | //lout << edev(MLog::eGui);
|
---|
215 | return;
|
---|
216 | case 1:
|
---|
217 | //lout << ddev(MLog::eGui);
|
---|
218 | gLog << inf2 << "- " << GetNodeName() << ": Deceleration set." << endl;
|
---|
219 | //lout << edev(MLog::eGui);
|
---|
220 | return;
|
---|
221 | }
|
---|
222 | break;
|
---|
223 |
|
---|
224 | case 0x3006:
|
---|
225 | switch (subidx)
|
---|
226 | {
|
---|
227 | case 0:
|
---|
228 | //lout << ddev(MLog::eGui);
|
---|
229 | gLog << inf2 << "- " << GetNodeName() << ": RPM mode switched." << endl;
|
---|
230 | //lout << edev(MLog::eGui);
|
---|
231 | return;
|
---|
232 |
|
---|
233 | case 1:
|
---|
234 | /*
|
---|
235 | lout << ddev(MLog::eGui);
|
---|
236 | lout << "- Velocity set (" << GetNodeName() << ")" << endl;
|
---|
237 | lout << edev(MLog::eGui);
|
---|
238 | */
|
---|
239 | return;
|
---|
240 | }
|
---|
241 | break;
|
---|
242 |
|
---|
243 | case 0x4000:
|
---|
244 | HandleNodeguard(tv);
|
---|
245 | return;
|
---|
246 |
|
---|
247 | case 0x6000:
|
---|
248 | //lout << ddev(MLog::eGui);
|
---|
249 | gLog << inf2 << "- " << GetNodeName() << ": Rotation direction set." << endl;
|
---|
250 | //lout << edev(MLog::eGui);
|
---|
251 | return;
|
---|
252 |
|
---|
253 | case 0x6002:
|
---|
254 | //lout << ddev(MLog::eGui);
|
---|
255 | gLog << inf2 << "- " << GetNodeName() << ": Velocitz resolution set." << endl;
|
---|
256 | //lout << edev(MLog::eGui);
|
---|
257 | return;
|
---|
258 |
|
---|
259 | case 0x6003:
|
---|
260 | switch (subidx)
|
---|
261 | {
|
---|
262 | case 0:
|
---|
263 | //lout << ddev(MLog::eGui);
|
---|
264 | gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
|
---|
265 | //lout << edev(MLog::eGui);
|
---|
266 | return;
|
---|
267 |
|
---|
268 | case 1:
|
---|
269 | //lout << ddev(MLog::eGui);
|
---|
270 | gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
|
---|
271 | //lout << edev(MLog::eGui);
|
---|
272 | return;
|
---|
273 | }
|
---|
274 | break;
|
---|
275 |
|
---|
276 | case 0x6004:
|
---|
277 | switch (subidx)
|
---|
278 | {
|
---|
279 | case 0:
|
---|
280 | //lout << ddev(MLog::eGui);
|
---|
281 | gLog << inf2 << "- " << GetNodeName() << ": Absolute positioning started." << endl;
|
---|
282 | //lout << edev(MLog::eGui);
|
---|
283 | return;
|
---|
284 |
|
---|
285 | case 1:
|
---|
286 | //lout << ddev(MLog::eGui);
|
---|
287 | gLog << inf2 << "- " << GetNodeName() << ": Relative positioning started." << endl;
|
---|
288 | //lout << edev(MLog::eGui);
|
---|
289 | return;
|
---|
290 | }
|
---|
291 | break;
|
---|
292 |
|
---|
293 |
|
---|
294 | }
|
---|
295 | NodeDrv::HandleSDOOK(idx, subidx, data, tv);
|
---|
296 | }
|
---|
297 |
|
---|
298 | void Macs::ReqVelRes()
|
---|
299 | {
|
---|
300 | gLog << inf2 << "- " << GetNodeName() << ": Requesting velocity resolution (velres, 0x6002)." << endl;
|
---|
301 | RequestSDO(0x6002);
|
---|
302 | WaitForSdo(0x6002);
|
---|
303 | }
|
---|
304 |
|
---|
305 | void Macs::ReqRes()
|
---|
306 | {
|
---|
307 | gLog << inf2 << "- " << GetNodeName() << ": Requesting encoder resolution (res, 0x6501)." << endl;
|
---|
308 | RequestSDO(0x6501);
|
---|
309 | WaitForSdo(0x6501);
|
---|
310 | }
|
---|
311 |
|
---|
312 | void Macs::SetPDO1On(BYTE_t flag)
|
---|
313 | {
|
---|
314 | gLog << inf2 << "- " << GetNodeName() << ": " << (flag?"Enable":"Disable") << " PDO1." << endl;
|
---|
315 | SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31);
|
---|
316 | WaitForSdo(0x1800, 1);
|
---|
317 | }
|
---|
318 |
|
---|
319 | void Macs::StartNode()
|
---|
320 | {
|
---|
321 | //
|
---|
322 | // Switch node from pre-operational state to operational state
|
---|
323 | // (This is not CANOpen compatible)
|
---|
324 | // After this the MACS will react on real movement commands.
|
---|
325 | //
|
---|
326 | gLog << inf2 << "- " << GetNodeName() << ": Starting Node." << endl;
|
---|
327 | SendSDO(0x1000, 1, (LWORD_t)1);
|
---|
328 | WaitForSdo(0x1000, 1);
|
---|
329 | }
|
---|
330 |
|
---|
331 | void Macs::CheckConnection()
|
---|
332 | {
|
---|
333 | RequestSDO(0x100b);
|
---|
334 | WaitForSdo(0x100b);
|
---|
335 | }
|
---|
336 |
|
---|
337 | void Macs::Init()
|
---|
338 | {
|
---|
339 | //
|
---|
340 | // Request current error status (FIXME: is the first entry in the
|
---|
341 | // error list)
|
---|
342 | //
|
---|
343 | gLog << inf2 << "- " << GetNodeName() << ": Requesting Error[0]." << endl;
|
---|
344 | RequestSDO(0x1003, 2);
|
---|
345 | WaitForSdo(0x1003, 2);
|
---|
346 | if (HasError())
|
---|
347 | {
|
---|
348 | gLog << err << "Macs::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
|
---|
349 | SetZombie();
|
---|
350 | }
|
---|
351 | if (IsZombieNode())
|
---|
352 | return;
|
---|
353 |
|
---|
354 | StopHostGuarding();
|
---|
355 | StopGuarding();
|
---|
356 |
|
---|
357 | gLog << inf2 << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl;
|
---|
358 | RequestSDO(0x100a);
|
---|
359 | WaitForSdo(0x100a);
|
---|
360 | // FIXME! Not statically linked!
|
---|
361 | //if (fSoftVersion<0x00000044) // 00.68
|
---|
362 | if (fSoftVersion<0x00000045) // 00.69
|
---|
363 | {
|
---|
364 | gLog << err << GetNodeName() << " - Software Version too old!" << endl;
|
---|
365 | SetZombie();
|
---|
366 | return;
|
---|
367 | }
|
---|
368 |
|
---|
369 | SetRpmMode(FALSE);
|
---|
370 |
|
---|
371 | ReqRes(); // Init fRes
|
---|
372 | ReqVelRes(); // Init fVelRes
|
---|
373 |
|
---|
374 | /* Should not be necessary anymore. This is done by the MACS itself.
|
---|
375 | lout << "- " << GetNodeName() << ": Motor on." << endl;
|
---|
376 | SendSDO(0x3000, string('o', 'n'));
|
---|
377 | WaitForSdo(0x3000);
|
---|
378 | */
|
---|
379 |
|
---|
380 | SetPDO1On(FALSE); // this is a workaround for the Macs
|
---|
381 | SetPDO1On(TRUE);
|
---|
382 |
|
---|
383 | //This is now standard in the MACS
|
---|
384 | //SetNoWait(TRUE);
|
---|
385 |
|
---|
386 | //StartGuarding(400, 1, kFALSE); // Using PDO1 @ 100ms
|
---|
387 | //StartGuarding(250, 4);
|
---|
388 | //StartHostGuarding();
|
---|
389 |
|
---|
390 | StartNode();
|
---|
391 | }
|
---|
392 | /*
|
---|
393 | void Macs::StopMotor()
|
---|
394 | {
|
---|
395 | //
|
---|
396 | // Stop the motor and switch off the position control unit
|
---|
397 | //
|
---|
398 | SendSDO(0x3000, string('s','t','o','p'));
|
---|
399 | WaitForSdo(0x3000);
|
---|
400 | }
|
---|
401 | */
|
---|
402 | void Macs::StopDevice()
|
---|
403 | {
|
---|
404 | //EnableTimeout(kFALSE);
|
---|
405 |
|
---|
406 | //No need to switch it off.
|
---|
407 | //SetNoWait(FALSE);
|
---|
408 |
|
---|
409 | StopHostGuarding();
|
---|
410 | StopGuarding();
|
---|
411 |
|
---|
412 | //
|
---|
413 | // FIXME: This isn't called if the initialization isn't done completely!
|
---|
414 | //
|
---|
415 |
|
---|
416 | SetRpmMode(FALSE);
|
---|
417 |
|
---|
418 | SetPDO1On(FALSE);
|
---|
419 |
|
---|
420 | /*
|
---|
421 | lout << "- " << GetNodeName() << ": Motor off." << endl;
|
---|
422 | SendSDO(0x3000, string('o', 'f', 'f'));
|
---|
423 | WaitForSdo(0x3000);
|
---|
424 | */
|
---|
425 |
|
---|
426 | /*
|
---|
427 | lout << "- Stopping Program of " << (int)GetId() << endl;
|
---|
428 | SendSDO(0x4000, (LWORD_t)0xaffe);
|
---|
429 | WaitForSdo();
|
---|
430 | */
|
---|
431 | }
|
---|
432 |
|
---|
433 | void Macs::ReqPos()
|
---|
434 | {
|
---|
435 | gLog << inf2 << "- " << GetNodeName() << ": Requesting Position." << endl;
|
---|
436 | RequestSDO(0x6004);
|
---|
437 | WaitForSdo(0x6004);
|
---|
438 | }
|
---|
439 |
|
---|
440 | void Macs::ReqVel()
|
---|
441 | {
|
---|
442 | gLog << inf2 << "- " << GetNodeName() << ": Requesting Velocity." << endl;
|
---|
443 | RequestSDO(0x2002);
|
---|
444 | WaitForSdo(0x2002);
|
---|
445 | }
|
---|
446 | /*
|
---|
447 | void Macs::SetHome(LWORDS_t pos, WORD_t maxtime)
|
---|
448 | {
|
---|
449 | StopHostGuarding();
|
---|
450 | StopGuarding();
|
---|
451 |
|
---|
452 | lout << "- " << GetNodeName() << ": Driving to home position, Offset=" << dec << pos << endl;
|
---|
453 | SendSDO(0x6003, 2, (LWORD_t)pos); // home
|
---|
454 | WaitForSdo(0x6003, 2);
|
---|
455 |
|
---|
456 | // home also defines the zero point of the system
|
---|
457 | // maximum time allowd for home drive: 25.000ms
|
---|
458 | SendSDO(0x3001, string('h','o','m','e')); // home
|
---|
459 | WaitForSdo(0x3001, 0, maxtime*1000);
|
---|
460 | lout << "- " << GetNodeName() << ": Home position reached. " << endl;
|
---|
461 |
|
---|
462 | SendSDO(0x6003, 0, string('s','e','t')); // home
|
---|
463 | WaitForSdo(0x6003, 0);
|
---|
464 |
|
---|
465 | StartGuarding();
|
---|
466 | StartHostGuarding();
|
---|
467 | }
|
---|
468 | */
|
---|
469 | void Macs::SetVelocity(LWORD_t vel)
|
---|
470 | {
|
---|
471 | SendSDO(0x2002, vel); // velocity
|
---|
472 | WaitForSdo(0x2002, 0);
|
---|
473 | }
|
---|
474 |
|
---|
475 | void Macs::SetAcceleration(LWORD_t acc)
|
---|
476 | {
|
---|
477 | SendSDO(0x2003, 0, acc); // acceleration
|
---|
478 | WaitForSdo(0x2003, 0);
|
---|
479 | }
|
---|
480 |
|
---|
481 | void Macs::SetDeceleration(LWORD_t dec)
|
---|
482 | {
|
---|
483 | SendSDO(0x2003, 1, dec); // acceleration
|
---|
484 | WaitForSdo(0x2003, 1);
|
---|
485 | }
|
---|
486 |
|
---|
487 | void Macs::SetRpmMode(BYTE_t mode)
|
---|
488 | {
|
---|
489 | //
|
---|
490 | // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
|
---|
491 | //
|
---|
492 | SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p'));
|
---|
493 | WaitForSdo(0x3006, 0);
|
---|
494 | }
|
---|
495 |
|
---|
496 | void Macs::SetRpmVelocity(LWORDS_t cvel)
|
---|
497 | {
|
---|
498 | SendSDO(0x3006, 1, (LWORD_t)cvel);
|
---|
499 | WaitForSdo(0x3006, 1);
|
---|
500 | }
|
---|
501 |
|
---|
502 | void Macs::StartRelPos(LWORDS_t pos)
|
---|
503 | {
|
---|
504 | SendSDO(0x6004, 1, (LWORD_t)pos);
|
---|
505 | }
|
---|
506 |
|
---|
507 | void Macs::StartAbsPos(LWORDS_t pos)
|
---|
508 | {
|
---|
509 | SendSDO(0x6004, 0, (LWORD_t)pos);
|
---|
510 | }
|
---|
511 |
|
---|
512 | /*
|
---|
513 | void Macs::SetNoWait(BYTE_t flag)
|
---|
514 | {
|
---|
515 | lout << "- " << GetNodeName() << ": Setting NOWAIT " << (flag?"ON":"OFF") << "." << endl;
|
---|
516 | SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f'));
|
---|
517 | WaitForSdo(0x3008);
|
---|
518 | }
|
---|
519 | */
|
---|
520 |
|
---|
521 | void Macs::StartVelSync()
|
---|
522 | {
|
---|
523 | //
|
---|
524 | // The syncronization mode is disabled by a 'MOTOR STOP'
|
---|
525 | // or by a positioning command (POSA, ...)
|
---|
526 | //
|
---|
527 | gLog << inf2 << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl;
|
---|
528 | SendSDO(0x3007, 0, string('s', 'y', 'n', 'c'));
|
---|
529 | WaitForSdo(0x3007, 0);
|
---|
530 | }
|
---|
531 |
|
---|
532 | void Macs::StartPosSync()
|
---|
533 | {
|
---|
534 | //
|
---|
535 | // The syncronization mode is disabled by a 'MOTOR STOP'
|
---|
536 | // or by a positioning command (POSA, ...)
|
---|
537 | //
|
---|
538 | gLog << inf2 << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl;
|
---|
539 | SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
|
---|
540 | WaitForSdo(0x3007, 1);
|
---|
541 | }
|
---|
542 | /*
|
---|
543 | void Macs::ReqAxEnd()
|
---|
544 | {
|
---|
545 | RequestSDO(0x2001);
|
---|
546 | WaitForSdo(0x2001);
|
---|
547 | }
|
---|
548 | */
|
---|
549 | void Macs::SendMsg(BYTE_t data[6])
|
---|
550 | {
|
---|
551 | GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
|
---|
552 | }
|
---|
553 |
|
---|
554 | void Macs::SendMsg(BYTE_t d0, BYTE_t d1, BYTE_t d2,
|
---|
555 | BYTE_t d3, BYTE_t d4, BYTE_t d5)
|
---|
556 | {
|
---|
557 | GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
|
---|
558 | }
|
---|
559 |
|
---|
560 | void Macs::HandlePDO1(const BYTE_t *data, const timeval_t &tv)
|
---|
561 | {
|
---|
562 | // FIXME!!!! Only 0x4000 should do this to be
|
---|
563 | // CanOpen conform
|
---|
564 | HandleNodeguard(tv);
|
---|
565 |
|
---|
566 | fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
|
---|
567 |
|
---|
568 | // data[3]&0x01; // motor not moving
|
---|
569 | fPosActive = data[3]&kPosActive; // positioning active
|
---|
570 | fRpmActive = data[3]&kRpmActive; // RPM mode switched on
|
---|
571 | // data[3]&0x08; // - unused -
|
---|
572 | // data[3]&0x10; // - unused -
|
---|
573 | // data[3]&0x20; // - unused -
|
---|
574 | fInControl = data[3]&0x40; // motor uncontrolled
|
---|
575 | // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
|
---|
576 |
|
---|
577 | fStatus = data[3];
|
---|
578 |
|
---|
579 | fPdoTime.Set(tv);
|
---|
580 | }
|
---|
581 |
|
---|
582 | void Macs::HandlePDO2(const BYTE_t *data, const timeval_t &tv)
|
---|
583 | {
|
---|
584 | LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
|
---|
585 | LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];
|
---|
586 |
|
---|
587 | //
|
---|
588 | // errnum==0 gives a sudden information that something happened. Now the
|
---|
589 | // microcontroller is running inside its interrupt procedure which
|
---|
590 | // stopped the normal program. The interrupt procedure should try to clear
|
---|
591 | // the error state of the hardware. This should never create a new error!
|
---|
592 | //
|
---|
593 | if (!errnum)
|
---|
594 | {
|
---|
595 | gLog << err << "- " << GetNodeName() << ": reports Error occursion." << endl;
|
---|
596 | gLog << "Macs::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
|
---|
597 | SetZombie();
|
---|
598 | SetError(-1);
|
---|
599 | return;
|
---|
600 | }
|
---|
601 |
|
---|
602 | //
|
---|
603 | // Now the error is handled by the hardware now it is the software part
|
---|
604 | // to react on it. The Error flag now is set to the correct value.
|
---|
605 | //
|
---|
606 | if (GetError()>0)
|
---|
607 | {
|
---|
608 | gLog << warn << GetNodeName() << ": WARNING! Error #" << GetError() << " unhandled (not cleared) by software." << endl;
|
---|
609 |
|
---|
610 | //
|
---|
611 | // If the error is unhadled and/or not cleared, don't try it again.
|
---|
612 | //
|
---|
613 | if (GetError()==errnum)
|
---|
614 | return;
|
---|
615 | }
|
---|
616 |
|
---|
617 | SetError(errnum);
|
---|
618 |
|
---|
619 | gLog << err << GetNodeName() << " reports: ";
|
---|
620 | switch (errnum)
|
---|
621 | {
|
---|
622 | case 3:
|
---|
623 | gLog << "Axis does not existing." << endl;
|
---|
624 | return;
|
---|
625 | case 5:
|
---|
626 | gLog << "Error not cleared (while trying to move axis)" << endl;
|
---|
627 | return;
|
---|
628 | case 6:
|
---|
629 | //
|
---|
630 | // Report the error to the user. All possible movements should have
|
---|
631 | // been stopped anyhow. Now delete the error to prevent the system
|
---|
632 | // from reporting this error a thousands of times.
|
---|
633 | //
|
---|
634 | gLog << "Home position not the first positioning command." << endl;
|
---|
635 | SetError(0);
|
---|
636 | return;
|
---|
637 | case 8:
|
---|
638 | gLog << "Control deviation overflow." << endl;
|
---|
639 | return;
|
---|
640 | case 9:
|
---|
641 | gLog << "Zero index not found." << endl;
|
---|
642 | return;
|
---|
643 | case 10:
|
---|
644 | gLog << "Unknown command, syntax error." << endl;
|
---|
645 | gLog << "Please recompile and reload program." << endl;
|
---|
646 | return;
|
---|
647 | case 11:
|
---|
648 | case 25:
|
---|
649 | switch (errinf)
|
---|
650 | {
|
---|
651 | case -1:
|
---|
652 | gLog << "Negative";
|
---|
653 | break;
|
---|
654 | case 1:
|
---|
655 | gLog << "Positive";
|
---|
656 | break;
|
---|
657 | default:
|
---|
658 | gLog << "-unknown-";
|
---|
659 | }
|
---|
660 | switch (errnum)
|
---|
661 | {
|
---|
662 | case 11:
|
---|
663 | gLog << " software";
|
---|
664 | break;
|
---|
665 | case 25:
|
---|
666 | gLog << " hardware";
|
---|
667 | break;
|
---|
668 | }
|
---|
669 | gLog << " endswitch activated." << endl;
|
---|
670 | return;
|
---|
671 | case 12:
|
---|
672 | gLog << "Wrong parameter number used in SET command." << endl;
|
---|
673 | return;
|
---|
674 | case 14:
|
---|
675 | gLog << " Too many LOOP calls." << endl;
|
---|
676 | return;
|
---|
677 | case 16:
|
---|
678 | gLog << "Parameter in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl;
|
---|
679 | gLog << "Please use APOSS to 'Reset' the MACS and reload the parameters." << endl;
|
---|
680 | return;
|
---|
681 | case 17:
|
---|
682 | gLog << "Program in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl;
|
---|
683 | gLog << "Please use APOSS to delete all Programs restore the programs." << endl;
|
---|
684 | return;
|
---|
685 | case 18:
|
---|
686 | gLog << "Reset by CPU (reset called by Watch-dog cause of CPU halted)" << endl;
|
---|
687 | gLog << "Possible reasons: short under-/overvoltage or shortcut." << endl;
|
---|
688 | return;
|
---|
689 | case 19:
|
---|
690 | gLog << "User break (autostart program stopped by user)" << endl;
|
---|
691 | return;
|
---|
692 | case 51:
|
---|
693 | gLog << "Too many (>=10) GOSUB calls." << endl;
|
---|
694 | return;
|
---|
695 | case 52:
|
---|
696 | gLog << "Too many RETURN calls." << endl;
|
---|
697 | return;
|
---|
698 | case 62:
|
---|
699 | gLog << "Error veryfiing EEPROM after access (Try again savaing parameters or program)" << endl;
|
---|
700 | return;
|
---|
701 | case 70:
|
---|
702 | gLog << "Error in DIM call (call to DIM doesn't fit existing DIM call)" << endl;
|
---|
703 | return;
|
---|
704 | case 71:
|
---|
705 | gLog << "Array out of bound." << endl;
|
---|
706 | return;
|
---|
707 | case 79:
|
---|
708 | gLog << "Timeout waiting for index (WAITNDX)." << endl;
|
---|
709 | return;
|
---|
710 | case 84:
|
---|
711 | gLog << "Too many (>12) ON TIME calls." << endl;
|
---|
712 | return;
|
---|
713 | case 87:
|
---|
714 | gLog << "Out of memory for variables - Check APOSS predifined number of" << endl;
|
---|
715 | gLog << "variables and try deleting the array by doing a 'Reset' from APOSS." << endl;
|
---|
716 | return;
|
---|
717 | case 89:
|
---|
718 | gLog << "CAN I/O error (REOPEN=" << dec << errinf << " " << (errinf==0?"OK":"ERR") << ")" << endl;
|
---|
719 | return;
|
---|
720 |
|
---|
721 | case 100:
|
---|
722 | //lout << "Connection timed out." << endl;
|
---|
723 | //EnableTimeout(false);
|
---|
724 | return;
|
---|
725 |
|
---|
726 | default:
|
---|
727 | gLog << "Error Nr. " << errnum << ", " << errinf << endl;
|
---|
728 | }
|
---|
729 | }
|
---|
730 |
|
---|
731 | void Macs::HandlePDO3(const BYTE_t *data, const timeval_t &tv)
|
---|
732 | {
|
---|
733 | // 3 5 7 9
|
---|
734 | // 1100 1010 1110 1001
|
---|
735 | if (fStatusPdo3 == data[3])
|
---|
736 | return;
|
---|
737 |
|
---|
738 | MTime time;
|
---|
739 | time.Now();
|
---|
740 |
|
---|
741 | gLog << inf << time << ": " << GetNodeName() << " - PDO3 = ";
|
---|
742 | const Bool_t ready = data[3]&0x01;
|
---|
743 | const Bool_t fuse = data[3]&0x02;
|
---|
744 | const Bool_t emcy = data[3]&0x04;
|
---|
745 | const Bool_t vltg = data[3]&0x08;
|
---|
746 | const Bool_t mode = data[3]&0x10;
|
---|
747 | const Bool_t rf = data[3]&0x20;
|
---|
748 | const Bool_t brake = data[3]&0x40;
|
---|
749 | if (ready) gLog << "DKC-Ready ";
|
---|
750 | if (fuse) gLog << "FuseOk ";
|
---|
751 | if (emcy) gLog << "EmcyOk ";
|
---|
752 | if (vltg) gLog << "OvervoltOk ";
|
---|
753 | if (mode) gLog << "SwitchToManualMode ";
|
---|
754 | if (rf) gLog << "RF ";
|
---|
755 | if (brake) gLog << "BrakeOpen ";
|
---|
756 | gLog << endl;
|
---|
757 |
|
---|
758 | fStatusPdo3 = data[3];
|
---|
759 | }
|
---|
760 |
|
---|
761 | // FIXME? Handling of fIsZombie?
|
---|
762 | void Macs::HandleError()
|
---|
763 | {
|
---|
764 | //
|
---|
765 | // If there is no error we must not handle anything
|
---|
766 | //
|
---|
767 | if (!HasError())
|
---|
768 | return;
|
---|
769 |
|
---|
770 | //
|
---|
771 | // If the program got into the: HandleError state before the hardware
|
---|
772 | // has finished handeling the error we have to wait for the hardware
|
---|
773 | // handeling the error
|
---|
774 | //
|
---|
775 | // FIXME: Timeout???
|
---|
776 | //
|
---|
777 | // while (GetError()<0)
|
---|
778 | // usleep(1);
|
---|
779 |
|
---|
780 | //
|
---|
781 | // After this software and hardware should be in a state so that
|
---|
782 | // we can go on working 'as usual' Eg. Initialize a Display Update
|
---|
783 | //
|
---|
784 | gLog << inf << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
|
---|
785 | switch (GetError())
|
---|
786 | {
|
---|
787 | case 6: // home
|
---|
788 | case 8: // control dev
|
---|
789 | case 9: // zero idx
|
---|
790 | case 84: // ON TIME
|
---|
791 | gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
|
---|
792 | return;
|
---|
793 |
|
---|
794 | case 11: // software endswitch
|
---|
795 | case 25: // hardware endswitch
|
---|
796 | gLog << "- " << GetNodeName() << ": Cannot handle error 'Endswitch!'" << endl;
|
---|
797 | return;
|
---|
798 |
|
---|
799 | case 100: // timeout (movement has been stopped, so we can go on)
|
---|
800 | DelError();
|
---|
801 | return;
|
---|
802 | /*
|
---|
803 | case 101:
|
---|
804 | //lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
|
---|
805 | DelError();
|
---|
806 | return;
|
---|
807 | */
|
---|
808 | default:
|
---|
809 | gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
|
---|
810 |
|
---|
811 | }
|
---|
812 | }
|
---|
813 |
|
---|
814 | /* 0x2000 0 rw Maximum positioning error */
|
---|
815 | /* 1 rw Negative Software Endswitch */
|
---|
816 | /* 2 rw Positive Software Endswitch */
|
---|
817 | void Macs::SetNegEndswitch(LWORDS_t val)
|
---|
818 | {
|
---|
819 | SendSDO(0x2000, 1, (LWORD_t)val);
|
---|
820 | WaitForSdo(0x2000, 1);
|
---|
821 | }
|
---|
822 |
|
---|
823 | void Macs::SetPosEndswitch(LWORDS_t val)
|
---|
824 | {
|
---|
825 | SendSDO(0x2000, 2, (LWORD_t)val);
|
---|
826 | WaitForSdo(0x2000, 2);
|
---|
827 | }
|
---|
828 |
|
---|
829 | void Macs::EnableEndswitches(bool neg, bool pos)
|
---|
830 | {
|
---|
831 | SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1)));
|
---|
832 | WaitForSdo(0x2000, 3);
|
---|
833 | }
|
---|
834 |
|
---|
835 | void Macs::SendNodeguard()
|
---|
836 | {
|
---|
837 | SendSDO(0x4000, 0, (LWORD_t)0, false);
|
---|
838 | }
|
---|
839 |
|
---|
840 | // --------------------------------------------------------------------------
|
---|
841 | //
|
---|
842 | // This starts the host guarding. The host guarding is only available
|
---|
843 | // if the node guarding is running. The host guarding works with the
|
---|
844 | // guardtime and the lifetimefactor from the nodeguarding.
|
---|
845 | //
|
---|
846 | void Macs::StartHostGuarding()
|
---|
847 | {
|
---|
848 | SendSDO(0x100c, 0, (LWORD_t)GetGuardTime());
|
---|
849 | WaitForSdo(0x100c);
|
---|
850 |
|
---|
851 | SendSDO(0x100d, 0, (LWORD_t)GetLifeTimeFactor());
|
---|
852 | WaitForSdo(0x100d);
|
---|
853 |
|
---|
854 | gLog << inf2 << "- " << GetNodeName() << ": Hostguarding started (" << dec;
|
---|
855 | gLog << GetLifeTimeFactor() << "*" << GetGuardTime() << "ms)" << endl;
|
---|
856 | }
|
---|
857 |
|
---|
858 | // --------------------------------------------------------------------------
|
---|
859 | //
|
---|
860 | // Stop the host guarding.
|
---|
861 | //
|
---|
862 | void Macs::StopHostGuarding()
|
---|
863 | {
|
---|
864 | SendSDO(0x100c, 0, (LWORD_t)0);
|
---|
865 | WaitForSdo(0x100c);
|
---|
866 |
|
---|
867 | SendSDO(0x100d, 0, (LWORD_t)0);
|
---|
868 | WaitForSdo(0x100d);
|
---|
869 |
|
---|
870 | gLog << inf2 << "- " << GetNodeName() << ": Hostguarding stopped." << endl;
|
---|
871 | }
|
---|
872 |
|
---|