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

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