Index: trunk/MagicSoft/Cosy/main/MPointing.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MPointing.cc (revision 3935) +++ trunk/MagicSoft/Cosy/main/MPointing.cc (revision 3935) @@ -0,0 +1,446 @@ +#include "MPointing.h" + +#include "MCosy.h" +#include "macs.h" +#include "MDriveCom.h" + +ClassImp(MPointing); + +//#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 MPointing::SetPosVelocity(const Float_t ratio, Float_t vel) +{ + // + // Set velocities + // + const int vr = fCosy->fMac1->GetVelRes(); + vel *= vr; + + if (ratio<1) + { + fCosy->fMac1->SetVelocity(vel); + fCosy->fMac2->SetVelocity(vel*ratio); + } + else + { + fCosy->fMac1->SetVelocity(vel/ratio); + fCosy->fMac2->SetVelocity(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 MPointing::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(rd.Zd()); + if (axe2) fCosy->fMac1->StartRelPos(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 MPointing::SetAccDec(Macs *mac, Float_t acc, Float_t dec) +{ + const int vr = mac->GetVelRes(); + mac->SetAcceleration(acc*vr); + mac->SetAcceleration(dec*vr); + return !mac->IsZombieNode(); +} + +bool MPointing::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 MPointing::SetPosition(const ZdAz &dst, Bool_t track) // [rad] +{ + /* + const ZdAz d = dst*kRad2Deg; + + MTime t(-1); + lout << 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*16384/2/TMath::Pi(); // [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: " << dst.Zd()*8192/TMath::Pi() << "se Az:" << dst.Az()*8192/TMath::Pi() << "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; + + int i; + for (i=0; i<(track?1:10) && !Break(); i++) + { + + lout << "- Step #" << i << endl; + // + // Get Shaft Encoder Positions + // + const ZdAz p=fCosy->GetSePos(); + + // + // calculate control deviation and rounded cd + // + ZdAz rd = dest-p; // [se] + + // =========================================== + const ZdAz ist = dst-rd*TMath::Pi()/8192; + + 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())*8192/TMath::Pi(); + const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi(); + // =========================================== + + ZdAz cd = rd; // [se] + 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(); + lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl; + fCosy->SetStatus(MDriveCom::kStopped); + fCosy->fCom->Send("POSITION DONE"); + return TRUE; + } + + // + // change units from se to re + // + rd *= fCosy->kGearRatio; // [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->kGearRatio.Y(); + if (rd.Az()>-y && rd.Az()fMac1, 0.05, 0.05); + } + else + 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) + // + + lout << "- Do Relative Positioning..." << endl; + DoRelPos(rd, cdzd, cdaz); + lout << "- Relative Positioning Done" << endl; + } + + + return FALSE;*/ + + const ZdAz d = dst*kRad2Deg; + + MTime t(-1); + lout << 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*16384/2/TMath::Pi(); // [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(); + + int i; + for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++) + { + + lout << "- Step #" << i << endl; + // + // Get Shaft Encoder Positions + // + const ZdAz p=fCosy->GetSePos(); + + // + // calculate control deviation and rounded cd + // + ZdAz rd = dest-p; // [se] + + // =========================================== + const ZdAz ist = dst-rd*TMath::Pi()/8192; + + 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())*8192/TMath::Pi(); + const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi(); + // =========================================== + + ZdAz cd = rd; // [se] + 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(); + lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl; + fCosy->SetStatus(MDriveCom::kStopped); + fCosy->fCom->Send("POSITION DONE"); + return TRUE; + } + + // + // change units from se to re + // + rd *= fCosy->kGearRatio; // [re] + rd.Zd(f2-f1); + + // + // Initialize Velocities so that we reach both positions + // at the same time + // +/* if (i) + { + fCosy->fMac1->SetAcceleration(0.1*vr); + fCosy->fMac2->SetAcceleration(0.1*vr); + + fCosy->fMac1->SetDeceleration(0.1*vr); + fCosy->fMac2->SetDeceleration(0.1*vr); + + fCosy->SetPosVelocity(1.0, 0.05); + } + else + { + if (rd.Az()>-15*fCosy->kGearRatio.Y() && rd.Az()<15*fCosy->kGearRatio.Y()) + { +#ifdef EXPERT + cout << " -------------- LO ---------------- " << endl; +#endif + fCosy->fMac1->SetAcceleration(0.05*vr); + fCosy->fMac1->SetDeceleration(0.05*vr); + } + else + { +#ifdef EXPERT + cout << " -------------- HI ---------------- " << endl; + fCosy->fMac1->SetAcceleration(0.4*vr);// 0.4 + fCosy->fMac1->SetDeceleration(0.4*vr);// 0.4 +#else + fCosy->fMac1->SetAcceleration(0.2*vr); + fCosy->fMac1->SetDeceleration(0.1*vr); +#endif + } + +#ifdef EXPERT + fCosy->fMac2->SetAcceleration(0.4*vr);// 0.4 + fCosy->fMac2->SetDeceleration(0.4*vr);// 0.4 + fCosy->SetPosVelocity(fabs(rd.Ratio()), 0.2); // fast: 0.6, slow: 0.2 +#else + fCosy->fMac2->SetAcceleration(0.2*vr); + fCosy->fMac2->SetDeceleration(0.1*vr); + fCosy->SetPosVelocity(fabs(rd.Ratio()), 0.1); +#endif + } + */ + 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->kGearRatio.Y(); + if (rd.Az()>-y && rd.Az()fMac1, 0.05, 0.05); + } + else + 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) + // + lout << "- Do Relative Positioning..." << endl; + DoRelPos(rd, cdzd, cdaz); + lout << "- Relative Positioning Done" << endl; + } + if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/) + { + t.Now(); + lout << t << " - Positioning done." << endl; + fCosy->SetStatus(MDriveCom::kStopped); + fCosy->fCom->Send("POSITION DONE"); + return TRUE; + } + + if (i<10) + fCosy->StopMovement(); + else + fCosy->SetStatus(MDriveCom::kStopped); + + t.Now(); + lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl; + + fCosy->fCom->Send("POSITION FAILED"); + + return FALSE; +} Index: trunk/MagicSoft/Cosy/main/MPointing.h =================================================================== --- trunk/MagicSoft/Cosy/main/MPointing.h (revision 3935) +++ trunk/MagicSoft/Cosy/main/MPointing.h (revision 3935) @@ -0,0 +1,40 @@ +#ifndef COSY_MPointing +#define COSY_MPointing + +#include "coord.h" +#include "log.h" + +#ifndef ROOT_TObject +#include +#endif + +class Macs; +class MCosy; + +class MPointing : public Log, public TObject +{ +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: + MPointing(MCosy *cosy, const Log &log) : Log(log), 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(MPointing, 0) +}; + +#endif Index: trunk/MagicSoft/Cosy/main/MTracking.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MTracking.cc (revision 3935) +++ trunk/MagicSoft/Cosy/main/MTracking.cc (revision 3935) @@ -0,0 +1,521 @@ +#include "MTracking.h" + +#include "macs.h" +#include "shaftencoder.h" + +#include "MCosy.h" +#include "SlaStars.h" + +#include "MDriveCom.h" + +ClassImp(MTracking); + +//#define EXPERT +#undef EXPERT + +// -------------------------------------------------------------------------- +// +// Initializes Tracking mode +// +// Initializes the accelerations of both axes with 90% of the maximum +// acceleration. Set the status for moving and tracking and starts thr +// revolution mode. +// +bool MTracking::InitTracking() +{ + // FIXME? Handling of Zombie OK? + if (fCosy->fMac1->IsZombieNode() || fCosy->fMac2->IsZombieNode()) + return false; + + // + // Start revolution mode + // + if (!SetAccDec(fCosy->fMac2, fTrackAcc, fTrackDec)) + return false; + + if (!SetAccDec(fCosy->fMac1, fTrackAcc, fTrackDec)) + return false; + + fCosy->SetStatus(MDriveCom::kMoving | MDriveCom::kTracking); + + fCosy->fMac2->SetRpmMode(TRUE); + if (fCosy->fMac2->IsZombieNode()) + return false; + + fCosy->fMac1->SetRpmMode(TRUE); + if (fCosy->fMac1->IsZombieNode()) + return false; + + return true; +} + +// -------------------------------------------------------------------------- +// +// Limits the speed. +// +// This function should work as a limiter. If a tracking error is too large +// to be corrected fast enough we would get enormous velocities. These +// velocities are limited to the maximum velocity. +// +Bool_t MTracking::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const +{ + Bool_t rc = kFALSE; + + // + // How to limit the speed. If the wind comes and blowes + // we cannot forbid changing of the sign. But on the other hand + // we don't want fast changes! + // + ULong_t vrzd = fCosy->fMac1->GetVelRes(); + ULong_t vraz = fCosy->fMac2->GetVelRes(); + +#define sgn(x) (x<0?-1:1) + + // + // When speed changes sign, the maximum allowed speed + // is 25% of the |v| + // + //const Float_t limit = 0.25; + + // + // The maximum allowed speed while tracking is 10% + // + const Float_t maxtrack = 0.1; + + if (fabs(vt->Az()) > maxtrack*vraz) + { + lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Az()) << " > " << maxtrack*vraz << ")... limited." << endl; + vt->Az(maxtrack*vraz*sgn(vcalc.Az())); + rc=kTRUE; + } + if (fabs(vt->Zd()) > maxtrack*vrzd) + { + lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Zd()) <<" > " << maxtrack*vrzd << ")... limited." << endl; + vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd())); + rc=kTRUE; + } + return rc; +} + +// -------------------------------------------------------------------------- +// +// Sets the tracking velocity +// +// The velocities are given in a ZdAz object in re/min. Return kTRUE +// in case of success, kFALSE in case of failure. +// +Bool_t MTracking::SetVelocity(const ZdAz &v) +{ + // + // Send the new velocities for both axes. + // + fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min] + fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min] + + // + // Wait for the objects to be acknoledged. + // + fCosy->fMac2->WaitForSdo(0x3006, 1); + fCosy->fMac1->WaitForSdo(0x3006, 1); + + // + // If the waiting for the objects wasn't interrupted return kTRUE + // + if (!Break()) + return kTRUE; + + // + // print a message if the interruption was due to a Can-node Error + // + if (fCosy->HasError()) + lout << "Error while setting tracking velocity (SDO #3006)" << endl; + + return kFALSE; +} + +void MTracking::TrackPosition(const RaDec &dst) // ra, dec [rad] +{ + SlaStars sla(fCosy->fObservatory); + + // + // Position to actual position + // + sla.Now(); + ZdAz dest = sla.CalcZdAz(dst); + + lout << sla.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl; + + // az between -180 and 180 + if (dst.Dec()>sla.GetPhi() && dest.Az()<0) + { + // align az between (roughly) 60 and 320 + lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl; + dest.Az(dest.Az() + TMath::Pi()*2); + } +/* + // FIXME: Determin tracking start point by star culmination + if (dest.Az()<-TMath::Pi()/2) + { + lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl; + dest.Az(dest.Az() + TMath::Pi()*2); + } + + if (dest.Az()>3*TMath::Pi()/2) + { + lout << "Substracting 360deg to Azimuth " << dest.Az()*kRad2Deg << endl; + dest.Az(dest.Az() -TMath::Pi()*2); + } + */ + if (!SetPosition(dest, kTRUE)) + //if (!SetPosition(dest, kFALSE)) + { + lout << "Error: Cannot start tracking, positioning failed." << endl; + return; + } + + // + // calculate offset from present se position + // + const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearRatio; + + if (!fCosy->RequestRePos()) + return; + + // + // Estimate Offset before starting to track + // + fCosy->fOffset = sepos-fCosy->GetRePos(); + + /* + cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl; + cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl; + cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl; + */ + + // + // Init accelerations and Rpm Mode + // + if (!InitTracking()) + { + fCosy->StopMovement(); + return; + } + + XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec())); + + sla.Now(); +// lout << sla.GetTime() << " - Start tracking:"; +// lout << " Ra: " << xy.X() << "h " << "Dec: " << xy.Y() << "\xb0" << endl; + +/*#ifdef EXPERT + ofstream fout("coordinates.txt"); + fout << xy; + fout.close(); +#endif +*/ // + // Initialize Tracker (slalib or starguider) + // + fCosy->fRaDec = dst; + + // StartThread + Start(); + + ZdAz pos = sla.CalcZdAz(fCosy->fRaDec); + + lout << sla.GetTime() << " - Start Tracking: Ra=" <fRaDec); // soll pointing [rad] + + //lout << sla.GetTime() << pointing.Zd()*kRad2Deg << " " << pointing.Az()*kRad2Deg << endl; + /* + ZdAz dest; + if (!AlignTrackingPos(pointing, dest)) + break; + */ + ZdAz dest = fCosy->AlignTrackingPos(pointing); + + // lout << "DEST: " << dest.Zd()*kRad2Deg << " " <fRaDec) * fCosy->kGearRatio2*4./60.; + //lout << "Vcalc: " << dest.Zd() << " " << dest.Az() << endl; + vcalc *= fCosy->kGearRatio2*4./60.; // [re/min] + + float dtime = -1; + //if (kFALSE /*fUseStarguider*/) + // dtime = Starguider(sla.GetMjd(), dest); + + if (dtime<0) + { + dest = fCosy->fBending(dest); // [rad] + + //lout << "DEST-BEND: " << dest.Zd()*kRad2Deg << " " <CheckRange(dest)) + break; + + dest *= 16384/TMath::Pi()/2; // [se] + dest *= fCosy->kGearRatio; // [re] + + *fCosy->fOutRep << "> ReqRePos1 " << endl; + + // + // Request absolute position of rotary encoder from Macs + // + if (!fCosy->RequestRePos()) + break; + + *fCosy->fOutRep << "> ReqRePos2 " << endl; + + // + // distance between (To+dt) and To [re] + // position time difference < 5usec + // fOffset does the synchronization between the + // Shaft- and the rotary encoders + dest -= fCosy->GetRePos() + fCosy->fOffset; + + dtime = dt; + + ZdAz repos = fCosy->GetRePos(); + // lout << "Repos: " << repos.Zd()/kGearRatio.X() << " " << repos.Az()*kGearRatio.Y() << endl; + // repos /= kGearRatio; + repos /= 16384/TMath::Pi()/2; + repos *= kRad2Deg; + } + + // + // Velocity to go [re/min] to reach the right position at time t+dt + // correct for the duration of RaDec2AltAz + // + const ZdAz v = dest*60.0/(dtime/*-(fMac2->GetTime()-sla)*/); + + // + // calculate real velocity of future [re/min] + // believing the Macs manual '/4' shouldn't be necessary, but it is. + // + ZdAz vt = v/4; + if (LimitSpeed(&vt, vcalc)) + { + lout << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <.9*fCosy->fMac1->GetVelRes() || vt.Az()>.9*fCosy->fMac2->GetVelRes()) + { + lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl; + break; + } + + // + // Set theoretical velocity (as early after calculation as possible) + // Maybe we should attenuate the changes + // + *fCosy->fOutRep << "> SetVelocity1 " << endl; + if (!SetVelocity(vt)) + break; + *fCosy->fOutRep << "> SetVelocity2 " << endl; + + // + // Now do 'unnecessary' things + // + fCosy->fVelocity = vt/fCosy->kGearRatio2*4; + +//--- const double mjd = fMac2->GetMjd(); +//--- fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " "; +//--- fout << setw(4) << vt.Zd() << " "; +//--- fout << setw(4) << vt.Az() << endl; + // + // FIXME? Calculate an accuracy for the tracking system? + // How good do we reach the calculated position in 'real' + // re valus? + // + + + // + // Update speed as often as possible. + // make sure, that dt is around 10 times larger than the + // update time + // + // + // The loop should not be executed faster than the ramp of + // a change in the velocity can be followed. + // (This is important on fast machines >500MHz) + // + /* + MTimeout t(1000); + while (!t.HasTimedOut()) + usleep(1); + */ + usleep(1000000); // 1s + cout << "." << flush; + //usleep(50000); // 0.05s + } + + sla.Now(); + + // StopThread + Stop(); + + fCosy->StopMovement(); + + lout << sla.GetTime() << " - Tracking stopped." << endl; +} + +void *MTracking::Thread() +{ + if (fCosy->fZd1->IsZombieNode() && fCosy->fZd2->IsZombieNode()) + return (void*)1; + + if (fCosy->fAz->IsZombieNode()) + return (void*)2; + + if (!fCosy->fMac1 || !fCosy->fMac2) + return (void*)3; + + lout << "- Tracking Thread started..." << endl; + + SlaStars sla(fCosy->fObservatory); + sla.Now(); + + ZdAz old; + ZdAz ist = fCosy->GetSePos(); // [se] + + ZdAz time; + + ZdAz sollzd = sla.CalcZdAz(fCosy->fRaDec); // [rad] + ZdAz sollaz = sollzd; // [rad] + + // + // only update fTrackingError while tracking + // + bool phca1=false; + bool phca2=false; + bool phcaz=false; + + while (!HasStopFlag()) + { + // + // Make changes (eg wind) smoother - attenuation of control function + // + const float weight = 1.; //0.3; + + // + // This is the time constant which defines how fast + // you correct for external influences (like wind) + // + *fCosy->fOutRep << "> ResetPosHasChanged" << endl; + fCosy->fZd1->ResetPosHasChanged(); + fCosy->fZd2->ResetPosHasChanged(); + fCosy->fAz->ResetPosHasChanged(); + *fCosy->fOutRep << "> Check for PosHasChanged" << endl; + do + { + phca1 = fCosy->fZd1->PosHasChanged(); + phca2 = fCosy->fZd2->PosHasChanged(); + phcaz = fCosy->fAz->PosHasChanged(); + usleep(1); + } while (!phca1 && !phca2 && !phcaz && !HasStopFlag()); + + //---usleep(100000); // 0.1s + + *fCosy->fOutRep << "> Do Calculation" << endl; + + // + // get position, where we are + // + old = ist; + ist = fCosy->GetSePos(); // [se] + + // + // if the position didn't change continue + // + /*--- + if ((int)ist.Zd() == (int)old.Zd() && + (int)ist.Az() == (int)old.Az()) + continue; + */ + ZdAz istre = fCosy->GetRePosPdo(); + + // + // Get time from last shaftencoder position change (position: ist) + // FIXME: I cannot take the avarage + // + // FIXME + //time.Zd(fZd1->GetMjd()); + /* OLD* */ + if (fCosy->fZd1->GetMjd()>fCosy->fZd2->GetMjd()) + time.Zd(fCosy->fZd1->GetMjd()); + else + time.Zd(fCosy->fZd2->GetMjd()); + + //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); + time.Az(fCosy->fAz->GetMjd()); + + // + // if Shaftencoder changed position + // calculate were we should be + // + if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/) + { + sollzd = sla.CalcZdAz(fCosy->fRaDec, time.Zd()); // [rad] + /* + ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); + sollzd = CorrectTarget(ist, dummy); // [se] + */ + fCosy->fOffset.Zd(fCosy->fOffset.Zd()*(1.-weight)+(ist.Zd()*fCosy->kGearRatio.X()-istre.Zd())*weight); + } + + if (phcaz /*(int)ist.Az() != (int)old.Az()*/) + { + sollaz = sla.CalcZdAz(fCosy->fRaDec, time.Az()); // [rad] + /* + ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); + sollaz = CorrectTarget(ist, dummy); // [se] + */ + fCosy->fOffset.Az(fCosy->fOffset.Az()*(1.-weight)+(ist.Az()*fCosy->kGearRatio.Y()-istre.Az())*weight); + } + + ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad] + + fCosy->fZdAzSoll = fCosy->AlignTrackingPos(soll); + + ist *= TMath::Pi()*2/16384; + soll = fCosy->fBending(fCosy->fZdAzSoll); + fCosy->fTrackingError.Set(ist.Zd()-soll.Zd(), ist.Az()-soll.Az()); + + //--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " "; + //--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " "; + //--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " "; + //--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl; + } + + lout << "- Tracking Thread done." << endl; + + return 0; +} Index: trunk/MagicSoft/Cosy/main/MTracking.h =================================================================== --- trunk/MagicSoft/Cosy/main/MTracking.h (revision 3935) +++ trunk/MagicSoft/Cosy/main/MTracking.h (revision 3935) @@ -0,0 +1,35 @@ +#ifndef COSY_MTracking +#define COSY_MTracking + +#ifndef COSY_MPointing +#include "MPointing.h" +#endif + +#ifndef COSY_MThread +#include "MThread.h" +#endif + +class MTracking : public MPointing, public MThread +{ +private: + Float_t fTrackAcc; + Float_t fTrackDec; + + bool SetVelocity(const ZdAz &v); + bool InitTracking(); + bool LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const; + + void *Thread(); + +public: + MTracking(MCosy *cosy, const Log &log) : MPointing(cosy, log), MThread(kFALSE), fTrackAcc(0.1), fTrackDec(0.1) { } + + void TrackPosition(const RaDec &dst); // ra, dec [rad] + void SetTrackAccDec(Float_t acc, Float_t dec) { fTrackAcc=0.1; fTrackDec=0.1; } + + //void TalkThreadTracking(); + + ClassDef(MTracking, 0) +}; + +#endif