source: trunk/MagicSoft/Cosy/main/MPointing.cc@ 4002

Last change on this file since 4002 was 3935, checked in by tbretz, 22 years ago
*** empty log message ***
File size: 13.2 KB
Line 
1#include "MPointing.h"
2
3#include "MCosy.h"
4#include "macs.h"
5#include "MDriveCom.h"
6
7ClassImp(MPointing);
8
9//#define EXPERT
10#undef EXPERT
11
12// --------------------------------------------------------------------------
13//
14// set the velocity and accelerations for position maneuvers.
15//
16// The acceleratin is set as given (in percent of maximum).
17// The velocity is given in percent, depending on the ratio (<1 or >1)
18// one of the axis becomes a slower velocity. This is used for maneuvers
19// in which both axis are moved synchromously and should reach their
20// target position at the same time.
21//
22void MPointing::SetPosVelocity(const Float_t ratio, Float_t vel)
23{
24 //
25 // Set velocities
26 //
27 const int vr = fCosy->fMac1->GetVelRes();
28 vel *= vr;
29
30 if (ratio<1)
31 {
32 fCosy->fMac1->SetVelocity(vel);
33 fCosy->fMac2->SetVelocity(vel*ratio);
34 }
35 else
36 {
37 fCosy->fMac1->SetVelocity(vel/ratio);
38 fCosy->fMac2->SetVelocity(vel);
39 }
40}
41
42// --------------------------------------------------------------------------
43//
44// Does a relative positioning.
45//
46// The steps to move are given in a ZdAz object relative to the current
47// position. The coordinates are given in Roteryencoder steps.
48// Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only
49// if Axis 2==kTRUE. The function waits for the movement to be finished.
50//
51void MPointing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
52{
53 if (fCosy->HasZombie())
54 return;
55
56 fCosy->SetStatus(MDriveCom::kMoving);
57
58 if (axe1) fCosy->fMac2->StartRelPos(rd.Zd());
59 if (axe2) fCosy->fMac1->StartRelPos(rd.Az());
60#ifdef EXPERT
61 cout << "Waiting for positioning..." << flush;
62#endif
63 if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1);
64 if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1);
65
66 fCosy->WaitForEndMovement();
67#ifdef EXPERT
68 cout << "done." << endl;
69#endif
70}
71
72bool MPointing::SetAccDec(Macs *mac, Float_t acc, Float_t dec)
73{
74 const int vr = mac->GetVelRes();
75 mac->SetAcceleration(acc*vr);
76 mac->SetAcceleration(dec*vr);
77 return !mac->IsZombieNode();
78}
79
80bool MPointing::Break()
81{
82 return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie();
83}
84
85// --------------------------------------------------------------------------
86//
87// Move the telescope to the given position. The position must be given in
88// a ZdAz object in rad.
89//
90// The first positioning is done absolutely. If we didn't reach the
91// correct psotion we try to correct for this by 10 relative position
92// maneuvers. If this doesn't help positioning failed.
93//
94// As a reference the shaftencoder values are used.
95//
96int MPointing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
97{
98 /*
99 const ZdAz d = dst*kRad2Deg;
100
101 MTime t(-1);
102 lout << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
103
104 //
105 // Calculate new target position (shortest distance to go)
106 //
107 const ZdAz src = fCosy->GetSePos(); // [se]
108
109 //
110 // Make sure that the motors are in sync mode (necessary if the
111 // MACS has been rebooted from a Zombie state.
112 //
113 //InitSync();
114 //if (fMac3->IsZombieNode())
115 // return false;
116
117 //
118 // Because we agreed on I don't search for the shortest move
119 // anymore
120 //
121 // const ZdAz dest = CorrectTarget(src, dst);
122 //
123 ZdAz bend = fCosy->fBending(dst); // [rad]
124
125 const ZdAz dest = bend*16384/2/TMath::Pi(); // [se]
126
127 if (!fCosy->CheckRange(bend))
128 return kFALSE;
129
130 bend *= kRad2Deg;
131 fCosy->fZdAzSoll = dst;
132
133 cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl;
134 cout << "Destination Zd: " << dst.Zd()*8192/TMath::Pi() << "se Az:" << dst.Az()*8192/TMath::Pi() << "se" << endl;
135 cout << "Bend'd Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl;
136 cout << "Bend'd Dest Zd: " << bend.Zd() << "deg Az:" << bend.Az() << "deg" << endl;
137
138 int i;
139 for (i=0; i<(track?1:10) && !Break(); i++)
140 {
141
142 lout << "- Step #" << i << endl;
143 //
144 // Get Shaft Encoder Positions
145 //
146 const ZdAz p=fCosy->GetSePos();
147
148 //
149 // calculate control deviation and rounded cd
150 //
151 ZdAz rd = dest-p; // [se]
152
153 // ===========================================
154 const ZdAz ist = dst-rd*TMath::Pi()/8192;
155
156 const double p1 = ist.Zd()-19.0605/kRad2Deg;
157 const double p2 = dst.Zd()-19.0605/kRad2Deg;
158
159 const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*8192/TMath::Pi();
160 const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi();
161 // ===========================================
162
163 ZdAz cd = rd; // [se]
164 cd.Round();
165
166 //
167 // Check if there is a control deviation on the axis
168 //
169 const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
170 const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
171
172 //
173 // check if we reached the correct position already
174 //
175 if (!cdzd && !cdaz)
176 {
177 t.Now();
178 lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
179 fCosy->SetStatus(MDriveCom::kStopped);
180 fCosy->fCom->Send("POSITION DONE");
181 return TRUE;
182 }
183
184 //
185 // change units from se to re
186 //
187 rd *= fCosy->kGearRatio; // [re]
188 rd.Zd(f2-f1);
189
190 //
191 // Initialize Velocities so that we reach both positions
192 // at the same time
193 //
194 if (i)
195 {
196 lout << "--- LO-SPEED ---" << endl;
197 SetAccDec(fCosy->fMac1, 0.1, 0.1);
198 SetAccDec(fCosy->fMac2, 0.1, 0.1);
199
200 SetPosVelocity(1.0, 0.05);
201 }
202 else
203 {
204 const Double_t y = 15*fCosy->kGearRatio.Y();
205 if (rd.Az()>-y && rd.Az()<y)
206 {
207 lout << "--- LO-SPEED Mac1 ---" << endl;
208 SetAccDec(fCosy->fMac1, 0.05, 0.05);
209 }
210 else
211 SetAccDec(fCosy->fMac1, fAcc, fDec);
212
213 SetAccDec(fCosy->fMac2, fAcc, fDec);
214
215 SetPosVelocity(fabs(rd.Ratio()), fVel);
216 }
217
218 rd.Round();
219
220 // FIXME? Check for Error or Zombie?
221
222 // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
223 // cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
224 // cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
225 // cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
226 // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
227
228 //
229 // repositioning (relative)
230 //
231
232 lout << "- Do Relative Positioning..." << endl;
233 DoRelPos(rd, cdzd, cdaz);
234 lout << "- Relative Positioning Done" << endl;
235 }
236
237
238 return FALSE;*/
239
240 const ZdAz d = dst*kRad2Deg;
241
242 MTime t(-1);
243 lout << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
244
245 //
246 // Calculate new target position (shortest distance to go)
247 //
248 //const ZdAz src = fCosy->GetSePos(); // [se]
249
250 //
251 // Make sure that the motors are in sync mode (necessary if the
252 // MACS has been rebooted from a Zombie state.
253 //
254 //InitSync();
255 //if (fMac3->IsZombieNode())
256 // return false;
257
258 //
259 // Because we agreed on I don't search for the shortest move
260 // anymore
261 //
262 // const ZdAz dest = CorrectTarget(src, dst);
263 //
264 ZdAz bend = fCosy->fBending(dst); // [rad]
265
266 const ZdAz dest = bend*16384/2/TMath::Pi(); // [se]
267
268 if (!fCosy->CheckRange(bend))
269 return kFALSE;
270
271 bend *= kRad2Deg;
272 fCosy->fZdAzSoll = dst;
273
274 //cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl;
275 //cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl;
276 //cout << "Bend'd Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl;
277 //cout << "Bend'd Dest Zd: " << bend.Zd() << "deg Az:" << bend.Az() << "deg" << endl;
278
279 //
280 // Set velocities
281 //
282 //const int vr = fCosy->fMac1->GetVelRes();
283
284 int i;
285 for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++)
286 {
287
288 lout << "- Step #" << i << endl;
289 //
290 // Get Shaft Encoder Positions
291 //
292 const ZdAz p=fCosy->GetSePos();
293
294 //
295 // calculate control deviation and rounded cd
296 //
297 ZdAz rd = dest-p; // [se]
298
299 // ===========================================
300 const ZdAz ist = dst-rd*TMath::Pi()/8192;
301
302 const double p1 = ist.Zd()-19.0605/kRad2Deg;
303 const double p2 = dst.Zd()-19.0605/kRad2Deg;
304
305 const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*8192/TMath::Pi();
306 const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi();
307 // ===========================================
308
309 ZdAz cd = rd; // [se]
310 cd.Round();
311
312 //
313 // Check if there is a control deviation on the axis
314 //
315 const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
316 const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
317
318 //
319 // check if we reached the correct position already
320 //
321 if (!cdzd && !cdaz)
322 {
323 t.Now();
324 lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
325 fCosy->SetStatus(MDriveCom::kStopped);
326 fCosy->fCom->Send("POSITION DONE");
327 return TRUE;
328 }
329
330 //
331 // change units from se to re
332 //
333 rd *= fCosy->kGearRatio; // [re]
334 rd.Zd(f2-f1);
335
336 //
337 // Initialize Velocities so that we reach both positions
338 // at the same time
339 //
340/* if (i)
341 {
342 fCosy->fMac1->SetAcceleration(0.1*vr);
343 fCosy->fMac2->SetAcceleration(0.1*vr);
344
345 fCosy->fMac1->SetDeceleration(0.1*vr);
346 fCosy->fMac2->SetDeceleration(0.1*vr);
347
348 fCosy->SetPosVelocity(1.0, 0.05);
349 }
350 else
351 {
352 if (rd.Az()>-15*fCosy->kGearRatio.Y() && rd.Az()<15*fCosy->kGearRatio.Y())
353 {
354#ifdef EXPERT
355 cout << " -------------- LO ---------------- " << endl;
356#endif
357 fCosy->fMac1->SetAcceleration(0.05*vr);
358 fCosy->fMac1->SetDeceleration(0.05*vr);
359 }
360 else
361 {
362#ifdef EXPERT
363 cout << " -------------- HI ---------------- " << endl;
364 fCosy->fMac1->SetAcceleration(0.4*vr);// 0.4
365 fCosy->fMac1->SetDeceleration(0.4*vr);// 0.4
366#else
367 fCosy->fMac1->SetAcceleration(0.2*vr);
368 fCosy->fMac1->SetDeceleration(0.1*vr);
369#endif
370 }
371
372#ifdef EXPERT
373 fCosy->fMac2->SetAcceleration(0.4*vr);// 0.4
374 fCosy->fMac2->SetDeceleration(0.4*vr);// 0.4
375 fCosy->SetPosVelocity(fabs(rd.Ratio()), 0.2); // fast: 0.6, slow: 0.2
376#else
377 fCosy->fMac2->SetAcceleration(0.2*vr);
378 fCosy->fMac2->SetDeceleration(0.1*vr);
379 fCosy->SetPosVelocity(fabs(rd.Ratio()), 0.1);
380#endif
381 }
382 */
383 if (i)
384 {
385 //lout << "--- LO-SPEED ---" << endl;
386 SetAccDec(fCosy->fMac1, 0.1, 0.1);
387 SetAccDec(fCosy->fMac2, 0.1, 0.1);
388
389 SetPosVelocity(1.0, 0.05);
390 }
391 else
392 {
393 const Double_t y = 15*fCosy->kGearRatio.Y();
394 if (rd.Az()>-y && rd.Az()<y)
395 {
396 //lout << "--- LO-SPEED Mac1 ---" << endl;
397 SetAccDec(fCosy->fMac1, 0.05, 0.05);
398 }
399 else
400 SetAccDec(fCosy->fMac1, fAcc, fDec);
401
402 SetAccDec(fCosy->fMac2, fAcc, fDec);
403
404 SetPosVelocity(fabs(rd.Ratio()), fVel);
405 }
406
407 rd.Round();
408
409 // FIXME? Check for Error or Zombie?
410
411
412 // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
413 // cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
414 // cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
415 // cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
416 // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
417
418
419 //
420 // repositioning (relative)
421 //
422 lout << "- Do Relative Positioning..." << endl;
423 DoRelPos(rd, cdzd, cdaz);
424 lout << "- Relative Positioning Done" << endl;
425 }
426 if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/)
427 {
428 t.Now();
429 lout << t << " - Positioning done." << endl;
430 fCosy->SetStatus(MDriveCom::kStopped);
431 fCosy->fCom->Send("POSITION DONE");
432 return TRUE;
433 }
434
435 if (i<10)
436 fCosy->StopMovement();
437 else
438 fCosy->SetStatus(MDriveCom::kStopped);
439
440 t.Now();
441 lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl;
442
443 fCosy->fCom->Send("POSITION FAILED");
444
445 return FALSE;
446}
Note: See TracBrowser for help on using the repository browser.