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