| 1 | #include "MPointing.h"
|
|---|
| 2 |
|
|---|
| 3 | #include "MCosy.h"
|
|---|
| 4 | #include "macs.h"
|
|---|
| 5 | #include "MDriveCom.h"
|
|---|
| 6 |
|
|---|
| 7 | ClassImp(MPointing);
|
|---|
| 8 |
|
|---|
| 9 | using namespace std;
|
|---|
| 10 |
|
|---|
| 11 | //#define EXPERT
|
|---|
| 12 | #undef EXPERT
|
|---|
| 13 |
|
|---|
| 14 | // --------------------------------------------------------------------------
|
|---|
| 15 | //
|
|---|
| 16 | // set the velocity and accelerations for position maneuvers.
|
|---|
| 17 | //
|
|---|
| 18 | // The acceleratin is set as given (in percent of maximum).
|
|---|
| 19 | // The velocity is given in percent, depending on the ratio (<1 or >1)
|
|---|
| 20 | // one of the axis becomes a slower velocity. This is used for maneuvers
|
|---|
| 21 | // in which both axis are moved synchromously and should reach their
|
|---|
| 22 | // target position at the same time.
|
|---|
| 23 | //
|
|---|
| 24 | void MPointing::SetPosVelocity(const Float_t ratio, Float_t vel)
|
|---|
| 25 | {
|
|---|
| 26 | //
|
|---|
| 27 | // Set velocities
|
|---|
| 28 | //
|
|---|
| 29 | const int vr = fCosy->fMac1->GetVelRes();
|
|---|
| 30 | vel *= vr;
|
|---|
| 31 |
|
|---|
| 32 | if (ratio<1)
|
|---|
| 33 | {
|
|---|
| 34 | fCosy->fMac1->SetVelocity(TMath::Nint(vel));
|
|---|
| 35 | fCosy->fMac2->SetVelocity(TMath::Nint(vel*ratio));
|
|---|
| 36 | }
|
|---|
| 37 | else
|
|---|
| 38 | {
|
|---|
| 39 | fCosy->fMac1->SetVelocity(TMath::Nint(vel/ratio));
|
|---|
| 40 | fCosy->fMac2->SetVelocity(TMath::Nint(vel));
|
|---|
| 41 | }
|
|---|
| 42 | }
|
|---|
| 43 |
|
|---|
| 44 | // --------------------------------------------------------------------------
|
|---|
| 45 | //
|
|---|
| 46 | // Does a relative positioning.
|
|---|
| 47 | //
|
|---|
| 48 | // The steps to move are given in a ZdAz object relative to the current
|
|---|
| 49 | // position. The coordinates are given in Roteryencoder steps.
|
|---|
| 50 | // Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only
|
|---|
| 51 | // if Axis 2==kTRUE. The function waits for the movement to be finished.
|
|---|
| 52 | //
|
|---|
| 53 | void MPointing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
|
|---|
| 54 | {
|
|---|
| 55 | if (fCosy->HasZombie())
|
|---|
| 56 | return;
|
|---|
| 57 |
|
|---|
| 58 | fCosy->SetStatus(MDriveCom::kMoving);
|
|---|
| 59 |
|
|---|
| 60 | if (axe1) fCosy->fMac2->StartRelPos(TMath::Nint(rd.Zd()));
|
|---|
| 61 | if (axe2) fCosy->fMac1->StartRelPos(TMath::Nint(rd.Az()));
|
|---|
| 62 | #ifdef EXPERT
|
|---|
| 63 | cout << "Waiting for positioning..." << flush;
|
|---|
| 64 | #endif
|
|---|
| 65 | if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1);
|
|---|
| 66 | if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1);
|
|---|
| 67 |
|
|---|
| 68 | fCosy->WaitForEndMovement();
|
|---|
| 69 | #ifdef EXPERT
|
|---|
| 70 | cout << "done." << endl;
|
|---|
| 71 | #endif
|
|---|
| 72 | }
|
|---|
| 73 |
|
|---|
| 74 | bool MPointing::SetAccDec(Macs *mac, Float_t acc, Float_t dec)
|
|---|
| 75 | {
|
|---|
| 76 | const int vr = mac->GetVelRes();
|
|---|
| 77 | mac->SetAcceleration(TMath::Nint(acc*vr));
|
|---|
| 78 | mac->SetDeceleration(TMath::Nint(dec*vr));
|
|---|
| 79 | return !mac->IsZombieNode();
|
|---|
| 80 | }
|
|---|
| 81 |
|
|---|
| 82 | bool MPointing::Break()
|
|---|
| 83 | {
|
|---|
| 84 | return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie();
|
|---|
| 85 | }
|
|---|
| 86 |
|
|---|
| 87 | // --------------------------------------------------------------------------
|
|---|
| 88 | //
|
|---|
| 89 | // Move the telescope to the given position. The position must be given in
|
|---|
| 90 | // a ZdAz object in rad.
|
|---|
| 91 | //
|
|---|
| 92 | // The first positioning is done absolutely. If we didn't reach the
|
|---|
| 93 | // correct psotion we try to correct for this by 10 relative position
|
|---|
| 94 | // maneuvers. If this doesn't help positioning failed.
|
|---|
| 95 | //
|
|---|
| 96 | // As a reference the shaftencoder values are used.
|
|---|
| 97 | //
|
|---|
| 98 | int MPointing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
|
|---|
| 99 | {
|
|---|
| 100 | const ZdAz d = dst*kRad2Deg;
|
|---|
| 101 |
|
|---|
| 102 | MTime t(-1);
|
|---|
| 103 | lout << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
|
|---|
| 104 |
|
|---|
| 105 | //
|
|---|
| 106 | // Calculate new target position (shortest distance to go)
|
|---|
| 107 | //
|
|---|
| 108 | //const ZdAz src = fCosy->GetSePos(); // [se]
|
|---|
| 109 |
|
|---|
| 110 | //
|
|---|
| 111 | // Make sure that the motors are in sync mode (necessary if the
|
|---|
| 112 | // MACS has been rebooted from a Zombie state.
|
|---|
| 113 | //
|
|---|
| 114 | //InitSync();
|
|---|
| 115 | //if (fMac3->IsZombieNode())
|
|---|
| 116 | // return false;
|
|---|
| 117 |
|
|---|
| 118 | //
|
|---|
| 119 | // Because we agreed on I don't search for the shortest move
|
|---|
| 120 | // anymore
|
|---|
| 121 | //
|
|---|
| 122 | // const ZdAz dest = CorrectTarget(src, dst);
|
|---|
| 123 | //
|
|---|
| 124 | ZdAz bend = fCosy->fBending(dst); // [rad]
|
|---|
| 125 |
|
|---|
| 126 | const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se]
|
|---|
| 127 |
|
|---|
| 128 | if (!fCosy->CheckRange(bend))
|
|---|
| 129 | return kFALSE;
|
|---|
| 130 |
|
|---|
| 131 | bend *= kRad2Deg;
|
|---|
| 132 | fCosy->fZdAzSoll = dst;
|
|---|
| 133 |
|
|---|
| 134 | //cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl;
|
|---|
| 135 | //cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl;
|
|---|
| 136 | //cout << "Bend'd Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl;
|
|---|
| 137 | //cout << "Bend'd Dest Zd: " << bend.Zd() << "deg Az:" << bend.Az() << "deg" << endl;
|
|---|
| 138 |
|
|---|
| 139 | //
|
|---|
| 140 | // Set velocities
|
|---|
| 141 | //
|
|---|
| 142 | //const int vr = fCosy->fMac1->GetVelRes();
|
|---|
| 143 |
|
|---|
| 144 | const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi();
|
|---|
| 145 |
|
|---|
| 146 | int i;
|
|---|
| 147 | for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++)
|
|---|
| 148 | {
|
|---|
| 149 |
|
|---|
| 150 | lout << "- Step #" << i << endl;
|
|---|
| 151 |
|
|---|
| 152 | // Get Shaft Encoder Positions
|
|---|
| 153 | const ZdAz p=fCosy->GetSePos();
|
|---|
| 154 |
|
|---|
| 155 | // calculate control deviation
|
|---|
| 156 | ZdAz rd = dest-p; // [se]
|
|---|
| 157 | ZdAz cd = rd; // [se]
|
|---|
| 158 | // Correct for having two SE available
|
|---|
| 159 | // FIMXE....
|
|---|
| 160 | // cd.Zd(cd.Zd()*2);
|
|---|
| 161 | // Round to check whether we are as near as possible
|
|---|
| 162 | // to the value we expect
|
|---|
| 163 | cd.Round();
|
|---|
| 164 |
|
|---|
| 165 | // Check if there is a control deviation on the axis
|
|---|
| 166 | const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
|
|---|
| 167 | const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
|
|---|
| 168 |
|
|---|
| 169 | // check if we reached the correct position already
|
|---|
| 170 | if (!cdzd && !cdaz)
|
|---|
| 171 | {
|
|---|
| 172 | t.Now();
|
|---|
| 173 | lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
|
|---|
| 174 | fCosy->SetStatus(MDriveCom::kStopped);
|
|---|
| 175 | fCosy->fCom->SendStatus("Target position reached.");
|
|---|
| 176 | return TRUE;
|
|---|
| 177 | }
|
|---|
| 178 |
|
|---|
| 179 | // ==============================================
|
|---|
| 180 | // Estimate the noncircularity of the zd axis
|
|---|
| 181 | const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE;
|
|---|
| 182 |
|
|---|
| 183 | const double p1 = ist.Zd()-19.0605/kRad2Deg;
|
|---|
| 184 | const double p2 = dst.Zd()-19.0605/kRad2Deg;
|
|---|
| 185 |
|
|---|
| 186 | const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*rad2se;
|
|---|
| 187 | const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se;
|
|---|
| 188 | // ==++=========================================
|
|---|
| 189 |
|
|---|
| 190 | // change units from se to re
|
|---|
| 191 | rd *= fCosy->kGearTot/fCosy->kResSE; // [re]
|
|---|
| 192 | rd.Zd(f2-f1);
|
|---|
| 193 |
|
|---|
| 194 | // Initialize Velocities so that we reach both positions
|
|---|
| 195 | // at the same time
|
|---|
| 196 | if (i)
|
|---|
| 197 | {
|
|---|
| 198 | //lout << "--- LO-SPEED ---" << endl;
|
|---|
| 199 | SetAccDec(fCosy->fMac1, 0.1, 0.1);
|
|---|
| 200 | SetAccDec(fCosy->fMac2, 0.1, 0.1);
|
|---|
| 201 |
|
|---|
| 202 | SetPosVelocity(1.0, 0.05);
|
|---|
| 203 | }
|
|---|
| 204 | else
|
|---|
| 205 | {
|
|---|
| 206 | const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y();
|
|---|
| 207 |
|
|---|
| 208 | // lout << "MPointing::SetPosition y: " << y << " rd.Az(): " << rd.Az() << endl;
|
|---|
| 209 | // lout << "MPointing::SetPosition fCosy->kGearTot.Y(): " << fCosy->kGearTot.Y() << " fCosy->kResSE.Y(): " << fCosy->kResSE.Y() << endl;
|
|---|
| 210 |
|
|---|
| 211 | if (rd.Az()>-y && rd.Az()<y)
|
|---|
| 212 | {
|
|---|
| 213 | //lout << "--- LO-SPEED Mac1 ---" << endl;
|
|---|
| 214 | SetAccDec(fCosy->fMac1, 0.05, 0.05);
|
|---|
| 215 | }
|
|---|
| 216 | else
|
|---|
| 217 | {
|
|---|
| 218 |
|
|---|
| 219 | //lout << "MPointing::SetPosition SetAccDec Mac1: " << fAcc << fDec << endl;
|
|---|
| 220 |
|
|---|
| 221 | SetAccDec(fCosy->fMac1, fAcc, fDec);
|
|---|
| 222 |
|
|---|
| 223 | }
|
|---|
| 224 |
|
|---|
| 225 |
|
|---|
| 226 | // new 16.05.05 F.G.
|
|---|
| 227 | // SetAccDec(fCosy->fMac1, fAcc, fDec);
|
|---|
| 228 |
|
|---|
| 229 | SetAccDec(fCosy->fMac2, fAcc, fDec);
|
|---|
| 230 |
|
|---|
| 231 | SetPosVelocity(fabs(rd.Ratio()), fVel);
|
|---|
| 232 | }
|
|---|
| 233 |
|
|---|
| 234 | rd.Round();
|
|---|
| 235 |
|
|---|
| 236 | // FIXME? Check for Error or Zombie?
|
|---|
| 237 |
|
|---|
| 238 | // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
|
|---|
| 239 | // cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
|
|---|
| 240 | // cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
|
|---|
| 241 | // cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
|
|---|
| 242 | // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
|
|---|
| 243 |
|
|---|
| 244 | // repositioning (relative)
|
|---|
| 245 | lout << "- Do Relative Positioning..." << endl;
|
|---|
| 246 | DoRelPos(rd, cdzd, cdaz);
|
|---|
| 247 | lout << "- Relative Positioning Done" << endl;
|
|---|
| 248 | }
|
|---|
| 249 | if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/)
|
|---|
| 250 | {
|
|---|
| 251 | t.Now();
|
|---|
| 252 | lout << t << " - Positioning done." << endl;
|
|---|
| 253 | fCosy->SetStatus(MDriveCom::kStopped);
|
|---|
| 254 | fCosy->fCom->SendStatus("Tracking preposition reached.");
|
|---|
| 255 | return TRUE;
|
|---|
| 256 | }
|
|---|
| 257 |
|
|---|
| 258 | if (i<10)
|
|---|
| 259 | fCosy->StopMovement();
|
|---|
| 260 | else
|
|---|
| 261 | fCosy->SetStatus(MDriveCom::kStopped);
|
|---|
| 262 |
|
|---|
| 263 | t.Now();
|
|---|
| 264 | lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl;
|
|---|
| 265 |
|
|---|
| 266 | fCosy->fCom->SendStatus("Target position missed!");
|
|---|
| 267 |
|
|---|
| 268 | return FALSE;
|
|---|
| 269 | }
|
|---|
