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

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