source: trunk/Cosy/main/MSlewing.cc@ 12105

Last change on this file since 12105 was 10008, checked in by tbretz, 14 years ago
Mainly unit fixes to get the moon tracking working.
File size: 6.5 KB
Line 
1#include "MSlewing.h"
2#include "MSlewing.h"
3
4#include "MLogManip.h"
5
6#include "MCosy.h"
7#include "dkc.h"
8#include "MDriveCom.h"
9
10#include "MString.h"
11#include "MPointing.h"
12
13ClassImp(MSlewing);
14
15using namespace std;
16
17//#define EXPERT
18#undef EXPERT
19
20bool MSlewing::SetAcc(Dkc *mac, Float_t acc)
21{
22 // FIXME: Get acceleration scale from DKC!
23 mac->SetAcceleration(TMath::Nint(acc*1000000000));
24 return !mac->IsZombieNode();
25}
26
27// --------------------------------------------------------------------------
28//
29// set the velocity and accelerations for position maneuvers.
30//
31// The acceleratin is set as given (in percent of maximum).
32// The velocity is given in percent, depending on the ratio (<1 or >1)
33// one of the axis becomes a slower velocity. This is used for maneuvers
34// in which both axis are moved synchromously and should reach their
35// target position at the same time.
36//
37void MSlewing::SetPosVelocity(const ZdAz &res, Float_t vel)
38{
39 const Double_t taz = TMath::Abs(res.Az())/fCosy->fMac1->GetVelMax();
40 const Double_t tzd = TMath::Abs(res.Zd())/fCosy->fMac2->GetVelMax();
41
42 if (tzd > taz)
43 {
44 fCosy->fMac1->SetVelocityRel(vel*TMath::Abs(res.Az()/res.Zd()));
45 fCosy->fMac2->SetVelocityRel(vel);
46 }
47 else
48 {
49 fCosy->fMac1->SetVelocityRel(vel);
50 fCosy->fMac2->SetVelocityRel(vel*TMath::Abs(res.Zd()/res.Az()));
51 }
52}
53
54// --------------------------------------------------------------------------
55//
56// Does an absolute positioning.
57//
58// The steps to move are given in a ZdAz object relative to the current
59// position. The coordinates are given in Roteryencoder steps.
60// Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only
61// if Axis 2==kTRUE. The function waits for the movement to be finished.
62//
63void MSlewing::DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
64{
65 if (fCosy->HasZombie())
66 return;
67
68 fCosy->SetStatus(MDriveCom::kMoving);
69
70 if (axe1) fCosy->fMac2->StartAbsPosRev(rd.Zd());
71 if (axe2) fCosy->fMac1->StartAbsPosRev(rd.Az());
72
73 if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 0);
74 if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 0);
75
76 // FIXME: We need a delay here to account for the delay of the
77 // toggle bit in the SPS. We need a more precise return value from
78 // the SPS:
79 usleep(150000);
80
81#ifdef EXPERT
82 cout << "Waiting for positioning..." << flush;
83#endif
84 fCosy->WaitForEndMovement();
85#ifdef EXPERT
86 cout << "done." << endl;
87#endif
88}
89
90bool MSlewing::Break()
91{
92 return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie();
93}
94
95
96// --------------------------------------------------------------------------
97//
98// Caluclate the difference between feedback 1 and feedback 2 at
99// the given zenith angle (feedback 2)
100//
101Double_t MSlewing::GetDiff(const ZdAz &za) const
102{
103 const Double_t zd = za.Zd(); //[revolutions]
104
105 const Double_t sh = -1.21 *(TMath::SinH(0.916*zd*TMath::TwoPi())-1);
106 const Double_t cs = 0.667 *TMath::Cos(1.735*(zd-0.236)*TMath::TwoPi());
107 const Double_t of = 0.6497;
108
109 return (sh+cs+of)/360; //[revolutions]
110}
111
112// --------------------------------------------------------------------------
113//
114// Move the telescope to the given position. The position must be given in
115// a ZdAz object in rad.
116//
117// The first positioning is done absolutely. If we didn't reach the
118// correct psotion we try to correct for this by 10 relative position
119// maneuvers. If this doesn't help positioning failed.
120//
121// As a reference the shaftencoder values are used.
122//
123int MSlewing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
124{
125 gLog << all << MTime(-1) << " - Target Position: " << dst.Zd()*TMath::RadToDeg() << "deg, " << dst.Az()*TMath::RadToDeg() << "deg (Zd/Az)" << endl;
126
127 //
128 // Because we agreed on I don't search for the shortest move
129 // anymore
130 //
131 // const ZdAz dest = CorrectTarget(src, dst);
132 //
133 const ZdAz bend = fCosy->fBending(dst); // [rad]
134 const ZdAz dest = bend/TMath::TwoPi(); // [revolutions]
135
136 // Check whether bending is valid!
137
138 if (!fCosy->CheckRange(bend))
139 return kFALSE;
140
141 fCosy->fZdAzSoll = dst;
142
143 int i;
144 for (i=0; i<(track?1:10) && !Break(); i++)
145 {
146 gLog << inf2 << "- Step #" << i << endl;
147
148 // Get feedback 2
149 const ZdAz sepos = fCosy->GetSePos();
150
151 // Calculate residual to move deviation
152 const ZdAz res = dest-sepos; // [revolutions]
153
154 gLog << inf2 << "- Shaftencoders show a residual deviation of dZd=";
155 gLog << MString::Format("%.2f", res.Zd()*360*60) << "' and dAz=";
156 gLog << MString::Format("%.2f", res.Az()*360*60) << "'" << endl;
157
158 // Check which axis should still be moved
159 ZdAz cd = res; // [revolutions]
160 cd *= 1./fMaxResidual; // Scale to units of the maximum residual
161 cd.Abs();
162
163 // Check if there is a control deviation on the axis
164 const Bool_t cdzd = cd.Zd()>0.5 ? kTRUE : kFALSE;
165 const Bool_t cdaz = cd.Az()>0.5 ? kTRUE : kFALSE;
166
167 // check if we reached the correct position already
168 if (!cdzd && !cdaz)
169 {
170 gLog << all << MTime(-1) << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
171 fCosy->SetStatus(MDriveCom::kStopped);
172 fCosy->fCom->SendStatus("Target position reached.");
173 return TRUE;
174 }
175
176 // ==============================================
177 // Estimate the noncircularity of the zd axis
178 const Double_t add = GetDiff(sepos)-GetDiff(dest);
179
180 const ZdAz dest2(dest.Zd()+add, dest.Az());
181 const ZdAz res2 = dest-sepos;
182 // =================================================
183
184 //gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl;
185
186 SetAcc(fCosy->fMac1, fAcc.Az());
187 SetAcc(fCosy->fMac2, fAcc.Zd());
188
189 SetPosVelocity(res2, fVel);
190
191 gLog << inf2 << "- Do absolute positioning..." << endl;
192 DoAbsPos(dest2, cdzd, cdaz);
193 gLog << inf2 << "- Absolute Positioning Done" << endl;
194 }
195 if (i==1 && track && !Break())
196 {
197 gLog << all << MTime(-1) << " - Positioning done." << endl;
198 fCosy->SetStatus(MDriveCom::kStopped);
199 fCosy->fCom->SendStatus("Tracking preposition reached.");
200 return TRUE;
201 }
202
203 if (i<10)
204 fCosy->StopMovement();
205 else
206 fCosy->SetStatus(MDriveCom::kStopped);
207
208 gLog << warn << MTime(-1) << " - Warning: Requested position not reached (i=" << i << ")" << endl;
209
210 fCosy->fCom->SendStatus("Target position missed!");
211
212 return FALSE;
213}
Note: See TracBrowser for help on using the repository browser.