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