| 1 | #include "MPointing.h"
|
|---|
| 2 |
|
|---|
| 3 | #include "MCosy.h"
|
|---|
| 4 | #include "macs.h"
|
|---|
| 5 | #include "MDriveCom.h"
|
|---|
| 6 |
|
|---|
| 7 | ClassImp(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 | //
|
|---|
| 22 | void 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 | //
|
|---|
| 51 | void 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 |
|
|---|
| 72 | bool MPointing::SetAccDec(Macs *mac, Float_t acc, Float_t dec)
|
|---|
| 73 | {
|
|---|
| 74 | const int vr = mac->GetVelRes();
|
|---|
| 75 | mac->SetAcceleration(acc*vr);
|
|---|
| 76 | mac->SetDeceleration(dec*vr);
|
|---|
| 77 | return !mac->IsZombieNode();
|
|---|
| 78 | }
|
|---|
| 79 |
|
|---|
| 80 | bool 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 | //
|
|---|
| 96 | int MPointing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
|
|---|
| 97 | {
|
|---|
| 98 | const ZdAz d = dst*kRad2Deg;
|
|---|
| 99 |
|
|---|
| 100 | MTime t(-1);
|
|---|
| 101 | lout << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
|
|---|
| 102 |
|
|---|
| 103 | //
|
|---|
| 104 | // Calculate new target position (shortest distance to go)
|
|---|
| 105 | //
|
|---|
| 106 | //const ZdAz src = fCosy->GetSePos(); // [se]
|
|---|
| 107 |
|
|---|
| 108 | //
|
|---|
| 109 | // Make sure that the motors are in sync mode (necessary if the
|
|---|
| 110 | // MACS has been rebooted from a Zombie state.
|
|---|
| 111 | //
|
|---|
| 112 | //InitSync();
|
|---|
| 113 | //if (fMac3->IsZombieNode())
|
|---|
| 114 | // return false;
|
|---|
| 115 |
|
|---|
| 116 | //
|
|---|
| 117 | // Because we agreed on I don't search for the shortest move
|
|---|
| 118 | // anymore
|
|---|
| 119 | //
|
|---|
| 120 | // const ZdAz dest = CorrectTarget(src, dst);
|
|---|
| 121 | //
|
|---|
| 122 | ZdAz bend = fCosy->fBending(dst); // [rad]
|
|---|
| 123 |
|
|---|
| 124 | const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se]
|
|---|
| 125 |
|
|---|
| 126 | if (!fCosy->CheckRange(bend))
|
|---|
| 127 | return kFALSE;
|
|---|
| 128 |
|
|---|
| 129 | bend *= kRad2Deg;
|
|---|
| 130 | fCosy->fZdAzSoll = dst;
|
|---|
| 131 |
|
|---|
| 132 | //cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl;
|
|---|
| 133 | //cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl;
|
|---|
| 134 | //cout << "Bend'd Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl;
|
|---|
| 135 | //cout << "Bend'd Dest Zd: " << bend.Zd() << "deg Az:" << bend.Az() << "deg" << endl;
|
|---|
| 136 |
|
|---|
| 137 | //
|
|---|
| 138 | // Set velocities
|
|---|
| 139 | //
|
|---|
| 140 | //const int vr = fCosy->fMac1->GetVelRes();
|
|---|
| 141 |
|
|---|
| 142 | const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi();
|
|---|
| 143 |
|
|---|
| 144 | int i;
|
|---|
| 145 | for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++)
|
|---|
| 146 | {
|
|---|
| 147 |
|
|---|
| 148 | lout << "- Step #" << i << endl;
|
|---|
| 149 |
|
|---|
| 150 | // Get Shaft Encoder Positions
|
|---|
| 151 | const ZdAz p=fCosy->GetSePos();
|
|---|
| 152 |
|
|---|
| 153 | // calculate control deviation
|
|---|
| 154 | ZdAz rd = dest-p; // [se]
|
|---|
| 155 | ZdAz cd = rd; // [se]
|
|---|
| 156 | // Correct for having two SE available
|
|---|
| 157 | // FIMXE....
|
|---|
| 158 | // cd.Zd(cd.Zd()*2);
|
|---|
| 159 | // Round to check whether we are as near as possible
|
|---|
| 160 | // to the value we expect
|
|---|
| 161 | cd.Round();
|
|---|
| 162 |
|
|---|
| 163 | // Check if there is a control deviation on the axis
|
|---|
| 164 | const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
|
|---|
| 165 | const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
|
|---|
| 166 |
|
|---|
| 167 | // check if we reached the correct position already
|
|---|
| 168 | if (!cdzd && !cdaz)
|
|---|
| 169 | {
|
|---|
| 170 | t.Now();
|
|---|
| 171 | lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
|
|---|
| 172 | fCosy->SetStatus(MDriveCom::kStopped);
|
|---|
| 173 | fCosy->fCom->SendStatus("Target position reached.");
|
|---|
| 174 | return TRUE;
|
|---|
| 175 | }
|
|---|
| 176 |
|
|---|
| 177 | // ==============================================
|
|---|
| 178 | // Estimate the noncircularity of the zd axis
|
|---|
| 179 | const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE;
|
|---|
| 180 |
|
|---|
| 181 | const double p1 = ist.Zd()-19.0605/kRad2Deg;
|
|---|
| 182 | const double p2 = dst.Zd()-19.0605/kRad2Deg;
|
|---|
| 183 |
|
|---|
| 184 | const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*rad2se;
|
|---|
| 185 | const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se;
|
|---|
| 186 | // ==++=========================================
|
|---|
| 187 |
|
|---|
| 188 | // change units from se to re
|
|---|
| 189 | rd *= fCosy->kGearTot/fCosy->kResSE; // [re]
|
|---|
| 190 | rd.Zd(f2-f1);
|
|---|
| 191 |
|
|---|
| 192 | // Initialize Velocities so that we reach both positions
|
|---|
| 193 | // at the same time
|
|---|
| 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->kGearTot.Y()/fCosy->kResSE.Y();
|
|---|
| 205 |
|
|---|
| 206 | // lout << "MPointing::SetPosition y: " << y << " rd.Az(): " << rd.Az() << endl;
|
|---|
| 207 | // lout << "MPointing::SetPosition fCosy->kGearTot.Y(): " << fCosy->kGearTot.Y() << " fCosy->kResSE.Y(): " << fCosy->kResSE.Y() << endl;
|
|---|
| 208 |
|
|---|
| 209 | if (rd.Az()>-y && rd.Az()<y)
|
|---|
| 210 | {
|
|---|
| 211 | //lout << "--- LO-SPEED Mac1 ---" << endl;
|
|---|
| 212 | SetAccDec(fCosy->fMac1, 0.05, 0.05);
|
|---|
| 213 | }
|
|---|
| 214 | else
|
|---|
| 215 | {
|
|---|
| 216 |
|
|---|
| 217 | //lout << "MPointing::SetPosition SetAccDec Mac1: " << fAcc << fDec << endl;
|
|---|
| 218 |
|
|---|
| 219 | SetAccDec(fCosy->fMac1, fAcc, fDec);
|
|---|
| 220 |
|
|---|
| 221 | }
|
|---|
| 222 |
|
|---|
| 223 |
|
|---|
| 224 | // new 16.05.05 F.G.
|
|---|
| 225 | // SetAccDec(fCosy->fMac1, fAcc, fDec);
|
|---|
| 226 |
|
|---|
| 227 | SetAccDec(fCosy->fMac2, fAcc, fDec);
|
|---|
| 228 |
|
|---|
| 229 | SetPosVelocity(fabs(rd.Ratio()), fVel);
|
|---|
| 230 | }
|
|---|
| 231 |
|
|---|
| 232 | rd.Round();
|
|---|
| 233 |
|
|---|
| 234 | // FIXME? Check for Error or Zombie?
|
|---|
| 235 |
|
|---|
| 236 | // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
|
|---|
| 237 | // cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
|
|---|
| 238 | // cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
|
|---|
| 239 | // cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
|
|---|
| 240 | // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
|
|---|
| 241 |
|
|---|
| 242 | // repositioning (relative)
|
|---|
| 243 | lout << "- Do Relative Positioning..." << endl;
|
|---|
| 244 | DoRelPos(rd, cdzd, cdaz);
|
|---|
| 245 | lout << "- Relative Positioning Done" << endl;
|
|---|
| 246 | }
|
|---|
| 247 | if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/)
|
|---|
| 248 | {
|
|---|
| 249 | t.Now();
|
|---|
| 250 | lout << t << " - Positioning done." << endl;
|
|---|
| 251 | fCosy->SetStatus(MDriveCom::kStopped);
|
|---|
| 252 | fCosy->fCom->SendStatus("Tracking preposition reached.");
|
|---|
| 253 | return TRUE;
|
|---|
| 254 | }
|
|---|
| 255 |
|
|---|
| 256 | if (i<10)
|
|---|
| 257 | fCosy->StopMovement();
|
|---|
| 258 | else
|
|---|
| 259 | fCosy->SetStatus(MDriveCom::kStopped);
|
|---|
| 260 |
|
|---|
| 261 | t.Now();
|
|---|
| 262 | lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl;
|
|---|
| 263 |
|
|---|
| 264 | fCosy->fCom->SendStatus("Target position missed!");
|
|---|
| 265 |
|
|---|
| 266 | return FALSE;
|
|---|
| 267 | }
|
|---|
