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

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