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