Index: trunk/MagicSoft/Cosy/main/MPointing.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MPointing.cc (revision 8814) +++ (revision ) @@ -1,271 +1,0 @@ -#include "MSlewing.h" - -#include "MCosy.h" -#include "macs.h" -#include "MDriveCom.h" - -#include "MPointing.h" - -ClassImp(MSlewing); - -using namespace std; - -//#define EXPERT -#undef EXPERT - -// -------------------------------------------------------------------------- -// -// set the velocity and accelerations for position maneuvers. -// -// The acceleratin is set as given (in percent of maximum). -// The velocity is given in percent, depending on the ratio (<1 or >1) -// one of the axis becomes a slower velocity. This is used for maneuvers -// in which both axis are moved synchromously and should reach their -// target position at the same time. -// -void MSlewing::SetPosVelocity(const Float_t ratio, Float_t vel) -{ - // - // Set velocities - // - const int vr = fCosy->fMac1->GetVelRes(); - vel *= vr; - - if (ratio<1) - { - fCosy->fMac1->SetVelocity(TMath::Nint(vel)); - fCosy->fMac2->SetVelocity(TMath::Nint(vel*ratio)); - } - else - { - fCosy->fMac1->SetVelocity(TMath::Nint(vel/ratio)); - fCosy->fMac2->SetVelocity(TMath::Nint(vel)); - } -} - -// -------------------------------------------------------------------------- -// -// Does a relative positioning. -// -// The steps to move are given in a ZdAz object relative to the current -// position. The coordinates are given in Roteryencoder steps. -// Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only -// if Axis 2==kTRUE. The function waits for the movement to be finished. -// -void MSlewing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) -{ - if (fCosy->HasZombie()) - return; - - fCosy->SetStatus(MDriveCom::kMoving); - - if (axe1) fCosy->fMac2->StartRelPos(TMath::Nint(rd.Zd())); - if (axe2) fCosy->fMac1->StartRelPos(TMath::Nint(rd.Az())); -#ifdef EXPERT - cout << "Waiting for positioning..." << flush; -#endif - if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1); - if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1); - - fCosy->WaitForEndMovement(); -#ifdef EXPERT - cout << "done." << endl; -#endif -} - -bool MSlewing::SetAccDec(Macs *mac, Float_t acc, Float_t dec) -{ - const int vr = mac->GetVelRes(); - mac->SetAcceleration(TMath::Nint(acc*vr)); - mac->SetDeceleration(TMath::Nint(dec*vr)); - return !mac->IsZombieNode(); -} - -bool MSlewing::Break() -{ - return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie(); -} - -// -------------------------------------------------------------------------- -// -// Move the telescope to the given position. The position must be given in -// a ZdAz object in rad. -// -// The first positioning is done absolutely. If we didn't reach the -// correct psotion we try to correct for this by 10 relative position -// maneuvers. If this doesn't help positioning failed. -// -// As a reference the shaftencoder values are used. -// -int MSlewing::SetPosition(const ZdAz &dst, Bool_t track) // [rad] -{ - const ZdAz d = dst*kRad2Deg; - - MTime t(-1); - gLog << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl; - - // - // Calculate new target position (shortest distance to go) - // - //const ZdAz src = fCosy->GetSePos(); // [se] - - // - // Make sure that the motors are in sync mode (necessary if the - // MACS has been rebooted from a Zombie state. - // - //InitSync(); - //if (fMac3->IsZombieNode()) - // return false; - - // - // Because we agreed on I don't search for the shortest move - // anymore - // - // const ZdAz dest = CorrectTarget(src, dst); - // - ZdAz bend = fCosy->fBending(dst); // [rad] - - const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se] - - if (!fCosy->CheckRange(bend)) - return kFALSE; - - bend *= kRad2Deg; - fCosy->fZdAzSoll = dst; - - //cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl; - //cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl; - //cout << "Bend'd Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; - //cout << "Bend'd Dest Zd: " << bend.Zd() << "deg Az:" << bend.Az() << "deg" << endl; - - // - // Set velocities - // - //const int vr = fCosy->fMac1->GetVelRes(); - - const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi(); - - int i; - for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++) - { - - gLog << "- Step #" << i << endl; - - // Get Shaft Encoder Positions - const ZdAz p=fCosy->GetSePos(); - - // calculate control deviation - ZdAz rd = dest-p; // [se] - ZdAz cd = rd; // [se] - // Correct for having two SE available -// FIMXE.... -// cd.Zd(cd.Zd()*2); - // Round to check whether we are as near as possible - // to the value we expect - cd.Round(); - - // Check if there is a control deviation on the axis - const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE; - const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE; - - // check if we reached the correct position already - if (!cdzd && !cdaz) - { - t.Now(); - gLog << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl; - fCosy->SetStatus(MDriveCom::kStopped); - fCosy->fCom->SendStatus("Target position reached."); - return TRUE; - } - - // ============================================== - // Estimate the noncircularity of the zd axis - const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE; - - const double p1 = ist.Zd()-19.0605/kRad2Deg; - const double p2 = dst.Zd()-19.0605/kRad2Deg; - - const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*rad2se; - const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se; - // ==++========================================= - - // change units from se to re - rd *= fCosy->kGearTot/fCosy->kResSE; // [re] - rd.Zd(f2-f1); - - // Initialize Velocities so that we reach both positions - // at the same time - if (i) - { - //lout << "--- LO-SPEED ---" << endl; - SetAccDec(fCosy->fMac1, 0.1, 0.1); - SetAccDec(fCosy->fMac2, 0.1, 0.1); - - SetPosVelocity(1.0, 0.05); - } - else - { - const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y(); - -// lout << "MSlewing::SetPosition y: " << y << " rd.Az(): " << rd.Az() << endl; -// lout << "MSlewing::SetPosition fCosy->kGearTot.Y(): " << fCosy->kGearTot.Y() << " fCosy->kResSE.Y(): " << fCosy->kResSE.Y() << endl; - - if (rd.Az()>-y && rd.Az()fMac1, 0.05, 0.05); - } - else - { - - //lout << "MSlewing::SetPosition SetAccDec Mac1: " << fAcc << fDec << endl; - - SetAccDec(fCosy->fMac1, fAcc, fDec); - - } - - - // new 16.05.05 F.G. - // SetAccDec(fCosy->fMac1, fAcc, fDec); - - SetAccDec(fCosy->fMac2, fAcc, fDec); - - SetPosVelocity(fabs(rd.Ratio()), fVel); - } - - rd.Round(); - - // FIXME? Check for Error or Zombie? - - // cout << " + " << (int)cdzd << " " << (int)cdaz << endl; - // cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl; - // cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl; - // cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl; - // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl; - - // repositioning (relative) - gLog << "- Do Relative Positioning..." << endl; - DoRelPos(rd, cdzd, cdaz); - gLog << "- Relative Positioning Done" << endl; - } - if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/) - { - t.Now(); - gLog << t << " - Positioning done." << endl; - fCosy->SetStatus(MDriveCom::kStopped); - fCosy->fCom->SendStatus("Tracking preposition reached."); - return TRUE; - } - - if (i<10) - fCosy->StopMovement(); - else - fCosy->SetStatus(MDriveCom::kStopped); - - t.Now(); - gLog << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl; - - fCosy->fCom->SendStatus("Target position missed!"); - - return FALSE; -} Index: trunk/MagicSoft/Cosy/main/MPointing.h =================================================================== --- trunk/MagicSoft/Cosy/main/MPointing.h (revision 8814) +++ (revision ) @@ -1,38 +1,0 @@ -#ifndef COSY_MSlewing -#define COSY_MSlewing - -#ifndef MARS_MLog -#include "MLog.h" -#endif - -class Macs; -class MCosy; -class ZdAz; - -class MSlewing : public MLog -{ -protected: - MCosy *fCosy; - - bool SetAccDec(Macs *mac, Float_t acc, Float_t dec); - bool Break(); - -private: - Float_t fVel; - Float_t fAcc; - Float_t fDec; - - void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2); - void SetPosVelocity(const Float_t ratio, Float_t vel); - -public: - MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1) { } - - void SetPointAccDec(Float_t acc, Float_t dec) { fAcc = acc; fDec = dec; } - void SetPointVelocity(Float_t vel) { fVel = vel; } - int SetPosition(const ZdAz &dst, Bool_t track=kFALSE); - - ClassDef(MSlewing, 0) -}; - -#endif Index: trunk/MagicSoft/Cosy/main/MSlewing.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MSlewing.cc (revision 8815) +++ trunk/MagicSoft/Cosy/main/MSlewing.cc (revision 8815) @@ -0,0 +1,271 @@ +#include "MSlewing.h" + +#include "MCosy.h" +#include "macs.h" +#include "MDriveCom.h" + +#include "MPointing.h" + +ClassImp(MSlewing); + +using namespace std; + +//#define EXPERT +#undef EXPERT + +// -------------------------------------------------------------------------- +// +// set the velocity and accelerations for position maneuvers. +// +// The acceleratin is set as given (in percent of maximum). +// The velocity is given in percent, depending on the ratio (<1 or >1) +// one of the axis becomes a slower velocity. This is used for maneuvers +// in which both axis are moved synchromously and should reach their +// target position at the same time. +// +void MSlewing::SetPosVelocity(const Float_t ratio, Float_t vel) +{ + // + // Set velocities + // + const int vr = fCosy->fMac1->GetVelRes(); + vel *= vr; + + if (ratio<1) + { + fCosy->fMac1->SetVelocity(TMath::Nint(vel)); + fCosy->fMac2->SetVelocity(TMath::Nint(vel*ratio)); + } + else + { + fCosy->fMac1->SetVelocity(TMath::Nint(vel/ratio)); + fCosy->fMac2->SetVelocity(TMath::Nint(vel)); + } +} + +// -------------------------------------------------------------------------- +// +// Does a relative positioning. +// +// The steps to move are given in a ZdAz object relative to the current +// position. The coordinates are given in Roteryencoder steps. +// Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only +// if Axis 2==kTRUE. The function waits for the movement to be finished. +// +void MSlewing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) +{ + if (fCosy->HasZombie()) + return; + + fCosy->SetStatus(MDriveCom::kMoving); + + if (axe1) fCosy->fMac2->StartRelPos(TMath::Nint(rd.Zd())); + if (axe2) fCosy->fMac1->StartRelPos(TMath::Nint(rd.Az())); +#ifdef EXPERT + cout << "Waiting for positioning..." << flush; +#endif + if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1); + if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1); + + fCosy->WaitForEndMovement(); +#ifdef EXPERT + cout << "done." << endl; +#endif +} + +bool MSlewing::SetAccDec(Macs *mac, Float_t acc, Float_t dec) +{ + const int vr = mac->GetVelRes(); + mac->SetAcceleration(TMath::Nint(acc*vr)); + mac->SetDeceleration(TMath::Nint(dec*vr)); + return !mac->IsZombieNode(); +} + +bool MSlewing::Break() +{ + return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie(); +} + +// -------------------------------------------------------------------------- +// +// Move the telescope to the given position. The position must be given in +// a ZdAz object in rad. +// +// The first positioning is done absolutely. If we didn't reach the +// correct psotion we try to correct for this by 10 relative position +// maneuvers. If this doesn't help positioning failed. +// +// As a reference the shaftencoder values are used. +// +int MSlewing::SetPosition(const ZdAz &dst, Bool_t track) // [rad] +{ + const ZdAz d = dst*kRad2Deg; + + MTime t(-1); + gLog << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl; + + // + // Calculate new target position (shortest distance to go) + // + //const ZdAz src = fCosy->GetSePos(); // [se] + + // + // Make sure that the motors are in sync mode (necessary if the + // MACS has been rebooted from a Zombie state. + // + //InitSync(); + //if (fMac3->IsZombieNode()) + // return false; + + // + // Because we agreed on I don't search for the shortest move + // anymore + // + // const ZdAz dest = CorrectTarget(src, dst); + // + ZdAz bend = fCosy->fBending(dst); // [rad] + + const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se] + + if (!fCosy->CheckRange(bend)) + return kFALSE; + + bend *= kRad2Deg; + fCosy->fZdAzSoll = dst; + + //cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl; + //cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl; + //cout << "Bend'd Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; + //cout << "Bend'd Dest Zd: " << bend.Zd() << "deg Az:" << bend.Az() << "deg" << endl; + + // + // Set velocities + // + //const int vr = fCosy->fMac1->GetVelRes(); + + const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi(); + + int i; + for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++) + { + + gLog << "- Step #" << i << endl; + + // Get Shaft Encoder Positions + const ZdAz p=fCosy->GetSePos(); + + // calculate control deviation + ZdAz rd = dest-p; // [se] + ZdAz cd = rd; // [se] + // Correct for having two SE available +// FIMXE.... +// cd.Zd(cd.Zd()*2); + // Round to check whether we are as near as possible + // to the value we expect + cd.Round(); + + // Check if there is a control deviation on the axis + const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE; + const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE; + + // check if we reached the correct position already + if (!cdzd && !cdaz) + { + t.Now(); + gLog << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl; + fCosy->SetStatus(MDriveCom::kStopped); + fCosy->fCom->SendStatus("Target position reached."); + return TRUE; + } + + // ============================================== + // Estimate the noncircularity of the zd axis + const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE; + + const double p1 = ist.Zd()-19.0605/kRad2Deg; + const double p2 = dst.Zd()-19.0605/kRad2Deg; + + const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*rad2se; + const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se; + // ==++========================================= + + // change units from se to re + rd *= fCosy->kGearTot/fCosy->kResSE; // [re] + rd.Zd(f2-f1); + + // Initialize Velocities so that we reach both positions + // at the same time + if (i) + { + //lout << "--- LO-SPEED ---" << endl; + SetAccDec(fCosy->fMac1, 0.1, 0.1); + SetAccDec(fCosy->fMac2, 0.1, 0.1); + + SetPosVelocity(1.0, 0.05); + } + else + { + const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y(); + +// lout << "MSlewing::SetPosition y: " << y << " rd.Az(): " << rd.Az() << endl; +// lout << "MSlewing::SetPosition fCosy->kGearTot.Y(): " << fCosy->kGearTot.Y() << " fCosy->kResSE.Y(): " << fCosy->kResSE.Y() << endl; + + if (rd.Az()>-y && rd.Az()fMac1, 0.05, 0.05); + } + else + { + + //lout << "MSlewing::SetPosition SetAccDec Mac1: " << fAcc << fDec << endl; + + SetAccDec(fCosy->fMac1, fAcc, fDec); + + } + + + // new 16.05.05 F.G. + // SetAccDec(fCosy->fMac1, fAcc, fDec); + + SetAccDec(fCosy->fMac2, fAcc, fDec); + + SetPosVelocity(fabs(rd.Ratio()), fVel); + } + + rd.Round(); + + // FIXME? Check for Error or Zombie? + + // cout << " + " << (int)cdzd << " " << (int)cdaz << endl; + // cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl; + // cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl; + // cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl; + // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl; + + // repositioning (relative) + gLog << "- Do Relative Positioning..." << endl; + DoRelPos(rd, cdzd, cdaz); + gLog << "- Relative Positioning Done" << endl; + } + if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/) + { + t.Now(); + gLog << t << " - Positioning done." << endl; + fCosy->SetStatus(MDriveCom::kStopped); + fCosy->fCom->SendStatus("Tracking preposition reached."); + return TRUE; + } + + if (i<10) + fCosy->StopMovement(); + else + fCosy->SetStatus(MDriveCom::kStopped); + + t.Now(); + gLog << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl; + + fCosy->fCom->SendStatus("Target position missed!"); + + return FALSE; +} Index: trunk/MagicSoft/Cosy/main/MSlewing.h =================================================================== --- trunk/MagicSoft/Cosy/main/MSlewing.h (revision 8815) +++ trunk/MagicSoft/Cosy/main/MSlewing.h (revision 8815) @@ -0,0 +1,38 @@ +#ifndef COSY_MSlewing +#define COSY_MSlewing + +#ifndef MARS_MLog +#include "MLog.h" +#endif + +class Macs; +class MCosy; +class ZdAz; + +class MSlewing : public MLog +{ +protected: + MCosy *fCosy; + + bool SetAccDec(Macs *mac, Float_t acc, Float_t dec); + bool Break(); + +private: + Float_t fVel; + Float_t fAcc; + Float_t fDec; + + void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2); + void SetPosVelocity(const Float_t ratio, Float_t vel); + +public: + MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1) { } + + void SetPointAccDec(Float_t acc, Float_t dec) { fAcc = acc; fDec = dec; } + void SetPointVelocity(Float_t vel) { fVel = vel; } + int SetPosition(const ZdAz &dst, Bool_t track=kFALSE); + + ClassDef(MSlewing, 0) +}; + +#endif