source: trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc@ 8875

Last change on this file since 8875 was 8869, checked in by tbretz, 17 years ago
*** empty log message ***
File size: 12.1 KB
Line 
1#include "shaftencoder.h"
2
3#include <TGLabel.h> // TGLabel->SetText
4
5#include "MLog.h"
6#include "MLogManip.h"
7
8#include "macs.h"
9#include "network.h"
10
11ClassImp(ShaftEncoder);
12
13using namespace std;
14
15ShaftEncoder::ShaftEncoder(const BYTE_t nodeid, const char *name)
16 : NodeDrv(nodeid, name),
17 fPos(0), fVel(0), fAcc(0), fTurn(0), fTicks(0), fTurns(0),
18 fDirection(kUndefined), fHysteresisPos(0), fHysteresisNeg(0),
19 fLabel(NULL), fUpdPos(0),
20 fPosHasChanged(false), fDirHasChanged(false),
21 fReport(NULL), fMotor(NULL),
22 fOffset(0), fDirChangedPos(0), fDirChangedOffset(0)
23{
24}
25/*
26void ShaftEncoder::CheckTwin(Int_t diff) const
27{
28 if (!fTwin)
29 return;
30
31 if (fTwin->fIsUpdated)
32 fTwin->fIsUpdated = kFALSE;
33 else
34 fTwin->fOffset += diff;
35}
36*/
37void ShaftEncoder::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv)
38{
39 switch (idx)
40 {
41 case 0x1000:
42 gLog << inf2 << "- Model: ";
43 switch (val&0xffff)
44 {
45 case 0x0196:
46 gLog << inf2 << "Shaft Encoder Type: ";
47 switch ((val>>16)&0xff)
48 {
49 case 0x01:
50 gLog << "Singleturn" << endl;
51 return;
52 case 0x02:
53 gLog << "Multiturn" << endl;
54 return;
55 default:
56 gLog << err << "?" << endl;
57 SetZombie();
58 return;
59 }
60 default:
61 gLog << err << "???" << endl;
62 SetZombie();
63 return;
64 }
65 case 0x100b:
66 // Do not display, this is used for CheckConnection
67 // lout << "Node ID: " << dec << val << endl;
68 return;
69
70 case 0x100c:
71 gLog << inf2 << "- Guardtime: " << dec << val << "ms" << endl;
72 return;
73
74 case 0x100d:
75 gLog << inf2 << "- Lifetimefactor: " << dec << val << endl;
76 return;
77
78 case 0x100e:
79 gLog << inf2 << "- CobId for guarding: 0x" << hex << val << endl;
80 return;
81
82 case 0x6000:
83 case 0x6500:
84 gLog << inf2 << "- Counting: " << (val&1 ?"anti-clockwise":"clockwise") << " ";
85 gLog << "HwTest: " << (val&2 ?"on":"off") << " ";
86 gLog << "Scaling: " << (val&4 ?"on":"off") << " ";
87 gLog << "Modulo: " << (val&4096?"on":"off") << endl;
88 return;
89
90 case 0x6001:
91 gLog << inf2 << "- Logical Ticks/Revolution: " << dec << val << endl;
92 return;
93
94 case 0x6004:
95 gLog << inf2 << "- Reported position: " << dec << (Int_t)val << endl;
96 fPos = val;
97 fTurn = 0;
98 fDirChangedPos = val;
99 fOffset = fMotor ? fMotor->GetPdoPos() : 0;
100 fPosHasChanged = true;
101 fDirHasChanged = true;
102 fDirection = kUndefined;
103 //fIsUpdated=kTRUE;
104 //fOffset = 0;
105 return;
106
107
108 case 0x6501:
109 gLog << inf2 << "- Phys. Ticks/Revolution: " << dec << val << endl;
110 fTicks = val;
111 return;
112
113 case 0x6502:
114 //if (val==0)
115 // val = 1; // Single Turn = Multiturn with one turn
116 gLog << inf2 << "- Number of Revolutions: " << dec << val << endl;
117 fTurns = val;
118 return;
119
120
121 }
122
123 NodeDrv::HandleSDO(idx, subidx, val, tv);
124
125// gLog << err << hex << setfill('0');
126// gLog << "Sdo=" << idx << "/" << (int)subidx << ": 0x" << setw(8) << val;
127// gLog << endl;
128}
129
130void ShaftEncoder::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
131{
132 switch (idx)
133 {
134 case 0x1802:
135 switch (subidx)
136 {
137 case 1:
138 //lout << ddev(MLog::eGui);
139 gLog << inf2 << "- " << GetNodeName() << ": PDOs configured." << endl;
140 //lout << edev(MLog::eGui);
141 return;
142 }
143 break;
144
145 case 0x6001:
146 switch (subidx)
147 {
148 case 0:
149 //lout << ddev(MLog::eGui);
150 gLog << inf2 << "- " << GetNodeName() << ": Log.ticks/revolution set." << endl;
151 //lout << edev(MLog::eGui);
152 return;
153 }
154 break;
155
156 case 0x6002:
157 switch (subidx)
158 {
159 case 0:
160 //lout << ddev(MLog::eGui);
161 gLog << inf2 << "- " << GetNodeName() << ": Max number of ticks set." << endl;
162 //lout << edev(MLog::eGui);
163 return;
164 }
165 break;
166
167 case 0x6003:
168 switch (subidx)
169 {
170 case 0:
171 //lout << ddev(MLog::eGui);
172 gLog << inf2 << "- " << GetNodeName() << ": Preset value set." << endl;
173 //lout << edev(MLog::eGui);
174 return;
175 }
176 break;
177 }
178 NodeDrv::HandleSDOOK(idx, subidx, data, tv);
179}
180
181void ShaftEncoder::DisplayVal()
182{
183 const LWORDS_t pos = GetPos();
184 if (IsZombieNode())
185 {
186 fLabel->SetText(new TGString(""));
187 fUpdPos = ~pos;
188 return;
189 }
190
191 char text[21]="";
192 if (pos!=fUpdPos && fLabel)
193 {
194 sprintf(text, "%ld", pos);
195 fLabel->SetText(new TGString(text));
196 fUpdPos = pos;
197 }
198}
199
200void ShaftEncoder::HandlePDOType0(const BYTE_t *data, const timeval_t &tv)
201{
202 //
203 // Decode information, we have a 14bit only
204 //
205 LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24);
206
207 //if (pos==fPos)
208 // return;
209
210 // Warning: A multiturn shaftencoder is assumed!
211 if ((pos>fDirChangedPos && pos<fPos) ||
212 (pos<fDirChangedPos && pos>fPos))
213 {
214 fDirChangedPos = pos;
215 fDirHasChanged = true;
216 }
217 else
218 fOffset = fMotor ? fMotor->GetPdoPos() : 0;
219
220 fPos = pos;
221 fTime.Set(tv);
222 fPosHasChanged = true;
223
224 //CheckTwin(fPos-pos);
225 //fIsUpdated=kTRUE;
226
227 if (fReport)
228 {
229 fReport->Lock("ShaftEncoder::HandlePDOType0");
230 *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO0 " << pos << " " << GetNodeName() << endl;
231 fReport->UnLock("ShaftEncoder::HandlePDOType0");
232 }
233}
234
235void ShaftEncoder::HandlePDOType1(const BYTE_t *data, const timeval_t &tv)
236{
237 //
238 // Decode information, we have a 14bit only
239 //
240 LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24);
241 BYTE_t flag = data[4];
242
243 //if (fPos==pos)
244 // return;
245
246 // Warning: A multiturn shaftencoder is assumed!
247 if ((pos>fDirChangedPos && pos<fPos) ||
248 (pos<fDirChangedPos && pos>fPos))
249 {
250 fDirChangedPos = pos;
251 fDirHasChanged = true;
252 }
253 else
254 fOffset = fMotor ? fMotor->GetPdoPos() : 0;
255
256 //CheckTwin(fPos-pos);
257 fPos=pos;
258 fTime.Set(tv);
259 fPosHasChanged=true;
260 //fIsUpdated=kTRUE;
261 //fOffset = 0;
262
263 flag=flag;
264
265 if (fReport)
266 {
267 fReport->Lock("ShaftEncoder::HandlePDOType1");
268 *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO1 " << pos << " " << (int)flag << " " << GetNodeName() << endl;
269 fReport->UnLock("ShaftEncoder::HandlePDOType1");
270 }
271}
272
273//#include <fstream.h>
274//ofstream fout("log/shaftencoder.log");
275
276void ShaftEncoder::HandlePDOType2(const BYTE_t *data, const timeval_t &tv)
277{
278 //
279 // Decode information, we have a 14bit only
280 //
281 LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24);
282
283 fVel = data[4] | (data[5]<<8);
284 fAcc = data[6] | (data[7]<<8);
285
286 const int dnlim = fTicks/10;
287 const int uplim = fTurns*fTicks-dnlim;
288
289 int turn = fTurn;
290
291 if (fPos > uplim && pos < dnlim)
292 turn++;
293
294 if (fPos < dnlim && pos > uplim)
295 turn--;
296
297 LWORDS_t multipos = pos+turn*fTicks; // SE position of multiturn shaftencoder
298
299 //if (fPos==pos && fTurn==fTurn)
300 // return;
301
302 //CheckTwin(fPos-pos);
303
304 fOffset = fMotor ? fMotor->GetPdoPos() : 0;
305
306 // Warning: A multiturn shaftencoder is assumed!
307 if ((multipos>fDirChangedPos && multipos<fPos) ||
308 (multipos<fDirChangedPos && multipos>fPos))
309 {
310 fDirChangedPos = GetPos();
311 fDirHasChanged = true;
312 fDirChangedOffset = fOffset;
313 }
314 else
315 {
316 fDirHasChanged = false;
317 }
318
319 if (multipos>GetPos())
320 fDirection = kForward;
321 else
322 if (multipos<GetPos())
323 fDirection = kBackward;
324 else
325 fDirection = kUndefined;
326
327
328 fPos = pos;
329 fTurn = turn;
330
331 fTime.Set(tv);
332 fPosHasChanged=true;
333 //fIsUpdated=kTRUE;
334 //fOffset = 0;
335
336 if (fReport)
337 {
338 fReport->Lock("ShaftEncoder::HandlePDOType2");
339 *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO2 " << pos << " " << fVel << " " << fAcc << " " << GetNodeName() << endl;
340 *fReport << "DIR-REPORT " << (int)GetId() << " " << (Int_t)fDirHasChanged << " " << (Int_t)fDirChangedPos << endl;
341 fReport->UnLock("ShaftEncoder::HandlePDOType2");
342 }
343}
344
345double ShaftEncoder::GetMjd()
346{
347 return fTime.GetMjd();
348}
349
350void ShaftEncoder::Init()
351{
352 //-----------------------------------------------------------------------
353 // Start Setup of the Shaft Encoder
354 //-----------------------------------------------------------------------
355
356 StopGuarding();
357
358 //
359 // Requesting and checking (FIXME) type of encoder
360 //
361 gLog << inf2 << "- " << GetNodeName() << ": Requesting Hardware Type (0x1000)." << endl;
362 RequestSDO(0x1000);
363 WaitForSdo(0x1000);
364 if (IsZombieNode())
365 return;
366
367 //
368 // Read physical ticks per revolution
369 //
370 gLog << inf2 << "- " << GetNodeName() << ": Requesting physical ticks/revolution (SDO 0x6501)." << endl;
371 RequestSDO(0x6501);
372 WaitForSdo(0x6501);
373
374 //
375 // Read number of possible ticks per revolution
376 //
377 gLog << inf2 << "- " << GetNodeName() << ": Requesting possible ticks/revolution (SDO 0x6502)." << endl;
378 RequestSDO(0x6502);
379 WaitForSdo(0x6502);
380
381 //
382 // Request Lifetimefactor for unknown reason to make guarding
383 // working in SE/Az... (FIXME)
384 //
385 // lout << "- " << GetNodeName() << ": Requesting Lifetimefactor (Workaround, FIXME!) (SDO 0x100d)." << endl;
386 // RequestSDO(0x100c);
387 // WaitForSdo(0x100c);
388 // RequestSDO(0x100d);
389 // WaitForSdo(0x100d);
390
391 //
392 // Set logic ticks/revolution = physical ticks/revolution => scale factor = 1
393 //
394 gLog << inf2 << "- " << GetNodeName() << ": Configuring log. tick/rev (0x6001)." << endl;
395 SendSDO(0x6001, fTicks);
396 WaitForSdo(0x6001);
397
398 //
399 // Set maximum number of ticks (ticks * turns)
400 //
401 gLog << inf2 << "- " << GetNodeName() << ": Configuring max number of ticks (0x6002)." << endl;
402 SendSDO(0x6002, (LWORD_t)(fTicks*fTurns));
403 WaitForSdo(0x6002);
404
405 //
406 // Delete preset Value
407 //
408 gLog << inf2 << "- " << GetNodeName() << ": Delete preset value (0x6003)." << endl;
409 SendSDO(0x6003, (LWORD_t)0xffffffff);
410 WaitForSdo(0x6003);
411
412 //
413 // Configure PDOs
414 //
415 gLog << inf2 << "- " << GetNodeName() << ": Configuring PDOs (0x1802)." << endl;
416 SendSDO(0x1802, 1, (LWORD_t)0x281);
417 WaitForSdo(0x1802, 1);
418
419 //
420 // Request Parameter
421 //
422 gLog << inf2 << "- " << GetNodeName() << ": Requesting SDO 0x6000." << endl;
423 RequestSDO(0x6000);
424 WaitForSdo(0x6000);
425
426 ReqPos();
427
428 gLog << inf2 << "- " << GetNodeName() << ": Start Node (NMT)." << endl;
429 SendNMT(kNMT_START);
430
431 /*
432 cout << "---1---" << endl;
433 MTimeout t(1000);
434 while (!t.HasTimedOut())
435 usleep(1);
436 cout << "---2---" << endl;
437 */
438
439 // StartGuarding(200, 1, kTRUE); // 175
440 // StartGuarding(10*GetId(), 2); // 175
441}
442
443void ShaftEncoder::CheckConnection()
444{
445 // Request Node number
446 RequestSDO(0x100b);
447 WaitForSdo(0x100b);
448}
449
450void ShaftEncoder::ReqPos()
451{
452 //
453 // Request Position
454 //
455 gLog << inf2 << "- " << GetNodeName() << ": Requesting Position." << endl;
456 RequestSDO(0x6004);
457 WaitForSdo(0x6004);
458}
459
460void ShaftEncoder::SetPreset(LWORD_t pre)
461{
462 gLog << inf2 << "- " << GetNodeName() << ": Setting Preset." << endl;
463
464 SendSDO(0x6003, (LWORD_t)pre);
465 if (!WaitForSdo(0x6003))
466 return;
467
468 fPos = pre%fTicks;
469 fTurn = pre/fTicks;
470 fDirChangedPos = fPos;
471 fOffset = fMotor ? fMotor->GetPdoPos() : 0;
472
473 fPosHasChanged = true;
474 fDirHasChanged = true;
475
476 fDirection = kUndefined;
477}
478
479void ShaftEncoder::StopDevice()
480{
481 gLog << inf2 << "- " << GetNodeName() << ": Stop Node (NMT)." << endl;
482 SendNMT(kNMT_STOP);
483}
484
Note: See TracBrowser for help on using the repository browser.