1 | #include "MCosy.h"
|
---|
2 | #include "MCosy.h"
|
---|
3 |
|
---|
4 | #include <iomanip.h>
|
---|
5 | #include <fstream.h>
|
---|
6 | #include <iostream.h>
|
---|
7 |
|
---|
8 | #include <TROOT.h>
|
---|
9 | #include <TEnv.h>
|
---|
10 | #include <TSystem.h>
|
---|
11 | #include <TApplication.h>
|
---|
12 | #include <TTimer.h>
|
---|
13 |
|
---|
14 | #include <TH2.h>
|
---|
15 | #include <TProfile.h>
|
---|
16 | #include <TCanvas.h>
|
---|
17 |
|
---|
18 | #include "MGCosy.h"
|
---|
19 | #include "SlaStars.h"
|
---|
20 |
|
---|
21 | #include "slalib/slalib.h" // FIXME: REMOVE
|
---|
22 |
|
---|
23 | #include "macs.h"
|
---|
24 | #include "base/timer.h"
|
---|
25 | #include "shaftencoder.h"
|
---|
26 |
|
---|
27 | //#include <sys/resource.h> // PRIO_PROCESS
|
---|
28 |
|
---|
29 | ClassImp(MCosy);
|
---|
30 |
|
---|
31 | typedef struct tm tm_t;
|
---|
32 |
|
---|
33 | /*
|
---|
34 | #define GEAR_RATIO_ALT 2475.6 // [U_mot/U_tel(360deg)]
|
---|
35 | #define GEAR_RATIO_AZ 5891.7 // [U_mot/U_tel(360deg)]
|
---|
36 |
|
---|
37 | #define RES_RE 500 // [re/U_mot]
|
---|
38 | #define RES_SE 16384 // [se/U_tel(360deg)]
|
---|
39 | */
|
---|
40 | /*
|
---|
41 | #define GEAR_RATIO_ALT (75.55*16384/1500) // 75.25 VERY IMPORTANT! unit=U_mot/U_tel
|
---|
42 | #define GEAR_RATIO_AZ (179.8*16384/1500) // VERY IMPORTANT! unit=U_mot/U_tel
|
---|
43 | */
|
---|
44 |
|
---|
45 | //const XY kGearRatio (GEAR_RATIO_ALT*RES_RE/RES_SE, GEAR_RATIO_AZ*RES_RE/RES_SE); //[re/se]
|
---|
46 | //const XY kGearRatio2(GEAR_RATIO_ALT*RES_RE/360.0, GEAR_RATIO_AZ*RES_RE/360.0); //[re/deg]
|
---|
47 |
|
---|
48 | /* +===================================+
|
---|
49 | FIXME: What if fMac3 (Sync) died?
|
---|
50 | +===================================+
|
---|
51 | */
|
---|
52 |
|
---|
53 | double MCosy::Rad2SE(double rad) const
|
---|
54 | {
|
---|
55 | return 16384.0/k2Pi*rad;
|
---|
56 | }
|
---|
57 |
|
---|
58 | double MCosy::Rad2ZdRE(double rad) const
|
---|
59 | {
|
---|
60 | return 16384.0/k2Pi*rad*kGearRatio.X();
|
---|
61 | }
|
---|
62 |
|
---|
63 | double MCosy::Rad2AzRE(double rad) const
|
---|
64 | {
|
---|
65 | return 16384.0/k2Pi*rad*kGearRatio.Y();
|
---|
66 | }
|
---|
67 |
|
---|
68 | double MCosy::Deg2ZdRE(double rad) const
|
---|
69 | {
|
---|
70 | return rad*kGearRatio2.X();
|
---|
71 | }
|
---|
72 |
|
---|
73 | double MCosy::Deg2AzRE(double rad) const
|
---|
74 | {
|
---|
75 | return rad*kGearRatio2.Y();
|
---|
76 | }
|
---|
77 |
|
---|
78 | ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
|
---|
79 | {
|
---|
80 | // CorrectTarget [se]
|
---|
81 |
|
---|
82 | // src [se]
|
---|
83 | // dst [rad]
|
---|
84 |
|
---|
85 | // fAltMax = 70
|
---|
86 | // fAltMin = -105/110
|
---|
87 | // fAzMin = -355
|
---|
88 | // fAzMax = 355
|
---|
89 |
|
---|
90 | ZdAz source = src * 360.0/16384.0;
|
---|
91 | ZdAz dest = dst * kRad2Deg;
|
---|
92 |
|
---|
93 | if (dest.Zd()>-3 && dest.Zd()<3)
|
---|
94 | dest.Zd(dest.Zd()<0?-3:3);
|
---|
95 |
|
---|
96 | if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
|
---|
97 | return dst*(16384.0/k2Pi);
|
---|
98 |
|
---|
99 | const float fZdMin = -67;
|
---|
100 | const float fZdMax = 67;
|
---|
101 | const float fAzMin = -29;
|
---|
102 | const float fAzMax = 423;
|
---|
103 |
|
---|
104 | //
|
---|
105 | // This corrects to target for the shortest distance, not for the fastest move!
|
---|
106 | //
|
---|
107 | ZdAz s = source-dest;
|
---|
108 |
|
---|
109 | float min = s.Sqr();
|
---|
110 |
|
---|
111 | //
|
---|
112 | // Is it enought to search inside one revolution?
|
---|
113 | //
|
---|
114 | ZdAz ret = dest;
|
---|
115 |
|
---|
116 | for (int i=-5; i<5+1; i++)
|
---|
117 | {
|
---|
118 | const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);
|
---|
119 |
|
---|
120 | //
|
---|
121 | // Range Check
|
---|
122 | //
|
---|
123 | if (p.Zd()<fZdMin || p.Zd()>fZdMax)
|
---|
124 | continue;
|
---|
125 |
|
---|
126 | if (p.Az()<fAzMin || p.Az()>fAzMax)
|
---|
127 | continue;
|
---|
128 |
|
---|
129 | //
|
---|
130 | // Calculate distance
|
---|
131 | //
|
---|
132 | s = source-p;
|
---|
133 |
|
---|
134 | const float dist = s.Sqr();
|
---|
135 |
|
---|
136 | if (dist > min)
|
---|
137 | continue;
|
---|
138 |
|
---|
139 | //
|
---|
140 | // New shortest distance
|
---|
141 | //
|
---|
142 | ret = p;
|
---|
143 | min = dist;
|
---|
144 | }
|
---|
145 | return ret*(16384.0/360.0);
|
---|
146 | }
|
---|
147 |
|
---|
148 | // --------------------------------------------------------------------------
|
---|
149 | //
|
---|
150 | // GetSePos, reads the Shaftencoder positions from the Can-drivers
|
---|
151 | // for the shaftencoders. The two shaft encoders at the elevation axis
|
---|
152 | // are avaraged. The values are returned as a ZdAz object.
|
---|
153 | //
|
---|
154 | // If one of the two shaftencoders on the elevation axis is missing
|
---|
155 | // the other one's position is returned.
|
---|
156 | //
|
---|
157 | // The positions are alway up-to-date because the shaftencoders are
|
---|
158 | // sending all changes immediatly.
|
---|
159 | //
|
---|
160 | ZdAz MCosy::GetSePos()
|
---|
161 | {
|
---|
162 | const int pa = fAz->GetPos();
|
---|
163 | if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
|
---|
164 | return ZdAz(0, pa);
|
---|
165 |
|
---|
166 | //
|
---|
167 | // Get the values
|
---|
168 | //
|
---|
169 | int p1 = (fZd1->GetPos()+8192)%16384;
|
---|
170 | int p2 = -(fZd2->GetPos()+8192)%16384;
|
---|
171 |
|
---|
172 | if (fZd1->IsZombieNode())
|
---|
173 | return ZdAz(p2, pa);
|
---|
174 | if (fZd2->IsZombieNode())
|
---|
175 | return ZdAz(p1, pa);
|
---|
176 |
|
---|
177 | //
|
---|
178 | // interpolate shaft encoder positions
|
---|
179 | //
|
---|
180 | float p = (float)(p1+p2)/2;
|
---|
181 |
|
---|
182 | return ZdAz(p, pa);
|
---|
183 | }
|
---|
184 |
|
---|
185 | // --------------------------------------------------------------------------
|
---|
186 | //
|
---|
187 | // request the current positions from the rotary encoders.
|
---|
188 | // use GetRePos to get the psotions. If the request fails the function
|
---|
189 | // returns kFALSE, otherwise kTRUE
|
---|
190 | //
|
---|
191 | Bool_t MCosy::RequestRePos()
|
---|
192 | {
|
---|
193 | //
|
---|
194 | // Send request
|
---|
195 | //
|
---|
196 | fMac2->RequestSDO(0x6004);
|
---|
197 | fMac1->RequestSDO(0x6004);
|
---|
198 |
|
---|
199 | //
|
---|
200 | // Wait until the objects are received.
|
---|
201 | //
|
---|
202 | fMac2->WaitForSdo(0x6004);
|
---|
203 | fMac1->WaitForSdo(0x6004);
|
---|
204 |
|
---|
205 | // FIXME, what when waiting times out (Zombie)
|
---|
206 | // WaitForSdos();
|
---|
207 |
|
---|
208 | //
|
---|
209 | // If waiting was not interrupted everything is ok. return.
|
---|
210 | //
|
---|
211 | if (!(Break() || HasError()) && !HasZombie())
|
---|
212 | return kTRUE;
|
---|
213 |
|
---|
214 | //
|
---|
215 | // If the waiting was interrupted due to a network error,
|
---|
216 | // print some logging message.
|
---|
217 | //
|
---|
218 | if (HasError())
|
---|
219 | lout << "Error while requesting re pos from Macs (SDO #6004)" << endl;
|
---|
220 |
|
---|
221 | return kFALSE;
|
---|
222 | }
|
---|
223 |
|
---|
224 | // --------------------------------------------------------------------------
|
---|
225 | //
|
---|
226 | // reads the Rotary encoder positions from the last request of the Macs.
|
---|
227 | //
|
---|
228 | // The positions are returned as a ZdAz object. Use RequestRePos to request
|
---|
229 | // the current positions first.
|
---|
230 | //
|
---|
231 | ZdAz MCosy::GetRePos()
|
---|
232 | {
|
---|
233 | return ZdAz(fMac2->GetPos(), fMac1->GetPos());
|
---|
234 | }
|
---|
235 |
|
---|
236 | // --------------------------------------------------------------------------
|
---|
237 | //
|
---|
238 | // reads the Rotary encoder positions from the Macs.
|
---|
239 | //
|
---|
240 | // The positions are returned as a ZdAz object. The positions are the ones
|
---|
241 | // which are send as PDOs to the computer. This is done at a given
|
---|
242 | // frequency. Which means, that this positions are not ought to be
|
---|
243 | // up-to-date.
|
---|
244 | //
|
---|
245 | ZdAz MCosy::GetRePosPdo()
|
---|
246 | {
|
---|
247 | return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
|
---|
248 | }
|
---|
249 |
|
---|
250 | // --------------------------------------------------------------------------
|
---|
251 | //
|
---|
252 | // set the velocity and accelerations for position maneuvers.
|
---|
253 | //
|
---|
254 | // The acceleratin is set as given (in percent of maximum).
|
---|
255 | // The velocity is given in percent, depending on the ratio (<1 or >1)
|
---|
256 | // one of the axis becomes a slower velocity. This is used for maneuvers
|
---|
257 | // in which both axis are moved synchromously and should reach their
|
---|
258 | // target position at the same time.
|
---|
259 | //
|
---|
260 | void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc)
|
---|
261 | {
|
---|
262 | //
|
---|
263 | // Set velocities
|
---|
264 | //
|
---|
265 | const int vr = fMac1->GetVelRes();
|
---|
266 |
|
---|
267 | vel *= vr;
|
---|
268 | acc *= vr;
|
---|
269 |
|
---|
270 | if (ratio <1)
|
---|
271 | {
|
---|
272 | fMac1->SetVelocity(vel);
|
---|
273 | fMac2->SetVelocity(vel*ratio);
|
---|
274 |
|
---|
275 | fMac1->SetAcceleration(acc);
|
---|
276 | fMac2->SetAcceleration(acc*ratio);
|
---|
277 |
|
---|
278 | fMac1->SetDeceleration(acc);
|
---|
279 | fMac2->SetDeceleration(acc*ratio);
|
---|
280 | }
|
---|
281 | else
|
---|
282 | {
|
---|
283 | fMac1->SetVelocity(vel/ratio);
|
---|
284 | fMac2->SetVelocity(vel);
|
---|
285 |
|
---|
286 | fMac1->SetAcceleration(acc/ratio);
|
---|
287 | fMac2->SetAcceleration(acc);
|
---|
288 |
|
---|
289 | fMac1->SetDeceleration(acc/ratio);
|
---|
290 | fMac2->SetDeceleration(acc);
|
---|
291 | }
|
---|
292 | }
|
---|
293 |
|
---|
294 | // --------------------------------------------------------------------------
|
---|
295 | //
|
---|
296 | // Does a relative positioning.
|
---|
297 | //
|
---|
298 | // The steps to move are given in a ZdAz object relative to the current
|
---|
299 | // position. The coordinates are given in Roteryencoder steps.
|
---|
300 | // Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only
|
---|
301 | // if Axis 2==kTRUE. The function waits for the movement to be finished.
|
---|
302 | //
|
---|
303 | void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
|
---|
304 | {
|
---|
305 | if (HasZombie())
|
---|
306 | return;
|
---|
307 | SetStatus(MCosy::kMoving);
|
---|
308 |
|
---|
309 | if (axe1) fMac2->StartRelPos(rd.Zd());
|
---|
310 | if (axe2) fMac1->StartRelPos(rd.Az());
|
---|
311 |
|
---|
312 | cout << "Waiting for positioning..." << flush;
|
---|
313 |
|
---|
314 | if (axe1) fMac2->WaitForSdo(0x6004, 1);
|
---|
315 | if (axe2) fMac1->WaitForSdo(0x6004, 1);
|
---|
316 |
|
---|
317 | WaitForEndMovement();
|
---|
318 |
|
---|
319 | cout << "done." << endl;
|
---|
320 | }
|
---|
321 |
|
---|
322 | // --------------------------------------------------------------------------
|
---|
323 | //
|
---|
324 | // check for a break-signal (from the msgqueue) and errors.
|
---|
325 | //
|
---|
326 | int MCosy::StopWaitingForSDO() const
|
---|
327 | {
|
---|
328 | return 0/*Break() || HasError()*/;
|
---|
329 | }
|
---|
330 |
|
---|
331 | // --------------------------------------------------------------------------
|
---|
332 | //
|
---|
333 | // Waits for a movement to become finished.
|
---|
334 | //
|
---|
335 | // First waits for all peding Sdos, then waits until both motors are stopped
|
---|
336 | // or waiting for SDOs was stopped (either by an error or by Break)
|
---|
337 | //
|
---|
338 | void MCosy::WaitForEndMovement()
|
---|
339 | {
|
---|
340 | // FIXME, what when waiting times out (Zombie)
|
---|
341 | while ((fMac1->IsPositioning() || fMac2->IsPositioning()) &&
|
---|
342 | !(Break() || HasError()) && !HasZombie())
|
---|
343 | usleep(1);
|
---|
344 | }
|
---|
345 |
|
---|
346 | // --------------------------------------------------------------------------
|
---|
347 | //
|
---|
348 | // Check for an error...
|
---|
349 | //
|
---|
350 | // This is ment for usage after the Action: All Motors Stop.
|
---|
351 | //
|
---|
352 | void MCosy::CheckForError()
|
---|
353 | {
|
---|
354 | //
|
---|
355 | // Check all Can-Nodes for an Error. If there is no error the motor
|
---|
356 | // status is set to stopped.
|
---|
357 | //
|
---|
358 | if (!HasError())
|
---|
359 | {
|
---|
360 | SetStatus(MCosy::kStopped);
|
---|
361 | return;
|
---|
362 | }
|
---|
363 |
|
---|
364 | //
|
---|
365 | // If there is an error, the error status is set to Error.
|
---|
366 | //
|
---|
367 | SetStatus(MCosy::kError);
|
---|
368 |
|
---|
369 | /*
|
---|
370 | FIXME: HANDLINGE ERROR
|
---|
371 |
|
---|
372 | //
|
---|
373 | // Now try to handle the error.
|
---|
374 | //
|
---|
375 | fMac1->HandleError();
|
---|
376 | fMac2->HandleError();
|
---|
377 |
|
---|
378 | //
|
---|
379 | // If the error couldn't get solved return
|
---|
380 | //
|
---|
381 | if (HasError())
|
---|
382 | return;
|
---|
383 |
|
---|
384 | //
|
---|
385 | // Set motor status to stopped
|
---|
386 | //
|
---|
387 | SetStatus(MCosy::kStopped);
|
---|
388 | */
|
---|
389 | }
|
---|
390 |
|
---|
391 | // --------------------------------------------------------------------------
|
---|
392 | //
|
---|
393 | // Move the telescope to the given position. The position must be given in
|
---|
394 | // a ZdAz object in rad.
|
---|
395 | //
|
---|
396 | // The first positioning is done absolutely. If we didn't reach the
|
---|
397 | // correct psotion we try to correct for this by 10 relative position
|
---|
398 | // maneuvers. If this doesn't help positioning failed.
|
---|
399 | //
|
---|
400 | // As a reference the shaftencoder values are used.
|
---|
401 | //
|
---|
402 | int MCosy::SetPosition(const ZdAz &dst) // [rad]
|
---|
403 | {
|
---|
404 | // FIXME: Correct by fOffset ?
|
---|
405 |
|
---|
406 | const ZdAz d = dst*kRad2Deg;
|
---|
407 |
|
---|
408 | lout << "Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
|
---|
409 |
|
---|
410 | if (d.Zd()<fMin.Zd() || d.Zd()>fMax.Zd())
|
---|
411 | {
|
---|
412 | lout << "ERROR: Requested Zenith Angle (" << d.Zd() << "deg) not ";
|
---|
413 | lout << "inside allowed range." << endl;
|
---|
414 | return kFALSE;
|
---|
415 | }
|
---|
416 |
|
---|
417 | if (d.Az()<fMin.Az() || d.Az()>fMax.Az())
|
---|
418 | {
|
---|
419 | lout << "ERROR: Requested Azimuth Angle (" << d.Az() << "deg) not ";
|
---|
420 | lout << "inside allowed range." << endl;
|
---|
421 | return kFALSE;
|
---|
422 | }
|
---|
423 |
|
---|
424 |
|
---|
425 | /*
|
---|
426 | if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
|
---|
427 | {
|
---|
428 | cout << "SetPosition: No connection to at least one of the MACS!" << endl;
|
---|
429 | return TRUE;
|
---|
430 | }
|
---|
431 | */
|
---|
432 |
|
---|
433 | //
|
---|
434 | // Make sure that the motors are in sync mode (necessary if the
|
---|
435 | // MACS has been rebooted from a Zombie state.
|
---|
436 | //
|
---|
437 | //InitSync();
|
---|
438 | //if (fMac3->IsZombieNode())
|
---|
439 | // return false;
|
---|
440 |
|
---|
441 | //
|
---|
442 | // Calculate new target position (shortest distance to go)
|
---|
443 | //
|
---|
444 | const ZdAz src = GetSePos(); // [se]
|
---|
445 |
|
---|
446 | //
|
---|
447 | // Because we agreed on I don't search for the shortest move
|
---|
448 | // anymore
|
---|
449 | //
|
---|
450 | // const ZdAz dest = CorrectTarget(src, dst);
|
---|
451 | //
|
---|
452 | const ZdAz dest = fBending(dst)*16384/2/TMath::Pi(); // [se]
|
---|
453 | fZdAzSoll = dst;
|
---|
454 |
|
---|
455 | cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl;
|
---|
456 | cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl;
|
---|
457 | cout << "Bend'd Dest Zd: " << Rad2SE(fZdAzSoll.Zd()) << "se Az:" << Rad2SE(fZdAzSoll.Az()) << "se" << endl;
|
---|
458 | cout << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl;
|
---|
459 |
|
---|
460 | int i;
|
---|
461 | for (i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++)
|
---|
462 | {
|
---|
463 |
|
---|
464 | lout << "Step #" << i << endl;
|
---|
465 | //
|
---|
466 | // Get Shaft Encoder Positions
|
---|
467 | //
|
---|
468 | const ZdAz p=GetSePos();
|
---|
469 |
|
---|
470 | //
|
---|
471 | // calculate control deviation and rounded cd
|
---|
472 | //
|
---|
473 | ZdAz rd = dest-p; // [se]
|
---|
474 |
|
---|
475 | ZdAz cd = rd; // [se]
|
---|
476 | cd.Round();
|
---|
477 |
|
---|
478 | //
|
---|
479 | // Check if there is a control deviation on the axis
|
---|
480 | //
|
---|
481 | const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
|
---|
482 | const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
|
---|
483 |
|
---|
484 | //
|
---|
485 | // check if we reached the correct position already
|
---|
486 | //
|
---|
487 | if (!cdzd && !cdaz)
|
---|
488 | {
|
---|
489 | lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl;
|
---|
490 | SetStatus(MCosy::kStopped);
|
---|
491 | return TRUE;
|
---|
492 | }
|
---|
493 |
|
---|
494 | //
|
---|
495 | // change units from se to re
|
---|
496 | //
|
---|
497 | rd *= kGearRatio; // [re]
|
---|
498 |
|
---|
499 | //
|
---|
500 | // Initialize Velocities so that we reach both positions
|
---|
501 | // at the same time
|
---|
502 | //
|
---|
503 | lout << "SetVelocity" << endl;
|
---|
504 | if (i)
|
---|
505 | SetPosVelocity(1.0, 0.05, 0.1);
|
---|
506 | else // vel(0.6) acc(0.5)
|
---|
507 | SetPosVelocity(fabs(rd.Ratio()), 0.15, 0.4);
|
---|
508 | // SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3);
|
---|
509 |
|
---|
510 | rd.Round();
|
---|
511 |
|
---|
512 | // FIXME? Check for Error or Zombie?
|
---|
513 |
|
---|
514 | /*
|
---|
515 | cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
|
---|
516 | cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
|
---|
517 | cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
|
---|
518 | cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
|
---|
519 | cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
|
---|
520 | */
|
---|
521 |
|
---|
522 | //
|
---|
523 | // repositioning (relative)
|
---|
524 | //
|
---|
525 | lout << "Do Relative Positioning Done" << endl;
|
---|
526 | DoRelPos(rd, cdzd, cdaz);
|
---|
527 |
|
---|
528 | lout << "Relative Positioning Done" << endl;
|
---|
529 | }
|
---|
530 |
|
---|
531 | StopMovement();
|
---|
532 | lout << "Warning: Requested position not reached (i=" << i << ")" << endl;
|
---|
533 | return FALSE;
|
---|
534 | }
|
---|
535 |
|
---|
536 | // --------------------------------------------------------------------------
|
---|
537 | //
|
---|
538 | // Sets the tracking velocity
|
---|
539 | //
|
---|
540 | // The velocities are given in a ZdAz object in re/min. Return kTRUE
|
---|
541 | // in case of success, kFALSE in case of failure.
|
---|
542 | //
|
---|
543 | Bool_t MCosy::SetVelocity(const ZdAz &v)
|
---|
544 | {
|
---|
545 | //
|
---|
546 | // Send the new velocities for both axes.
|
---|
547 | //
|
---|
548 | fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min]
|
---|
549 | fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min]
|
---|
550 |
|
---|
551 | //
|
---|
552 | // Wait for the objects to be OKed.
|
---|
553 | //
|
---|
554 |
|
---|
555 | fMac2->WaitForSdo(0x3006, 1);
|
---|
556 | fMac1->WaitForSdo(0x3006, 1);
|
---|
557 |
|
---|
558 | // FIXME, what when waiting times out (Zombie)
|
---|
559 | // WaitForSdos();
|
---|
560 |
|
---|
561 | //
|
---|
562 | // If the waiting for the objects wasn't interrupted return kTRUE
|
---|
563 | //
|
---|
564 | if (!(Break() || HasError()) && !HasZombie())
|
---|
565 | return kTRUE;
|
---|
566 |
|
---|
567 | //
|
---|
568 | // print a message if the interruption was due to a Can-node Error
|
---|
569 | //
|
---|
570 | if (HasError())
|
---|
571 | lout << "Error while setting velocity (SDO #3006)" << endl;
|
---|
572 |
|
---|
573 | return kFALSE;
|
---|
574 | }
|
---|
575 |
|
---|
576 | // --------------------------------------------------------------------------
|
---|
577 | //
|
---|
578 | // Initializes Tracking mode
|
---|
579 | //
|
---|
580 | // Initializes the accelerations of both axes with 90% of the maximum
|
---|
581 | // acceleration. Set the status for moving and tracking and starts thr
|
---|
582 | // revolution mode.
|
---|
583 | //
|
---|
584 | bool MCosy::InitTracking()
|
---|
585 | {
|
---|
586 | // FIXME? Handling of Zombie OK?
|
---|
587 | if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
|
---|
588 | return false;
|
---|
589 |
|
---|
590 | //
|
---|
591 | // Start revolution mode
|
---|
592 | //
|
---|
593 | fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
|
---|
594 | fMac2->SetDeceleration(0.90*fMac2->GetVelRes());
|
---|
595 | if (fMac2->IsZombieNode())
|
---|
596 | return false;
|
---|
597 |
|
---|
598 | fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
|
---|
599 | fMac1->SetDeceleration(0.90*fMac1->GetVelRes());
|
---|
600 | if (fMac1->IsZombieNode())
|
---|
601 | return false;
|
---|
602 |
|
---|
603 | SetStatus(MCosy::kMoving | MCosy::kTracking);
|
---|
604 |
|
---|
605 | fMac2->SetRpmMode(TRUE);
|
---|
606 | if (fMac2->IsZombieNode())
|
---|
607 | return false;
|
---|
608 |
|
---|
609 | fMac1->SetRpmMode(TRUE);
|
---|
610 | if (fMac1->IsZombieNode())
|
---|
611 | return false;
|
---|
612 |
|
---|
613 | return true;
|
---|
614 | }
|
---|
615 |
|
---|
616 | // --------------------------------------------------------------------------
|
---|
617 | //
|
---|
618 | // Limits the speed.
|
---|
619 | //
|
---|
620 | // This function should work as a limiter. If a tracking error is too large
|
---|
621 | // to be corrected fast enough we would get enormous velocities. These
|
---|
622 | // velocities are limited to the maximum velocity.
|
---|
623 | //
|
---|
624 | void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
|
---|
625 | {
|
---|
626 | //
|
---|
627 | // How to limit the speed. If the wind comes and blowes
|
---|
628 | // we cannot forbid changing of the sign. But on the other hand
|
---|
629 | // we don't want fast changes!
|
---|
630 | //
|
---|
631 |
|
---|
632 | ULong_t vrzd = fMac1->GetVelRes();
|
---|
633 | ULong_t vraz = fMac2->GetVelRes();
|
---|
634 |
|
---|
635 | #define sgn(x) (x<0?-1:1)
|
---|
636 |
|
---|
637 | const Float_t limit = 0.25;
|
---|
638 | /*
|
---|
639 | if (sgn(vt->Az()) != sgn(vcalc.Az()))
|
---|
640 | vt->Az(0);
|
---|
641 | // else
|
---|
642 | {
|
---|
643 | if (fabs(vt->Az()) < fabs(vcalc.Az()) *0.5)
|
---|
644 | vt->Az(0.5*vcalc.Az());
|
---|
645 |
|
---|
646 | if (fabs(vt->Az()) > fabs(vcalc.Az()) *1.5)
|
---|
647 | vt->Az(1.5*vcalc.Az());
|
---|
648 | }
|
---|
649 |
|
---|
650 | if (sgn(vt->Zd()) != sgn(vcalc.Zd()))
|
---|
651 | vt->Zd(0);
|
---|
652 | // else
|
---|
653 | {
|
---|
654 | if (fabs(vt->Zd()) > fabs(vcalc.Az()) *1.5)
|
---|
655 | vt->Zd(1.5*vcalc.Zd());
|
---|
656 |
|
---|
657 | if (fabs(vt->Zd()) < fabs(vcalc.Az()) *0.5)
|
---|
658 | vt->Zd(0.5*vcalc.Zd());
|
---|
659 | }
|
---|
660 | */
|
---|
661 |
|
---|
662 | if (sgn(vt->Az()) != sgn(vcalc.Az())
|
---|
663 | && fabs(vt->Az()) < limit*fabs(vcalc.Az())
|
---|
664 | )
|
---|
665 | vt->Az(0);
|
---|
666 | else
|
---|
667 | if (fabs(vt->Az()) > 0.9*vraz)
|
---|
668 | {
|
---|
669 | lout << "Warning: Azimuth speed limit exceeded. Limiting speed to 90% max." << endl;
|
---|
670 | vt->Az(0.9*vraz*sgn(vt->Az()));
|
---|
671 | }
|
---|
672 |
|
---|
673 | if (sgn(vt->Zd()) != sgn(vcalc.Zd())
|
---|
674 | && fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
|
---|
675 | )
|
---|
676 | vt->Zd(0);
|
---|
677 | else
|
---|
678 | if (fabs(vt->Zd()) > 0.9*vrzd)
|
---|
679 | {
|
---|
680 | lout << "Warning: Altitude speed limit exceeded. Limiting speed to 90% max." << endl;
|
---|
681 | vt->Zd(0.9*vrzd*sgn(vt->Zd()));
|
---|
682 | }
|
---|
683 | }
|
---|
684 |
|
---|
685 | void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
|
---|
686 | {
|
---|
687 | lout << "Track Position: " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg << "deg (Ra/Dec)" << endl;
|
---|
688 |
|
---|
689 | SlaStars sla(fObservatory);
|
---|
690 |
|
---|
691 | //
|
---|
692 | // Position to actual position
|
---|
693 | //
|
---|
694 | sla.Now();
|
---|
695 | ZdAz dest = sla.CalcZdAz(dst);
|
---|
696 |
|
---|
697 | if (!SetPosition(dest))
|
---|
698 | {
|
---|
699 | lout << "Error: Cannot start tracking, positioning failed." << endl;
|
---|
700 | return;
|
---|
701 | }
|
---|
702 |
|
---|
703 | //
|
---|
704 | // calculate offset from present se position
|
---|
705 | //
|
---|
706 | const ZdAz sepos = GetSePos()*kGearRatio;
|
---|
707 |
|
---|
708 | if (!RequestRePos())
|
---|
709 | return;
|
---|
710 |
|
---|
711 | //
|
---|
712 | // Estimate Offset before starting to track
|
---|
713 | //
|
---|
714 | fOffset = sepos-GetRePos();
|
---|
715 |
|
---|
716 | /*
|
---|
717 | cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
|
---|
718 | cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
|
---|
719 | cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
|
---|
720 | */
|
---|
721 |
|
---|
722 | //
|
---|
723 | // Init accelerations and Rpm Mode
|
---|
724 | //
|
---|
725 | if (!InitTracking())
|
---|
726 | return;
|
---|
727 |
|
---|
728 | XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
|
---|
729 |
|
---|
730 | lout << "Start tracking:";
|
---|
731 | lout << " Ra: " << xy.X() << "h " << "Dec: " << xy.Y() << "\xb0" << endl;
|
---|
732 |
|
---|
733 | ofstream fout("coordinates.txt");
|
---|
734 | fout << xy;
|
---|
735 | fout.close();
|
---|
736 |
|
---|
737 | //
|
---|
738 | // Initialize Tracker (slalib or starguider)
|
---|
739 | //
|
---|
740 | fRaDec = dst;
|
---|
741 | fBackground = kBgdTracking;
|
---|
742 |
|
---|
743 | //--- ofstream fout("log/cosy.pos");
|
---|
744 | //--- fout << "Tracking:";
|
---|
745 | //--- fout << " Ra: " << Rad2Deg(dst.Ra()) << "\x9c ";
|
---|
746 | //--- fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
|
---|
747 | //--- fout << " Mjd/10ms V/re/min/4" << endl;
|
---|
748 |
|
---|
749 | //
|
---|
750 | // We want to reach the theoretical position exactly in about 0.5s
|
---|
751 | //
|
---|
752 | // *OLD*const float dt = 1; // 1 second
|
---|
753 | const float dt = 3; // 1 second
|
---|
754 | while (!(Break() || HasError()) && !HasZombie())
|
---|
755 | {
|
---|
756 | //
|
---|
757 | // Request Target position for this moment
|
---|
758 | //
|
---|
759 | sla.Now();
|
---|
760 |
|
---|
761 | //
|
---|
762 | // Request theoretical Position for a time in the future (To+dt) from CPU
|
---|
763 | //
|
---|
764 | sla.SetMjd(sla.GetMjd()+dt/(60*60*24));
|
---|
765 | ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
|
---|
766 | dest = CorrectTarget(GetSePos(), dummy); // [se]
|
---|
767 |
|
---|
768 | const ZdAz d = dest*360./16384; // [deg]
|
---|
769 | dest *= kGearRatio; // [re]
|
---|
770 |
|
---|
771 | ZdAz min = fBending(fMin/kRad2Deg)*kRad2Deg;
|
---|
772 | ZdAz max = fBending(fMax/kRad2Deg)*kRad2Deg;
|
---|
773 |
|
---|
774 | if (d.Zd()<min.Zd() || d.Az()<min.Az())
|
---|
775 | {
|
---|
776 | lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
|
---|
777 | lout << "< " << d.Zd() << " " << min.Zd() << " " << d.Az() << " " << min.Az() << endl;
|
---|
778 | break;
|
---|
779 | }
|
---|
780 | if (d.Zd()>max.Zd() || d.Az()>max.Az())
|
---|
781 | {
|
---|
782 | lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
|
---|
783 | lout << "> " << d.Zd() << " " << max.Zd() << " " << d.Az() << " " << max.Az() << endl;
|
---|
784 | break;
|
---|
785 | }
|
---|
786 |
|
---|
787 | ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min]
|
---|
788 |
|
---|
789 | //
|
---|
790 | // Request absolute position of rotary encoder from Macs
|
---|
791 | //
|
---|
792 | if (!RequestRePos())
|
---|
793 | break;
|
---|
794 |
|
---|
795 | //
|
---|
796 | // distance between (To+dt) and To [re]
|
---|
797 | // position time difference < 5usec
|
---|
798 | // fOffset does the synchronization between the
|
---|
799 | // Shaft- and the rotary encoders
|
---|
800 | //
|
---|
801 | dest -= GetRePos() + fOffset;
|
---|
802 |
|
---|
803 | //
|
---|
804 | // Velocity to go [re/min] to reach the right position at time t+dt
|
---|
805 | // correct for the duration of RaDec2AltAz
|
---|
806 | //
|
---|
807 | const ZdAz v = dest*60.0/(dt/*-(fMac2->GetTime()-sla)*/);
|
---|
808 |
|
---|
809 | //
|
---|
810 | // calculate real velocity of future [re/min]
|
---|
811 | // believing the Macs manual '/4' shouldn't be necessary, but it is.
|
---|
812 | //
|
---|
813 | ZdAz vt = v/4;
|
---|
814 | LimitSpeed(&vt, vcalc);
|
---|
815 | vt.Round();
|
---|
816 |
|
---|
817 | //
|
---|
818 | // check if the drive is fast enough to follow the star
|
---|
819 | //
|
---|
820 | if (vt.Zd()>.9*fMac1->GetVelRes() || vt.Az()>.9*fMac2->GetVelRes())
|
---|
821 | {
|
---|
822 | lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
|
---|
823 | break;
|
---|
824 | }
|
---|
825 |
|
---|
826 | //
|
---|
827 | // Set theoretical velocity (as early after calculation as possible)
|
---|
828 | // Maybe we should attenuate the changes
|
---|
829 | //
|
---|
830 | if (!SetVelocity(vt))
|
---|
831 | break;
|
---|
832 |
|
---|
833 | //
|
---|
834 | // Now do 'unnecessary' things
|
---|
835 | //
|
---|
836 | fVelocity = vt/kGearRatio2*4;
|
---|
837 |
|
---|
838 | //--- const double mjd = fMac2->GetMjd();
|
---|
839 | //--- fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " ";
|
---|
840 | //--- fout << setw(4) << vt.Zd() << " ";
|
---|
841 | //--- fout << setw(4) << vt.Az() << endl;
|
---|
842 | //
|
---|
843 | // FIXME? Calculate an accuracy for the tracking system?
|
---|
844 | // How good do we reach the calculated position in 'real'
|
---|
845 | // re valus?
|
---|
846 | //
|
---|
847 |
|
---|
848 |
|
---|
849 | //
|
---|
850 | // Update speed as often as possible.
|
---|
851 | // make sure, that dt is around 10 times larger than the
|
---|
852 | // update time
|
---|
853 | //
|
---|
854 | //
|
---|
855 | // The loop should not be executed faster than the ramp of
|
---|
856 | // a change in the velocity can be followed.
|
---|
857 | // (This is important on fast machines >500MHz)
|
---|
858 | //
|
---|
859 | usleep(50000); // 0.25s
|
---|
860 | //usleep(50000); // 0.05s
|
---|
861 | }
|
---|
862 |
|
---|
863 | fBackground = kBgdNone;
|
---|
864 | StopMovement();
|
---|
865 | lout << "Tracking stopped." << endl;
|
---|
866 | }
|
---|
867 |
|
---|
868 | // --------------------------------------------------------------------------
|
---|
869 | //
|
---|
870 | // Stops the movement of both motors.
|
---|
871 | //
|
---|
872 | // Sets the status to stopping. Sets the deceleration to 50% of the maximum.
|
---|
873 | // stops. Quits the revolution mode and wait for the end of the movement.
|
---|
874 | //
|
---|
875 | void MCosy::StopMovement()
|
---|
876 | {
|
---|
877 | //
|
---|
878 | // Set status to Stopping
|
---|
879 | //
|
---|
880 | SetStatus(MCosy::kStopping);
|
---|
881 |
|
---|
882 | //
|
---|
883 | // set deceleration to 50%
|
---|
884 | //
|
---|
885 | cout << "Stopping movement..." << endl;
|
---|
886 | fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
|
---|
887 | fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
|
---|
888 |
|
---|
889 | fMac1->SetRpmMode(FALSE);
|
---|
890 | fMac2->SetRpmMode(FALSE);
|
---|
891 |
|
---|
892 | //
|
---|
893 | // Wait for the movement to really be finished.
|
---|
894 | //
|
---|
895 | cout << "Waiting for silence..." << endl;
|
---|
896 | WaitForEndMovement();
|
---|
897 |
|
---|
898 | //
|
---|
899 | // Check whether everything works fine.
|
---|
900 | //
|
---|
901 | CheckForError();
|
---|
902 | cout << "Movement stopped." << endl;
|
---|
903 | }
|
---|
904 |
|
---|
905 | bool MCosy::CheckNetwork()
|
---|
906 | {
|
---|
907 | //return kTRUE;
|
---|
908 | //CheckConnections();
|
---|
909 |
|
---|
910 | if (HasZombie())
|
---|
911 | {
|
---|
912 | lout << "- Found Zombies in Network..." << endl;
|
---|
913 | if (!RebootZombies())
|
---|
914 | return false;
|
---|
915 | }
|
---|
916 |
|
---|
917 | /*
|
---|
918 | FIXME HANDLING ERROR
|
---|
919 | */
|
---|
920 | if (HasError())
|
---|
921 | {
|
---|
922 | fMac1->HandleError();
|
---|
923 | fMac2->HandleError();
|
---|
924 | fMac3->HandleError();
|
---|
925 | if (HasError() || HasZombie())
|
---|
926 | return false;
|
---|
927 | }
|
---|
928 |
|
---|
929 | return true;
|
---|
930 | }
|
---|
931 |
|
---|
932 | void *MCosy::Proc(int msg, void *mp)
|
---|
933 | {
|
---|
934 | switch (msg)
|
---|
935 | {
|
---|
936 | case WM_WAIT:
|
---|
937 | cout << "Wait for execution of Proc(WM_*, ): done." << endl;
|
---|
938 | return NULL;
|
---|
939 |
|
---|
940 | case WM_STOP:
|
---|
941 | cout << "MCosy::Proc: Stop." << endl;
|
---|
942 | if (!CheckNetwork())
|
---|
943 | return (void*)0xebb0;
|
---|
944 | StopMovement();
|
---|
945 | return NULL;
|
---|
946 |
|
---|
947 | case WM_PRESET:
|
---|
948 | cout << "WM_Preset: start." << endl;
|
---|
949 | if (!CheckNetwork())
|
---|
950 | return (void*)0xebb0;
|
---|
951 | fZd1->SetPreset();
|
---|
952 | fZd2->SetPreset();
|
---|
953 | fAz->SetPreset();
|
---|
954 | cout << "WM_Preset: done. (return 0xaffe)" << endl;
|
---|
955 | return (void*)0xaffe;
|
---|
956 |
|
---|
957 | case WM_CALIB:
|
---|
958 | {
|
---|
959 | cout << "WM_Calib: start." << endl;
|
---|
960 | if (!CheckNetwork())
|
---|
961 | return (void*)0xebb0;
|
---|
962 |
|
---|
963 | SlaStars sla(fObservatory);
|
---|
964 | sla.Now();
|
---|
965 |
|
---|
966 | RaDec rd = *((RaDec*)mp);
|
---|
967 |
|
---|
968 | //RaDec rd(37.94, 89.2644); // POLARIS
|
---|
969 | //RaDec rd(213.915417, 19.1825); // ACTURUS
|
---|
970 |
|
---|
971 | cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
|
---|
972 |
|
---|
973 | ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;
|
---|
974 |
|
---|
975 | cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
|
---|
976 |
|
---|
977 | ZdAz sepos = GetSePos();
|
---|
978 | cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
|
---|
979 |
|
---|
980 | fZd1->SetPreset(za.Zd());
|
---|
981 | fZd2->SetPreset(-za.Zd());
|
---|
982 | fAz->SetPreset(za.Az());
|
---|
983 |
|
---|
984 | cout << "WM_Calib: done. (return 0xaffe)" << endl;
|
---|
985 | }
|
---|
986 | return (void*)0xaffe;
|
---|
987 |
|
---|
988 | case WM_TPOINT:
|
---|
989 | {
|
---|
990 | cout << "WM_TPoint: start." << endl;
|
---|
991 | SlaStars sla(fObservatory);
|
---|
992 | sla.Now();
|
---|
993 |
|
---|
994 | RaDec rd = *((RaDec*)mp);
|
---|
995 | cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
|
---|
996 |
|
---|
997 | AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;
|
---|
998 |
|
---|
999 | cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
---|
1000 | *tpout << za.Az() << " " << za.Alt() << " ";
|
---|
1001 |
|
---|
1002 | ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;;
|
---|
1003 | za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
|
---|
1004 | za *= kRad2Deg;
|
---|
1005 |
|
---|
1006 | cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
|
---|
1007 | *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << endl;
|
---|
1008 |
|
---|
1009 | cout << "WM_TPoint: done. (return 0xaffe)" << endl;
|
---|
1010 | }
|
---|
1011 | return (void*)0xca1b;
|
---|
1012 |
|
---|
1013 | case WM_POSITION:
|
---|
1014 | cout << "WM_Position: start." << endl;
|
---|
1015 | {
|
---|
1016 | if (!CheckNetwork())
|
---|
1017 | return (void*)0xebb0;
|
---|
1018 |
|
---|
1019 | ZdAz dest = *((ZdAz*)mp);
|
---|
1020 | SetPosition(dest*kDeg2Rad);
|
---|
1021 | }
|
---|
1022 | cout << "WM_Position: done. (return 0x7777)" << endl;
|
---|
1023 | return (void*)0x7777;
|
---|
1024 |
|
---|
1025 | case WM_TESTSE:
|
---|
1026 | cout << "WM_TestSe: start." << endl;
|
---|
1027 | fBackground = mp ? kBgdSeTest : kBgdNone;
|
---|
1028 | cout << "WM_TestSe: done. (return 0x1e51)" << endl;
|
---|
1029 | return (void*)0x1e51;
|
---|
1030 |
|
---|
1031 | case WM_GEAR:
|
---|
1032 | cout << "WM_Gear: start." << endl;
|
---|
1033 | fBackground = mp ? kBgdGear : kBgdNone;
|
---|
1034 | cout << "WM_Gear: done. (return 0xfeaf)" << endl;
|
---|
1035 | return (void*)0xfeaf;
|
---|
1036 |
|
---|
1037 | case WM_DISPLAY:
|
---|
1038 | cout << "WM_Display: start." << endl;
|
---|
1039 | fTriggerDisplay = kTRUE;
|
---|
1040 | cout << "WM_Disply: done. (return 0xd1e1)" << endl;
|
---|
1041 | return (void*)0xd1e1;
|
---|
1042 |
|
---|
1043 | case WM_TRACK:
|
---|
1044 | cout << "WM_Track: START" << endl;
|
---|
1045 | {
|
---|
1046 | if (!CheckNetwork())
|
---|
1047 | return (void*)0xebb0;
|
---|
1048 |
|
---|
1049 | RaDec dest = *((RaDec*)mp);
|
---|
1050 | TrackPosition(dest*kDeg2Rad);
|
---|
1051 | }
|
---|
1052 | cout << "WM_Track: done. (return 0x8888)" << endl;
|
---|
1053 | return (void*)0x8888;
|
---|
1054 |
|
---|
1055 | case WM_NEWTRACK:
|
---|
1056 | cout << "WM_NewTrack: START" << endl;
|
---|
1057 | fRaDec = *((RaDec*)mp);
|
---|
1058 | cout << "WM_NewTrack: done. (return 0x9999)" << endl;
|
---|
1059 | return (void*)0x9999;
|
---|
1060 |
|
---|
1061 | case WM_LOADBENDING:
|
---|
1062 | cout << "WM_LoadBending: START" << endl;
|
---|
1063 | fBending.Load("bending.txt");
|
---|
1064 | cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
|
---|
1065 | return (void*)0xbe0d;
|
---|
1066 |
|
---|
1067 | case WM_RESETBENDING:
|
---|
1068 | cout << "WM_ResetBending: START" << endl;
|
---|
1069 | fBending.Reset();
|
---|
1070 | cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
|
---|
1071 | return (void*)0xbe0e;
|
---|
1072 |
|
---|
1073 | case WM_HOME:
|
---|
1074 | cout << "WM_Home: START" << endl;
|
---|
1075 | if (!CheckNetwork())
|
---|
1076 | return (void*)0xebb0;
|
---|
1077 | else
|
---|
1078 | {
|
---|
1079 | cout << "HOME NOT ALLOWED... for Magic." << endl;
|
---|
1080 | /*
|
---|
1081 | cout << "Going Home..." << endl;
|
---|
1082 | TEnv env(".cosyrc");
|
---|
1083 |
|
---|
1084 | SetStatus(MCosy::kMoving);
|
---|
1085 |
|
---|
1086 | fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
|
---|
1087 | fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
|
---|
1088 |
|
---|
1089 | lout << "SETHOME DONE" << endl;
|
---|
1090 |
|
---|
1091 | SetStatus(HasError() ? MCosy::kError : MCosy::kStopped);
|
---|
1092 |
|
---|
1093 | fAz->SetPreset();
|
---|
1094 | fZd1->SetPreset();
|
---|
1095 | fZd2->SetPreset();
|
---|
1096 |
|
---|
1097 | fMac1->ReqPos();
|
---|
1098 | fMac2->ReqPos();
|
---|
1099 | fMac3->StopMotor();
|
---|
1100 | */
|
---|
1101 | }
|
---|
1102 | cout << "WM_Home: done. (return 0x403e)" << endl;
|
---|
1103 | return (void*)0x403e;
|
---|
1104 |
|
---|
1105 | case WM_CALCALTAZ:
|
---|
1106 | {
|
---|
1107 | cout << endl;
|
---|
1108 |
|
---|
1109 | SlaStars sla(fObservatory);
|
---|
1110 | sla.Now();
|
---|
1111 |
|
---|
1112 | XY xy = *((XY*)mp);
|
---|
1113 | RaDec rd(xy.X()*15., xy.Y()); // [deg]
|
---|
1114 |
|
---|
1115 | ZdAz a0 = sla.CalcZdAz(rd*kDeg2Rad);
|
---|
1116 | ZdAz a1 = fBending(a0);
|
---|
1117 | ZdAz se = CorrectTarget(GetSePos(), a1);
|
---|
1118 | a0 *= kRad2Deg;
|
---|
1119 | a1 *= kRad2Deg;
|
---|
1120 | ZdAz a2 = a1*16384/360;
|
---|
1121 | cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
|
---|
1122 | cout << "Zd/Az source: " << a0.Zd() << "° " << a0.Az() << "°" << endl;
|
---|
1123 | cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
|
---|
1124 | cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
|
---|
1125 | cout << "SE target: " << se.Zd() << " " << se.Az() << endl;
|
---|
1126 | }
|
---|
1127 | return (void*)0xa17a;
|
---|
1128 |
|
---|
1129 | case WM_QUIT:
|
---|
1130 | cout << "WM_Quit: now." << endl;
|
---|
1131 | if (!CheckNetwork())
|
---|
1132 | {
|
---|
1133 | lout << "ERROR: Cannot shutdown CANbus network." << endl;
|
---|
1134 | return (void*)0xebb0;
|
---|
1135 | }
|
---|
1136 | TerminateApp();
|
---|
1137 | cout << "WM_Quit: done." << endl;
|
---|
1138 | return (void*)0xaaaa;
|
---|
1139 | }
|
---|
1140 | cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
|
---|
1141 | return (void*)0xffffffff;
|
---|
1142 | }
|
---|
1143 |
|
---|
1144 | void *MTTalk::Thread()
|
---|
1145 | {
|
---|
1146 | fCosy->TalkThread();
|
---|
1147 | return NULL;
|
---|
1148 | }
|
---|
1149 |
|
---|
1150 | void MCosy::ReadConfig()
|
---|
1151 | {
|
---|
1152 | cout << "Reading configuration file..." << flush;
|
---|
1153 | TEnv env(".cosyrc");
|
---|
1154 | cout << "done." << endl;
|
---|
1155 |
|
---|
1156 | cout << "Reading telescope range..." << flush;
|
---|
1157 | const Double_t amin = env.GetValue("Az_Min[deg]", -74.5);
|
---|
1158 | const Double_t zmin = env.GetValue("Zd_Min[deg]", -90.0);
|
---|
1159 | fMin.Set(zmin, amin);
|
---|
1160 |
|
---|
1161 | cout << " Min: " << zmin << "deg " << amin << "deg" << endl;
|
---|
1162 |
|
---|
1163 | const Double_t amax = env.GetValue("Az_Max[deg]", 318.0);
|
---|
1164 | const Double_t zmax = env.GetValue("Zd_Max[deg]", 100.5);
|
---|
1165 | fMax.Set(zmax, amax);
|
---|
1166 |
|
---|
1167 | cout << " Max: " << zmax << "deg " << amax << "deg" << endl;
|
---|
1168 |
|
---|
1169 | cout << "Reading gear ratios..." << flush;
|
---|
1170 | const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0);
|
---|
1171 | const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0);
|
---|
1172 |
|
---|
1173 | Double_t resreaz = 0;
|
---|
1174 | if (fMac1 && !fMac1->IsZombieNode())
|
---|
1175 | resreaz = fMac1->GetRes();
|
---|
1176 | else
|
---|
1177 | if (fMac3 && !fMac3->IsZombieNode())
|
---|
1178 | resreaz = fMac3->GetRes();
|
---|
1179 | else
|
---|
1180 | resreaz = env.GetValue("Az_ResRE[re/U_mot]", 1500);
|
---|
1181 |
|
---|
1182 | Double_t resrezd = 0;
|
---|
1183 | if (fMac2 && !fMac2->IsZombieNode())
|
---|
1184 | resrezd = fMac2->GetRes();
|
---|
1185 | else
|
---|
1186 | resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500);
|
---|
1187 |
|
---|
1188 | Double_t ressezd = 0;
|
---|
1189 | if (fZd1 && !fZd1->IsZombieNode())
|
---|
1190 | ressezd = fZd1->GetPhysRes();
|
---|
1191 | else
|
---|
1192 | if (fZd2 && !fZd2->IsZombieNode())
|
---|
1193 | ressezd = fZd2->GetPhysRes();
|
---|
1194 | else
|
---|
1195 | ressezd = env.GetValue("Zd_ResSE[se/U_mot]", 16384);
|
---|
1196 |
|
---|
1197 | Double_t resseaz = 0;
|
---|
1198 | if (fAz && !fAz->IsZombieNode())
|
---|
1199 | resseaz = fAz->GetPhysRes();
|
---|
1200 | else
|
---|
1201 | resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384);
|
---|
1202 |
|
---|
1203 | kGearRatio.Set (gzd*resrezd*4/ressezd, gaz*resreaz*4/resseaz); //[re/se]
|
---|
1204 | kGearRatio2.Set(gzd*resrezd*4/360.0, gaz*resreaz*4/360.0); //[re/deg]
|
---|
1205 | cout << "done." << endl;
|
---|
1206 |
|
---|
1207 | cout << " * Setting Gear Ratios:" << endl;
|
---|
1208 | cout << " --------------------" << endl;
|
---|
1209 | cout << " * X: " << gzd << "*" << resrezd << "/" << ressezd << "=" << kGearRatio.X() << endl;
|
---|
1210 | cout << " * Y: " << gaz << "*" << resreaz << "/" << resseaz << "=" << kGearRatio.Y() << endl;
|
---|
1211 | }
|
---|
1212 |
|
---|
1213 | void MCosy::InitSync()
|
---|
1214 | {
|
---|
1215 | if (!fMac3)
|
---|
1216 | {
|
---|
1217 | lout << "Unable to Init Sync! Mac3 not available." << endl;
|
---|
1218 | return;
|
---|
1219 | }
|
---|
1220 |
|
---|
1221 | const int res = fMac3->GetVelRes();
|
---|
1222 |
|
---|
1223 | fMac3->SetVelocity(res);
|
---|
1224 | fMac3->SetAcceleration(res);
|
---|
1225 | fMac3->SetDeceleration(res);
|
---|
1226 | fMac3->StartPosSync();
|
---|
1227 | }
|
---|
1228 |
|
---|
1229 | void MCosy::TalkThreadTracking()
|
---|
1230 | {
|
---|
1231 | if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
|
---|
1232 | return;
|
---|
1233 |
|
---|
1234 | if (fAz->IsZombieNode())
|
---|
1235 | return;
|
---|
1236 |
|
---|
1237 | if (!fMac1 || !fMac2)
|
---|
1238 | return;
|
---|
1239 |
|
---|
1240 | lout << "Tracking Thread started..." << endl;
|
---|
1241 |
|
---|
1242 | SlaStars sla(fObservatory);
|
---|
1243 |
|
---|
1244 | ZdAz old;
|
---|
1245 | ZdAz ist;
|
---|
1246 |
|
---|
1247 | ZdAz sollzd;
|
---|
1248 | ZdAz sollaz;
|
---|
1249 |
|
---|
1250 | ZdAz istre = -fOffset; // [re]
|
---|
1251 | ZdAz time;
|
---|
1252 |
|
---|
1253 | //
|
---|
1254 | // only update fTrackingError while tracking
|
---|
1255 | //
|
---|
1256 | bool phca1=false;
|
---|
1257 | bool phca2=false;
|
---|
1258 | bool phcaz=false;
|
---|
1259 |
|
---|
1260 | while (fBackground==kBgdTracking)
|
---|
1261 | {
|
---|
1262 | //
|
---|
1263 | // Make changes (eg wind) smoother - attenuation of control function
|
---|
1264 | //
|
---|
1265 | const float weight = 1.; //0.3;
|
---|
1266 |
|
---|
1267 | //
|
---|
1268 | // This is the time constant which defines how fast
|
---|
1269 | // you correct for external influences (like wind)
|
---|
1270 | //
|
---|
1271 | fZd1->ResetPosHasChanged();
|
---|
1272 | fZd2->ResetPosHasChanged();
|
---|
1273 | fAz->ResetPosHasChanged();
|
---|
1274 | do
|
---|
1275 | {
|
---|
1276 | phca1 = fZd1->PosHasChanged();
|
---|
1277 | phca2 = fZd2->PosHasChanged();
|
---|
1278 | phcaz = fAz->PosHasChanged();
|
---|
1279 | usleep(1);
|
---|
1280 | } while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking);
|
---|
1281 |
|
---|
1282 | //---usleep(100000); // 0.1s
|
---|
1283 |
|
---|
1284 | //
|
---|
1285 | // get position, where we are
|
---|
1286 | //
|
---|
1287 | old = ist;
|
---|
1288 | ist = GetSePos(); // [se]
|
---|
1289 |
|
---|
1290 | //
|
---|
1291 | // if the position didn't change continue
|
---|
1292 | //
|
---|
1293 | /*---
|
---|
1294 | if ((int)ist.Zd() == (int)old.Zd() &&
|
---|
1295 | (int)ist.Az() == (int)old.Az())
|
---|
1296 | continue;
|
---|
1297 | */
|
---|
1298 | istre = GetRePosPdo();
|
---|
1299 |
|
---|
1300 | //
|
---|
1301 | // Get time from last shaftencoder position change (position: ist)
|
---|
1302 | // FIXME: I cannot take the avarage
|
---|
1303 | //
|
---|
1304 | // FIXME
|
---|
1305 | //time.Zd(fZd1->GetMjd());
|
---|
1306 | /* OLD* */
|
---|
1307 | if (fZd1->GetMjd()>fZd2->GetMjd())
|
---|
1308 | time.Zd(fZd1->GetMjd());
|
---|
1309 | else
|
---|
1310 | time.Zd(fZd2->GetMjd());
|
---|
1311 |
|
---|
1312 | //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
|
---|
1313 | time.Az(fAz->GetMjd());
|
---|
1314 |
|
---|
1315 | //
|
---|
1316 | // if Shaftencoder changed position
|
---|
1317 | // calculate were we should be
|
---|
1318 | //
|
---|
1319 | if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/)
|
---|
1320 | {
|
---|
1321 | sla.SetMjd(time.Zd());
|
---|
1322 |
|
---|
1323 | ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
|
---|
1324 | sollzd = CorrectTarget(ist, dummy); // [se]
|
---|
1325 |
|
---|
1326 | fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight);
|
---|
1327 | }
|
---|
1328 |
|
---|
1329 | if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
|
---|
1330 | {
|
---|
1331 | sla.SetMjd(time.Az());
|
---|
1332 |
|
---|
1333 | ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
|
---|
1334 | sollaz = CorrectTarget(ist, dummy); // [se]
|
---|
1335 |
|
---|
1336 | fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
|
---|
1337 | }
|
---|
1338 |
|
---|
1339 | ZdAz soll(sollzd.Zd(), sollaz.Az()); // [se]
|
---|
1340 | fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384);
|
---|
1341 |
|
---|
1342 | fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
|
---|
1343 | (ist.Az()-sollaz.Az())*kGearRatio.Y());
|
---|
1344 |
|
---|
1345 | //--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
|
---|
1346 | //--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " ";
|
---|
1347 | //--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " ";
|
---|
1348 | //--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl;
|
---|
1349 | }
|
---|
1350 |
|
---|
1351 | lout << "Tracking Thread done." << endl;
|
---|
1352 |
|
---|
1353 | //--- fout << endl << endl;
|
---|
1354 | }
|
---|
1355 |
|
---|
1356 | void MCosy::TalkThreadSeTest()
|
---|
1357 | {
|
---|
1358 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
---|
1359 | // return;
|
---|
1360 |
|
---|
1361 | if (fHist)
|
---|
1362 | {
|
---|
1363 | lout << "You are much too fast... try again." << endl;
|
---|
1364 | return;
|
---|
1365 | }
|
---|
1366 |
|
---|
1367 | fHist = new TH2F("Diff", "Difference of SE values",
|
---|
1368 | 201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
|
---|
1369 | fHist->SetXTitle("ZA [\\circ]");
|
---|
1370 | fHist->SetYTitle("\\Delta SE");
|
---|
1371 |
|
---|
1372 | Double_t offset = 0;
|
---|
1373 |
|
---|
1374 | int cnt = 0;
|
---|
1375 |
|
---|
1376 | lout << "Starting Shaftencoder Test..." << endl;
|
---|
1377 |
|
---|
1378 | while (fBackground==kBgdSeTest)
|
---|
1379 | {
|
---|
1380 | fZd1->ResetPosHasChanged();
|
---|
1381 | fZd2->ResetPosHasChanged();
|
---|
1382 |
|
---|
1383 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
---|
1384 | fBackground==kBgdSeTest)
|
---|
1385 | usleep(1);
|
---|
1386 |
|
---|
1387 | const Double_t pos[3] = {
|
---|
1388 | (fZd1->GetPos()+8192)%16384,
|
---|
1389 | (fZd2->GetPos()+8192)%16384,
|
---|
1390 | fAz->GetPos() };
|
---|
1391 |
|
---|
1392 | //
|
---|
1393 | // Estimate Offset from the first ten positions
|
---|
1394 | //
|
---|
1395 | if (cnt++<10)
|
---|
1396 | {
|
---|
1397 | offset += pos[0]+pos[1];
|
---|
1398 | continue;
|
---|
1399 | }
|
---|
1400 | if (cnt==11)
|
---|
1401 | {
|
---|
1402 | offset /= 10;
|
---|
1403 | cnt++;
|
---|
1404 | }
|
---|
1405 |
|
---|
1406 | Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
|
---|
1407 |
|
---|
1408 | ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
|
---|
1409 | fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
|
---|
1410 | }
|
---|
1411 |
|
---|
1412 | lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;
|
---|
1413 |
|
---|
1414 | fBackground=kBgdSeTestDispl;
|
---|
1415 | }
|
---|
1416 |
|
---|
1417 | void MCosy::TalkThreadGear()
|
---|
1418 | {
|
---|
1419 | // if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
|
---|
1420 | // return;
|
---|
1421 |
|
---|
1422 | if (fHist)
|
---|
1423 | {
|
---|
1424 | lout << "You are much too fast... try again." << endl;
|
---|
1425 | return;
|
---|
1426 | }
|
---|
1427 |
|
---|
1428 | // fHist = new TH2F("Gear", "Gear Ratio Re/Se",
|
---|
1429 | // 201, fMin.Zd(), fMax.Zd(), 61, 349.5, 500.5);
|
---|
1430 | fHist = new TH2F("Gear", "Gear Ratio Re/Se",
|
---|
1431 | 201, fMin.Az(), fMax.Az(), 61, 419.5, 570.5);
|
---|
1432 | fHist->SetXTitle("ZA [\\circ]");
|
---|
1433 | fHist->SetYTitle("Re/Se");
|
---|
1434 |
|
---|
1435 | lout << "Starting Gear determination..." << endl;
|
---|
1436 |
|
---|
1437 | ZdAz se0 = GetSePos();
|
---|
1438 | ZdAz re0 = GetRePosPdo();
|
---|
1439 |
|
---|
1440 | while (fBackground==kBgdGear)
|
---|
1441 | {
|
---|
1442 | fZd1->ResetPosHasChanged();
|
---|
1443 | fZd2->ResetPosHasChanged();
|
---|
1444 | fAz->ResetPosHasChanged();
|
---|
1445 |
|
---|
1446 | while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
|
---|
1447 | !fAz->PosHasChanged() && fBackground==kBgdGear)
|
---|
1448 | usleep(1);
|
---|
1449 |
|
---|
1450 | ZdAz se = GetSePos();
|
---|
1451 | ZdAz re = GetRePosPdo();
|
---|
1452 |
|
---|
1453 | ZdAz dse = se-se0;
|
---|
1454 | ZdAz dre = re-re0;
|
---|
1455 |
|
---|
1456 | if (fabs(dse.Zd())*144>16384) // Each 2.5deg
|
---|
1457 | {
|
---|
1458 | se0.Zd(se.Zd());
|
---|
1459 | re0.Zd(re.Zd());
|
---|
1460 |
|
---|
1461 | ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
|
---|
1462 | fHist->Fill(bend.Zd(), dre.Zd()/dse.Zd());
|
---|
1463 | }
|
---|
1464 |
|
---|
1465 | if (fabs(dse.Az())*144>16384) // Each 2.5deg
|
---|
1466 | {
|
---|
1467 | se0.Az(se.Az());
|
---|
1468 | re0.Az(re.Az());
|
---|
1469 |
|
---|
1470 | ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
|
---|
1471 | fHist->Fill(bend.Az(), dre.Az()/dse.Az());
|
---|
1472 |
|
---|
1473 | cout << bend.Az() << ": " << dre.Az()/dse.Az() << endl;
|
---|
1474 | }
|
---|
1475 |
|
---|
1476 | /*
|
---|
1477 | const Double_t pos[3] = {
|
---|
1478 | (fZd1->GetPos()+8192)%16384,
|
---|
1479 | (fZd2->GetPos()+8192)%16384,
|
---|
1480 | fAz->GetPos() };
|
---|
1481 |
|
---|
1482 | //
|
---|
1483 | // Estimate Offset from the first ten positions
|
---|
1484 | //
|
---|
1485 | if (cnt++<10)
|
---|
1486 | {
|
---|
1487 | offset += pos[0]+pos[1];
|
---|
1488 | continue;
|
---|
1489 | }
|
---|
1490 | if (cnt==11)
|
---|
1491 | {
|
---|
1492 | offset /= 10;
|
---|
1493 | cnt++;
|
---|
1494 | }
|
---|
1495 |
|
---|
1496 | Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
|
---|
1497 |
|
---|
1498 | ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
|
---|
1499 | fHistTestSe->Fill(bend.Zd(), pos[0]+pos[1]-offset);
|
---|
1500 | */
|
---|
1501 | }
|
---|
1502 | lout << "Gear Test Stopped... displaying Histogram." << endl;
|
---|
1503 |
|
---|
1504 | fBackground=kBgdGearDispl;
|
---|
1505 | }
|
---|
1506 |
|
---|
1507 | void MCosy::TalkThread()
|
---|
1508 | {
|
---|
1509 | /* ========== FIXME? =============
|
---|
1510 | if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
|
---|
1511 | return;
|
---|
1512 | */
|
---|
1513 |
|
---|
1514 | if (fMac1 && fMac2)
|
---|
1515 | {
|
---|
1516 | fMac1->ReqPos();
|
---|
1517 | fMac2->ReqPos();
|
---|
1518 | }
|
---|
1519 |
|
---|
1520 | InitSync();
|
---|
1521 |
|
---|
1522 | /*** FOR DEMO MODE ***/
|
---|
1523 | if (!fZd1 || !fZd2 || !fAz)
|
---|
1524 | return;
|
---|
1525 | /*** FOR DEMO MODE ***/
|
---|
1526 |
|
---|
1527 | //
|
---|
1528 | // Start the Network
|
---|
1529 | //
|
---|
1530 | while (1)
|
---|
1531 | {
|
---|
1532 | //
|
---|
1533 | // wait until a tracking session is started
|
---|
1534 | //
|
---|
1535 | while (fBackground==kBgdNone)
|
---|
1536 | usleep(1);
|
---|
1537 |
|
---|
1538 | switch (fBackground)
|
---|
1539 | {
|
---|
1540 | case kBgdNone:
|
---|
1541 | continue;
|
---|
1542 |
|
---|
1543 | case kBgdTracking:
|
---|
1544 | TalkThreadTracking();
|
---|
1545 | continue;
|
---|
1546 |
|
---|
1547 | case kBgdSeTest:
|
---|
1548 | TalkThreadSeTest();
|
---|
1549 | continue;
|
---|
1550 |
|
---|
1551 | case kBgdGear:
|
---|
1552 | TalkThreadGear();
|
---|
1553 | continue;
|
---|
1554 |
|
---|
1555 | default:
|
---|
1556 | continue;
|
---|
1557 | }
|
---|
1558 | }
|
---|
1559 | }
|
---|
1560 |
|
---|
1561 | Bool_t MCosy::HandleTimer(TTimer *t)
|
---|
1562 | {
|
---|
1563 | //
|
---|
1564 | // Update Gui, foremer MTGui.
|
---|
1565 | //
|
---|
1566 | if (fZd1)
|
---|
1567 | fZd1->DisplayVal();
|
---|
1568 | if (fZd2)
|
---|
1569 | fZd2->DisplayVal();
|
---|
1570 | if (fAz)
|
---|
1571 | fAz->DisplayVal();
|
---|
1572 |
|
---|
1573 | ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
|
---|
1574 | ZdAz bendist = fBending.CorrectBack(seist);
|
---|
1575 |
|
---|
1576 | Byte_t avail = 0;
|
---|
1577 |
|
---|
1578 | avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
|
---|
1579 | avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
|
---|
1580 | avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
|
---|
1581 | avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0;
|
---|
1582 | avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0;
|
---|
1583 | avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0;
|
---|
1584 |
|
---|
1585 | if (HasError())
|
---|
1586 | SetStatus(MCosy::kError);
|
---|
1587 |
|
---|
1588 | lout.UpdateGui();
|
---|
1589 |
|
---|
1590 | fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError/kGearRatio2,
|
---|
1591 | fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail);
|
---|
1592 |
|
---|
1593 | /*
|
---|
1594 | cout << (int)(fMac1->GetStatus()&Macs::kOutOfControl) << " ";
|
---|
1595 | cout << (int)(fMac2->GetStatus()&Macs::kOutOfControl) << " ";
|
---|
1596 | cout << (int)(fMac3->GetStatus()&Macs::kOutOfControl) << endl;
|
---|
1597 | */
|
---|
1598 |
|
---|
1599 | const Bool_t trigger = fTriggerDisplay;
|
---|
1600 | fTriggerDisplay = kFALSE;
|
---|
1601 |
|
---|
1602 | if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
|
---|
1603 | DisplayHistTestSe(!trigger);
|
---|
1604 |
|
---|
1605 | if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
|
---|
1606 | DisplayHistGear(!trigger);
|
---|
1607 |
|
---|
1608 | return kTRUE;
|
---|
1609 | }
|
---|
1610 |
|
---|
1611 | void MCosy::DisplayHistTestSe(Bool_t del)
|
---|
1612 | {
|
---|
1613 | lout << "Displaying histogram..." << endl;
|
---|
1614 |
|
---|
1615 | TH2F &hist = *fHist;
|
---|
1616 |
|
---|
1617 | if (del)
|
---|
1618 | {
|
---|
1619 | fHist = NULL;
|
---|
1620 | fBackground = kBgdNone;
|
---|
1621 | }
|
---|
1622 |
|
---|
1623 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
---|
1624 | c->Divide(1,2);
|
---|
1625 |
|
---|
1626 | c->cd(1);
|
---|
1627 | TH2 *h=(TH2*)hist.DrawCopy();
|
---|
1628 |
|
---|
1629 | TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
|
---|
1630 | p->SetLineColor(kBlue);
|
---|
1631 | p->Draw("same");
|
---|
1632 | p->SetBit(kCanDelete);
|
---|
1633 |
|
---|
1634 | c->cd(2);
|
---|
1635 |
|
---|
1636 | TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
|
---|
1637 | hist.GetBinLowEdge(hist.GetNbinsX()+1));
|
---|
1638 | p2.SetXTitle("ZA [\\circ]");
|
---|
1639 | for (int i=0; i<hist.GetNbinsX(); i++)
|
---|
1640 | p2.SetBinError(i, p->GetBinError(i));
|
---|
1641 | p2.SetLineColor(kRed);
|
---|
1642 | p2.SetStats(0);
|
---|
1643 | p2.DrawCopy();
|
---|
1644 |
|
---|
1645 | if (del)
|
---|
1646 | delete &hist;
|
---|
1647 | }
|
---|
1648 |
|
---|
1649 | void MCosy::DisplayHistGear(Bool_t del)
|
---|
1650 | {
|
---|
1651 | lout << "Displaying histogram..." << endl;
|
---|
1652 |
|
---|
1653 | TH2F &hist = *fHist;
|
---|
1654 |
|
---|
1655 | if (del)
|
---|
1656 | {
|
---|
1657 | fHist = NULL;
|
---|
1658 | fBackground = kBgdNone;
|
---|
1659 | }
|
---|
1660 |
|
---|
1661 | TCanvas *c=new TCanvas("c1", "", 1000, 1000);
|
---|
1662 | c->Divide(1,2);
|
---|
1663 |
|
---|
1664 | c->cd(1);
|
---|
1665 | TH2 *h=(TH2*)hist.DrawCopy();
|
---|
1666 |
|
---|
1667 | TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
|
---|
1668 | p->SetLineColor(kBlue);
|
---|
1669 | p->Draw("same");
|
---|
1670 | p->SetBit(kCanDelete);
|
---|
1671 |
|
---|
1672 | c->cd(2);
|
---|
1673 |
|
---|
1674 | TH1F p2("spread", "Spread of the gear [16384/1500/4*U/U]", hist.GetNbinsX(), hist.GetBinLowEdge(1),
|
---|
1675 | hist.GetBinLowEdge(hist.GetNbinsX()+1));
|
---|
1676 | p2.SetXTitle("ZA [\\circ]");
|
---|
1677 | for (int i=0; i<hist.GetNbinsX(); i++)
|
---|
1678 | p2.SetBinError(i, p->GetBinError(i));
|
---|
1679 | p2.SetLineColor(kRed);
|
---|
1680 | p2.SetStats(0);
|
---|
1681 | p2.DrawCopy();
|
---|
1682 |
|
---|
1683 | if (del)
|
---|
1684 | delete &hist;
|
---|
1685 | }
|
---|
1686 |
|
---|
1687 | // --------------------------------------------------------------------------
|
---|
1688 | //
|
---|
1689 | // Start the work of the application:
|
---|
1690 | //
|
---|
1691 | // Start the Can-Network.
|
---|
1692 | // Start the MCosy::TalkThread thread.
|
---|
1693 | // turn on the gui update
|
---|
1694 | //
|
---|
1695 | void MCosy::Start()
|
---|
1696 | {
|
---|
1697 | // Don't call this function twice!
|
---|
1698 | Network::Start();
|
---|
1699 |
|
---|
1700 | ReadConfig();
|
---|
1701 |
|
---|
1702 | lout << "- Starting TX Thread." << endl;
|
---|
1703 | fTTalk = new MTTalk(this);
|
---|
1704 |
|
---|
1705 | lout << "- Starting GUI update." << endl;
|
---|
1706 | fUpdateGui->TurnOn();
|
---|
1707 | }
|
---|
1708 |
|
---|
1709 | // --------------------------------------------------------------------------
|
---|
1710 | //
|
---|
1711 | // Start the work of the application:
|
---|
1712 | //
|
---|
1713 | // Turn of the gui update
|
---|
1714 | // stop the MCosy::TalkThread thread.
|
---|
1715 | // Stop the network
|
---|
1716 | //
|
---|
1717 | void MCosy::Stop()
|
---|
1718 | {
|
---|
1719 | lout << "- Stopping GUI update." << endl;
|
---|
1720 | fUpdateGui->TurnOff();
|
---|
1721 | lout << "- GUI Update stopped." << endl;
|
---|
1722 |
|
---|
1723 | delete fTTalk;
|
---|
1724 | lout << "- TX Thread stopped." << endl;
|
---|
1725 |
|
---|
1726 | Network::Stop();
|
---|
1727 | }
|
---|
1728 |
|
---|
1729 | // --------------------------------------------------------------------------
|
---|
1730 | //
|
---|
1731 | // Disable the synchronization by using a negative CAN Id for id2.
|
---|
1732 | //
|
---|
1733 | void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
|
---|
1734 | Int_t id4, Int_t id5, Int_t id6)
|
---|
1735 | {
|
---|
1736 | //
|
---|
1737 | // Create Nodes
|
---|
1738 | //
|
---|
1739 | lout << "- Setting up network." << endl;
|
---|
1740 |
|
---|
1741 | fMac1=new Macs(id1, "Mac/Az", lout);
|
---|
1742 | fMac2=new Macs(id3, "Mac/Zd", lout);
|
---|
1743 | if (id2>=0)
|
---|
1744 | fMac3=new Macs(id2, "Mac/Az-Sync", lout);
|
---|
1745 |
|
---|
1746 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
---|
1747 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
---|
1748 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
---|
1749 |
|
---|
1750 | lout << "- Connecting devices to network." << endl;
|
---|
1751 |
|
---|
1752 | //
|
---|
1753 | // Connect the devices to the network
|
---|
1754 | //
|
---|
1755 | SetNode(fMac1);
|
---|
1756 | SetNode(fMac2);
|
---|
1757 | if (id2>=0)
|
---|
1758 | SetNode(fMac3);
|
---|
1759 | SetNode(fZd1);
|
---|
1760 | SetNode(fZd2);
|
---|
1761 | SetNode(fAz);
|
---|
1762 |
|
---|
1763 | //
|
---|
1764 | // Create Gui Event timer and Gui
|
---|
1765 | //
|
---|
1766 | lout << "- Initializing GUI Timer." << endl;
|
---|
1767 | fUpdateGui = new TTimer(this, 100); // 100ms
|
---|
1768 |
|
---|
1769 | lout << "- Starting GUI." << endl;
|
---|
1770 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
---|
1771 | }
|
---|
1772 |
|
---|
1773 | void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
|
---|
1774 | {
|
---|
1775 | //
|
---|
1776 | // Create Nodes
|
---|
1777 | //
|
---|
1778 | lout << "- Setting up network." << endl;
|
---|
1779 |
|
---|
1780 | fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
|
---|
1781 | fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
|
---|
1782 | fAz =new ShaftEncoder(id6, "SE/Az", lout);
|
---|
1783 |
|
---|
1784 | lout << "- Connecting devices to network." << endl;
|
---|
1785 |
|
---|
1786 | //
|
---|
1787 | // Connect the devices to the network
|
---|
1788 | //
|
---|
1789 | SetNode(fZd1);
|
---|
1790 | SetNode(fZd2);
|
---|
1791 | SetNode(fAz);
|
---|
1792 |
|
---|
1793 | //
|
---|
1794 | // Create Gui Event timer and Gui
|
---|
1795 | //
|
---|
1796 | lout << "- Initializing GUI Timer." << endl;
|
---|
1797 | fUpdateGui = new TTimer(this, 100); // 100ms
|
---|
1798 |
|
---|
1799 | lout << "- Starting GUI." << endl;
|
---|
1800 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
---|
1801 | }
|
---|
1802 |
|
---|
1803 | void MCosy::ConstructorDemo()
|
---|
1804 | {
|
---|
1805 | //
|
---|
1806 | // Create Nodes
|
---|
1807 | //
|
---|
1808 | lout << "- Setting up network." << endl;
|
---|
1809 |
|
---|
1810 | //
|
---|
1811 | // Create Gui Event timer and Gui
|
---|
1812 | //
|
---|
1813 | lout << "- Initializing GUI Timer." << endl;
|
---|
1814 | fUpdateGui = new TTimer(this, 100); // 100ms
|
---|
1815 |
|
---|
1816 | lout << "- Starting GUI." << endl;
|
---|
1817 | fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
|
---|
1818 | }
|
---|
1819 |
|
---|
1820 | bool MCosy::HasZombie() const
|
---|
1821 | {
|
---|
1822 | bool ses = fZd1->IsZombieNode() | fZd2->IsZombieNode();
|
---|
1823 |
|
---|
1824 | return fMac1->IsZombieNode() | fMac2->IsZombieNode() | fAz->IsZombieNode() | ses;
|
---|
1825 | }
|
---|
1826 |
|
---|
1827 | MCosy::MCosy(int mode, const char *dev, const int baud, MLog &out)
|
---|
1828 | : Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone)
|
---|
1829 | {
|
---|
1830 | TEnv env(".cosyrc");
|
---|
1831 | const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
|
---|
1832 | const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
|
---|
1833 | const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3
|
---|
1834 | const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4
|
---|
1835 | const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5
|
---|
1836 | const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6
|
---|
1837 |
|
---|
1838 | lout << "- Program in ";
|
---|
1839 | switch (mode)
|
---|
1840 | {
|
---|
1841 | case 0:
|
---|
1842 | lout << "<<Stanard mode>>" << endl;
|
---|
1843 | fBending.Load("bending.txt");
|
---|
1844 | Constructor(id1, id2, id3, id4, id5, id6);
|
---|
1845 | break;
|
---|
1846 | case 1:
|
---|
1847 | lout << "<<SE mode>>" << endl;
|
---|
1848 | fBending.Load("bending.txt");
|
---|
1849 | ConstructorSE(id4, id5, id6);
|
---|
1850 | break;
|
---|
1851 | default:
|
---|
1852 | lout << "<<Demo mode>>" << endl;
|
---|
1853 | ConstructorDemo();
|
---|
1854 | }
|
---|
1855 |
|
---|
1856 | lout.SetOutputGui(fWin->GetLog(), kTRUE);
|
---|
1857 |
|
---|
1858 | fZd1->SetDisplay(fWin->GetLabel2());
|
---|
1859 | fZd2->SetDisplay(fWin->GetLabel3());
|
---|
1860 | fAz->SetDisplay(fWin->GetLabel1());
|
---|
1861 |
|
---|
1862 | int i=0;
|
---|
1863 | char name[100];
|
---|
1864 | while (1)
|
---|
1865 | {
|
---|
1866 | sprintf(name, "/home/tbretz/TPoint/tpoint%03d.txt", i++);
|
---|
1867 | if (gSystem->AccessPathName(name, kFileExists))
|
---|
1868 | break;
|
---|
1869 | }
|
---|
1870 |
|
---|
1871 | Timer time;
|
---|
1872 | time.Now();
|
---|
1873 |
|
---|
1874 | tpout = new ofstream(name);
|
---|
1875 | *tpout << "Magic Model TPOINT data file" << endl;
|
---|
1876 | *tpout << ": ALTAZ" << endl;
|
---|
1877 | *tpout << "49 48 0 ";
|
---|
1878 | *tpout << time.Year() << " " << time.Month() << " " << time.Day() << " ";
|
---|
1879 | *tpout << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
|
---|
1880 | // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
|
---|
1881 | }
|
---|
1882 |
|
---|
1883 | void MCosy::TerminateApp()
|
---|
1884 | {
|
---|
1885 | cout << "MCosy::TerminateApp()" << endl;
|
---|
1886 | /*
|
---|
1887 | Int_t rc;
|
---|
1888 | TGMessageBox msg(this, gClient->GetRoot(),
|
---|
1889 | "Information",
|
---|
1890 | "Cosy is shutting down the system - this may take wa while!",
|
---|
1891 | kMBIconExclamation,
|
---|
1892 | kMBOK, //kMBClose
|
---|
1893 | &rc, 0);
|
---|
1894 | */
|
---|
1895 |
|
---|
1896 | lout.DisableOutputDevice(MLog::eGui);
|
---|
1897 | // FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
|
---|
1898 | // lout.SetOutputGui(NULL, kFALSE);
|
---|
1899 |
|
---|
1900 | gApplication->Terminate(0);
|
---|
1901 | }
|
---|
1902 |
|
---|
1903 | MCosy::~MCosy()
|
---|
1904 | {
|
---|
1905 | *tpout << "END" << endl;
|
---|
1906 | delete tpout;
|
---|
1907 |
|
---|
1908 | cout << "Deleting GUI timer." << endl;
|
---|
1909 |
|
---|
1910 | delete fUpdateGui;
|
---|
1911 |
|
---|
1912 | cout << "Deleting Nodes." << endl;
|
---|
1913 |
|
---|
1914 | delete fAz;
|
---|
1915 | delete fZd1;
|
---|
1916 | delete fZd2;
|
---|
1917 | delete fMac1;
|
---|
1918 | delete fMac2;
|
---|
1919 | if (fMac3)
|
---|
1920 | delete fMac3;
|
---|
1921 |
|
---|
1922 | cout << "Deleting MGCosy." << endl;
|
---|
1923 |
|
---|
1924 | lout.DisableOutputDevice(MLog::eGui);
|
---|
1925 |
|
---|
1926 | delete fWin;
|
---|
1927 |
|
---|
1928 | cout << "MGCosy destructed." << endl;
|
---|
1929 | }
|
---|