Index: trunk/MagicSoft/Cosy/main/MCaos.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MCaos.cc (revision 3935) +++ trunk/MagicSoft/Cosy/main/MCaos.cc (revision 4076) @@ -307,5 +307,5 @@ } -void MCaos::Run(byte *img, bool printl, bool printr, const ZdAz &pos, const MTime &t) +Ring MCaos::Run(byte *img, bool printl, bool printr, const ZdAz &pos, const MTime &t) { Leds &leds = *fLeds; @@ -344,13 +344,7 @@ Rings rings; - rings.CalcRings(leds, 266, 272); + rings.CalcRings(leds, 266, 268); const Ring ¢er = rings.GetCenter(); - - f.DrawCircle(center, 0x80); - f.DrawCircle(center, 5.0, 0x80); - f.DrawCircle(center, 115.0, 0x80); - f.DrawCircle(center, 190.0, 0x80); - // f.MarkPoint(center.GetX(), center.GetY(), 0xa0); // FIXME! @@ -380,4 +374,5 @@ } + return center; /* if (fCaosAnalyse->IsEntryEnabled(IDM_kStopAnalyse)) Index: trunk/MagicSoft/Cosy/main/MCaos.h =================================================================== --- trunk/MagicSoft/Cosy/main/MCaos.h (revision 3935) +++ trunk/MagicSoft/Cosy/main/MCaos.h (revision 4076) @@ -4,4 +4,7 @@ #ifndef CAOS_Leds #include "Leds.h" +#endif +#ifndef CAOS_Ring +#include "Ring.h" #endif @@ -68,5 +71,5 @@ void ResetHistograms(); - void Run(byte *img, bool printl, bool printr, const ZdAz &pos, const MTime &t); + Ring Run(byte *img, bool printl, bool printr, const ZdAz &pos, const MTime &t); }; Index: trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.cc (revision 3935) +++ trunk/MagicSoft/Cosy/main/MCosy.cc (revision 4076) @@ -1,3 +1,2 @@ -#include "MCosy.h" #include "MCosy.h" @@ -22,4 +21,6 @@ #include "MStarguider.h" #include "SlaStars.h" +#include "MPointing.h" +#include "MTracking.h" #include "slalib/slalib.h" // FIXME: REMOVE @@ -31,19 +32,4 @@ typedef struct tm tm_t; - -/* -#define GEAR_RATIO_ALT 2475.6 // [U_mot/U_tel(360deg)] -#define GEAR_RATIO_AZ 5891.7 // [U_mot/U_tel(360deg)] - -#define RES_RE 500 // [re/U_mot] -#define RES_SE 16384 // [se/U_tel(360deg)] -*/ -/* - #define GEAR_RATIO_ALT (75.55*16384/1500) // 75.25 VERY IMPORTANT! unit=U_mot/U_tel - #define GEAR_RATIO_AZ (179.8*16384/1500) // VERY IMPORTANT! unit=U_mot/U_tel -*/ - -//const XY kGearRatio (GEAR_RATIO_ALT*RES_RE/RES_SE, GEAR_RATIO_AZ*RES_RE/RES_SE); //[re/se] -//const XY kGearRatio2(GEAR_RATIO_ALT*RES_RE/360.0, GEAR_RATIO_AZ*RES_RE/360.0); //[re/deg] /* +===================================+ @@ -54,29 +40,4 @@ //#define EXPERT #undef EXPERT - -double MCosy::Rad2SE(double rad) const -{ - return 16384.0/k2Pi*rad; -} - -double MCosy::Rad2ZdRE(double rad) const -{ - return 16384.0/k2Pi*rad*kGearRatio.X(); -} - -double MCosy::Rad2AzRE(double rad) const -{ - return 16384.0/k2Pi*rad*kGearRatio.Y(); -} - -double MCosy::Deg2ZdRE(double rad) const -{ - return rad*kGearRatio2.X(); -} - -double MCosy::Deg2AzRE(double rad) const -{ - return rad*kGearRatio2.Y(); -} /* @@ -191,40 +152,4 @@ // -------------------------------------------------------------------------- // -// request the current positions from the rotary encoders. -// use GetRePos to get the psotions. If the request fails the function -// returns kFALSE, otherwise kTRUE -// -Bool_t MCosy::RequestRePos() -{ - // - // Send request - // - fMac2->RequestSDO(0x6004); - fMac1->RequestSDO(0x6004); - - // - // Wait until the objects are received. - // - fMac2->WaitForSdo(0x6004); - fMac1->WaitForSdo(0x6004); - - // - // If waiting was not interrupted everything is ok. return. - // - if (!(Break() || HasError() || HasZombie())) - return kTRUE; - - // - // If the waiting was interrupted due to a network error, - // print some logging message. - // - if (HasError()) - lout << "Error while requesting re pos from Macs (SDO #6004)" << endl; - - return kFALSE; -} - -// -------------------------------------------------------------------------- -// // reads the Rotary encoder positions from the last request of the Macs. // @@ -253,65 +178,4 @@ // -------------------------------------------------------------------------- // -// 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 MCosy::SetPosVelocity(const Float_t ratio, Float_t vel) -{ - // - // Set velocities - // - const int vr = fMac1->GetVelRes(); - - vel *= vr; - - if (ratio <1) - { - fMac1->SetVelocity(vel); - fMac2->SetVelocity(vel*ratio); - } - else - { - fMac1->SetVelocity(vel/ratio); - 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 MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) -{ - if (HasZombie()) - return; - - SetStatus(MDriveCom::kMoving); - - if (axe1) fMac2->StartRelPos(rd.Zd()); - if (axe2) fMac1->StartRelPos(rd.Az()); -#ifdef EXPERT - cout << "Waiting for positioning..." << flush; -#endif - if (axe1) fMac2->WaitForSdo(0x6004, 1); - if (axe2) fMac1->WaitForSdo(0x6004, 1); - - WaitForEndMovement(); -#ifdef EXPERT - cout << "done." << endl; -#endif -} - -// -------------------------------------------------------------------------- -// // check for a break-signal (from the msgqueue) and errors. // @@ -341,6 +205,5 @@ return; - MTime t; - t.Now(); + MTime t(-1); lout << t << " - MCosy::WaitForEndMovement aborted..."; if (Break()) @@ -434,393 +297,4 @@ } -// -------------------------------------------------------------------------- -// -// 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 MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad] -{ - const ZdAz d = dst*kRad2Deg; - - MTime t; - t.Now(); - lout << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl; - - // - // Calculate new target position (shortest distance to go) - // - const ZdAz src = 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 = fBending(dst); // [rad] - - const ZdAz dest = bend*16384/2/TMath::Pi(); // [se] - - if (!CheckRange(bend)) - return kFALSE; - - bend *= kRad2Deg; - 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 = fMac1->GetVelRes(); - - int i; - for (i=0; i<(track?1:10) && !(Break() || HasError() || HasZombie()); i++) - { - - lout << "- Step #" << i << endl; - // - // Get Shaft Encoder Positions - // - const ZdAz p=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; - SetStatus(MDriveCom::kStopped); - return TRUE; - } - - // - // change units from se to re - // - rd *= kGearRatio; // [re] - rd.Zd(f2-f1); - - // - // Initialize Velocities so that we reach both positions - // at the same time - // - if (i) - { - fMac1->SetAcceleration(0.1*vr); - fMac2->SetAcceleration(0.1*vr); - - fMac1->SetDeceleration(0.1*vr); - fMac2->SetDeceleration(0.1*vr); - - SetPosVelocity(1.0, 0.05); - } - else - { - if (rd.Az()>-15*kGearRatio.Y() && rd.Az()<15*kGearRatio.Y()) - { -#ifdef EXPERT - cout << " -------------- LO ---------------- " << endl; -#endif - fMac1->SetAcceleration(0.05*vr); - fMac1->SetDeceleration(0.05*vr); - } - else - { -#ifdef EXPERT - cout << " -------------- HI ---------------- " << endl; - fMac1->SetAcceleration(0.4*vr);// 0.4 - fMac1->SetDeceleration(0.4*vr);// 0.4 -#else - fMac1->SetAcceleration(0.2*vr); - fMac1->SetDeceleration(0.1*vr); -#endif - } - -#ifdef EXPERT - fMac2->SetAcceleration(0.4*vr);// 0.4 - fMac2->SetDeceleration(0.4*vr);// 0.4 - SetPosVelocity(fabs(rd.Ratio()), 0.2); // fast: 0.6, slow: 0.2 -#else - fMac2->SetAcceleration(0.2*vr); - fMac2->SetDeceleration(0.1*vr); - SetPosVelocity(fabs(rd.Ratio()), 0.1); -#endif - } - - 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() || HasError() || HasZombie())) - { - t.Now(); - lout << t << " - Positioning done." << endl; - SetStatus(MDriveCom::kStopped); - return TRUE; - } - - if (i<10) - StopMovement(); - else - SetStatus(MDriveCom::kStopped); - - t.Now(); - lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl; - return FALSE; -} - -// -------------------------------------------------------------------------- -// -// 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 MCosy::SetVelocity(const ZdAz &v) -{ - // - // Send the new velocities for both axes. - // - fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min] - fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min] - - // - // Wait for the objects to be acknoledged. - // - fMac2->WaitForSdo(0x3006, 1); - fMac1->WaitForSdo(0x3006, 1); - - // - // If the waiting for the objects wasn't interrupted return kTRUE - // - if (!(Break() || HasError() || HasZombie())) - return kTRUE; - - // - // print a message if the interruption was due to a Can-node Error - // - if (HasError()) - lout << "Error while setting velocity (SDO #3006)" << endl; - - return kFALSE; -} - -// -------------------------------------------------------------------------- -// -// 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 MCosy::InitTracking() -{ - // FIXME? Handling of Zombie OK? - if (fMac1->IsZombieNode() || fMac2->IsZombieNode()) - return false; - - // - // Start revolution mode - // - fMac2->SetAcceleration(0.1*fMac2->GetVelRes()); - fMac2->SetDeceleration(0.1*fMac2->GetVelRes()); - if (fMac2->IsZombieNode()) - return false; - - fMac1->SetAcceleration(0.1*fMac1->GetVelRes()); - fMac1->SetDeceleration(0.1*fMac1->GetVelRes()); - if (fMac1->IsZombieNode()) - return false; - - SetStatus(MDriveCom::kMoving | MDriveCom::kTracking); - - fMac2->SetRpmMode(TRUE); - if (fMac2->IsZombieNode()) - return false; - - fMac1->SetRpmMode(TRUE); - if (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 MCosy::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 = fMac1->GetVelRes(); - ULong_t vraz = 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 (sgn(vt->Az()) != sgn(vcalc.Az())) - vt->Az(0); -// else - { - if (fabs(vt->Az()) < fabs(vcalc.Az()) *0.5) - vt->Az(0.5*vcalc.Az()); - - if (fabs(vt->Az()) > fabs(vcalc.Az()) *1.5) - vt->Az(1.5*vcalc.Az()); - } - - if (sgn(vt->Zd()) != sgn(vcalc.Zd())) - vt->Zd(0); -// else - { - if (fabs(vt->Zd()) > fabs(vcalc.Az()) *1.5) - vt->Zd(1.5*vcalc.Zd()); - - if (fabs(vt->Zd()) < fabs(vcalc.Az()) *0.5) - vt->Zd(0.5*vcalc.Zd()); - } - */ - /* - if (sgn(vt->Az()) != sgn(vcalc.Az()) - && fabs(vt->Az()) < limit*fabs(vcalc.Az()) - ) - { - lout << "Warning: Negative Azimuth speed limit (" << limit*100 << "%) exceeded... set to 0." << endl; - vt->Az(0); - } - else*/ - 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 (sgn(vt->Zd()) != sgn(vcalc.Zd()) - && fabs(vt->Zd()) < limit*fabs(vcalc.Zd()) - ) - { - lout << "Warning: Negative Altitude speed limit (" << limit*100 << "%) exceeded... set to 0." << endl; - vt->Zd(0); - } - else - */ 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; -} -/* -Bool_t MCosy::AlignTrackingPos(ZdAz pointing, ZdAz &za) const -{ - // pointing [deg] - if (pointing.Zd()<0) - { - pointing.Zd(-pointing.Zd()); - pointing.Az(pointing.Az()+180); - } - - const ZdAz se = GetSePos()*2*TMath::Pi()/16384; // [rad] - const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing - - do - { - const Double_t d = unbendedse.Az() - pointing.Az(); - if (d>-180 && d<=180) - break; - - pointing.Az(pointing.Az()+TMath::Sign(360., d)); - } while (1); - - const Bool_t rc = CheckRange(pointing); - za = pointing/kRad2Deg; // [rad] - - if (!rc) - lout << "Error: Aligned position out of Range." << endl; - - return rc; -} -*/ - ZdAz MCosy::AlignTrackingPos(ZdAz pointing) const { @@ -837,5 +311,5 @@ } - const ZdAz se = GetSePos()*2*TMath::Pi()/16384; // [rad] + const ZdAz se = GetSePos()*TMath::TwoPi()/kResSE; // [rad] const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing @@ -897,6 +371,5 @@ dest -= point; - dest *= 16384/TMath::Pi()/2; // [se] - dest *= -kGearRatio; // [re] + dest *= -kGearTot/TMath::TwoPi(); // [re] cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl; @@ -905,238 +378,44 @@ } +// -------------------------------------------------------------------------- +// +// 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 MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad] +{ + MPointing point(this, lout); + +//#ifdef EXPERT +// point.SetAccDec(0.4, 0.4); +// point.SetVelocity(0.2); // fast: 0.6, slow: 0.2 +//#else + point.SetPointAccDec(0.2, 0.1); + point.SetPointVelocity(0.1); +//#endif + + return point.SetPosition(dst, track); +} + void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad] { - SlaStars sla(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; - - // 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 = GetSePos()*kGearRatio; - - if (!RequestRePos()) - return; - - // - // Estimate Offset before starting to track - // - fOffset = sepos-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()) - { - 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) - // - fRaDec = dst; - fBackground = kBgdTracking; - - ZdAz pos = sla.CalcZdAz(fRaDec); - - lout << sla.GetTime() << " - Start Tracking: Ra=" <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*fMac1->GetVelRes() || vt.Az()>.9*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 - // - if (!SetVelocity(vt)) - break; - - // - // Now do 'unnecessary' things - // - fVelocity = vt/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(); - - fBackground = kBgdNone; - StopMovement(); - - lout << sla.GetTime() << " - Tracking stopped." << endl; + MTracking track(this, lout); + track.SetOut(fOutRep); +//#ifdef EXPERT +// track.SetPointAccDec(0.4, 0.4); +// track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2 +//#else + track.SetPointAccDec(0.2, 0.1); + track.SetPointVelocity(0.1); +//#endif + track.SetTrackAccDec(0.1, 0.1); + + track.TrackPosition(dst); } @@ -1201,45 +480,4 @@ } -void MCosy::StopTracking() -{ - // - // Set status to Stopping - // - SetStatus(MDriveCom::kStopping); - - // - // set deceleration to 50% - // - cout << "Stopping tracking (dec=20%)..." << endl; - fMac1->SetDeceleration(0.2*fMac1->GetVelRes()); - fMac2->SetDeceleration(0.2*fMac2->GetVelRes()); - - fMac2->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min] - fMac1->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min] - fMac2->WaitForSdo(0x3006, 1); - fMac1->WaitForSdo(0x3006, 1); - - cout << "Waiting for end of movement..." << endl; - WaitForEndMovement(); - - // - // Wait for the objects to be OKed. - // - fMac1->SetRpmMode(FALSE); - fMac2->SetRpmMode(FALSE); - - // - // Wait for the movement to really be finished. - // - //cout << "Waiting for end of movement..." << endl; - //WaitForEndMovement(); - - // - // Check whether everything works fine. - // - CheckForError(); - cout << "Movement stopped." << endl; -} - bool MCosy::CheckNetwork() { @@ -1281,5 +519,5 @@ case WM_STOP: - cout << "MCosy::Proc: Stop." << endl; + //cout << "MCosy::Proc: Stop." << endl; if (!CheckNetwork()) return (void*)0xebb0; @@ -1331,5 +569,5 @@ case WM_TPOINT: { - cout << "WM_TPoint: start." << endl; + //cout << "WM_TPoint: start." << endl; SlaStars sla(fObservatory); sla.Now(); @@ -1340,28 +578,45 @@ AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg; + if (!fOutTp) + { + // + // open tpoint file + // + const TString name = GetFileName("tpoint/tpoint_%s.txt"); + cout << "TPoint-Cosy File ********* " << name << " ********** " << endl; + + fOutTp = new ofstream(name); + *fOutTp << "Magic Model TPOINT data file" << endl; + *fOutTp << ": ALTAZ" << endl; + *fOutTp << "49 48 0 "; + *fOutTp << sla.GetTime().Year() << " " << sla.GetTime().Month() << " " << sla.GetTime().Day() << " "; + *fOutTp << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl; + // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m) + } + cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl; - *tpout << setprecision(7) << za.Az() << " " << za.Alt() << " "; - - ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;; + *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " "; + + ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE; za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az()); za *= kRad2Deg; cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl; - *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << " "; + *fOutTp << fmod(za.Az()+360, 360) << " " << za.Alt() << " "; if (fStarguider) { XY tp = fStarguider->GetCoordinates(); - *tpout << 90-tp.X() << " " << tp.Y() << " "; + *fOutTp << 90-tp.X() << " " << tp.Y() << " "; } - *tpout << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl; - - cout << "WM_TPoint: done. (return 0xaffe)" << endl; + *fOutTp << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl; + + //cout << "WM_TPoint: done. (return 0xaffe)" << endl; } return (void*)0xca1b; case WM_TRACKPOS: - cout << "WM_TrackPosition: start." << endl; + //cout << "WM_TrackPosition: start." << endl; { if (!CheckNetwork()) @@ -1376,13 +631,11 @@ RaDec rd = sla.CalcRaDec(dest); - cout << dest.Zd()*180/3.1415 << " " << dest.Az()*180/3.1415 << endl; - cout << rd.Ra()*12/3.1415 << " " << rd.Dec()*180/3.1415 << endl; TrackPosition(rd); } - cout << "WM_TrackPosition: done. (return 0xabcd)" << endl; + //cout << "WM_TrackPosition: done. (return 0xabcd)" << endl; return (void*)0xabcd; case WM_POSITION: - cout << "WM_Position: start." << endl; + //cout << "WM_Position: start." << endl; { if (!CheckNetwork()) @@ -1392,9 +645,9 @@ SetPosition(dest*kDeg2Rad); } - cout << "WM_Position: done. (return 0x7777)" << endl; + //cout << "WM_Position: done. (return 0x7777)" << endl; return (void*)0x7777; case WM_POSITION1: - cout << "WM_Position1: start." << endl; + //cout << "WM_Position1: start." << endl; { if (!CheckNetwork()) @@ -1404,27 +657,27 @@ SetPosition(dest*kDeg2Rad, kTRUE); } - cout << "WM_Position: done. (return 0x7777)" << endl; + //cout << "WM_Position: done. (return 0x7777)" << endl; return (void*)0x7777; case WM_TESTSE: - cout << "WM_TestSe: start." << endl; + //cout << "WM_TestSe: start." << endl; fBackground = mp ? kBgdSeTest : kBgdNone; - cout << "WM_TestSe: done. (return 0x1e51)" << endl; + //cout << "WM_TestSe: done. (return 0x1e51)" << endl; return (void*)0x1e51; case WM_GEAR: - cout << "WM_Gear: start." << endl; + //cout << "WM_Gear: start." << endl; fBackground = mp ? kBgdGear : kBgdNone; - cout << "WM_Gear: done. (return 0xfeaf)" << endl; + //cout << "WM_Gear: done. (return 0xfeaf)" << endl; return (void*)0xfeaf; case WM_DISPLAY: - cout << "WM_Display: start." << endl; + //cout << "WM_Display: start." << endl; fTriggerDisplay = kTRUE; - cout << "WM_Disply: done. (return 0xd1e1)" << endl; + //cout << "WM_Disply: done. (return 0xd1e1)" << endl; return (void*)0xd1e1; case WM_TRACK: - cout << "WM_Track: START" << endl; + //cout << "WM_Track: START" << endl; { RaDec dest = ((RaDec*)mp)[0]; @@ -1435,27 +688,27 @@ TrackPosition(dest*kDeg2Rad); } - cout << "WM_Track: done. (return 0x8888)" << endl; + //cout << "WM_Track: done. (return 0x8888)" << endl; return (void*)0x8888; case WM_NEWTRACK: - cout << "WM_NewTrack: START" << endl; + //cout << "WM_NewTrack: START" << endl; fRaDec = *((RaDec*)mp); - cout << "WM_NewTrack: done. (return 0x9999)" << endl; + //cout << "WM_NewTrack: done. (return 0x9999)" << endl; return (void*)0x9999; case WM_LOADBENDING: - cout << "WM_LoadBending: START" << endl; + //cout << "WM_LoadBending: START" << endl; fBending.Load("bending.txt"); - cout << "WM_LoadBending: done. (return 0xbe0d)" << endl; + //cout << "WM_LoadBending: done. (return 0xbe0d)" << endl; return (void*)0xbe0d; case WM_RESETBENDING: - cout << "WM_ResetBending: START" << endl; + //cout << "WM_ResetBending: START" << endl; fBending.Reset(); - cout << "WM_ResetBending: done. (return 0xbe0e)" << endl; + //cout << "WM_ResetBending: done. (return 0xbe0e)" << endl; return (void*)0xbe0e; case WM_HOME: - cout << "WM_Home: START" << endl; + //cout << "WM_Home: START" << endl; if (!CheckNetwork()) return (void*)0xebb0; @@ -1485,5 +738,5 @@ */ } - cout << "WM_Home: done. (return 0x403e)" << endl; + //cout << "WM_Home: done. (return 0x403e)" << endl; return (void*)0x403e; @@ -1510,5 +763,5 @@ a1 *= kRad2Deg; - const ZdAz a2 = a1*16384/360; + const ZdAz a2 = a1*kResSE/360; cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl; @@ -1519,5 +772,5 @@ case WM_ENDSWITCH: { - ZdAz pos = GetSePos()*TMath::Pi()*2/16384; + ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE; pos = fBending.SubtractOffsets(pos)*kRad2Deg; @@ -1575,45 +828,49 @@ cout << "Reading gear ratios..." << flush; - const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0); - const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0); - - Double_t resreaz = 0; + kGear.Y(env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0)); + kGear.X(env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0)); + + kResRE.Y(0); if (fMac1 && !fMac1->IsZombieNode()) - resreaz = fMac1->GetRes(); + kResRE.Y(fMac1->GetRes()); else if (fMac3 && !fMac3->IsZombieNode()) - resreaz = fMac3->GetRes(); + kResRE.Y(fMac3->GetRes()); else - resreaz = env.GetValue("Az_ResRE[re/U_mot]", 1500); - - Double_t resrezd = 0; + kResRE.Y(env.GetValue("Az_ResRE[re/U_mot]", 1500)); + + kResRE.X(0); if (fMac2 && !fMac2->IsZombieNode()) - resrezd = fMac2->GetRes(); + kResRE.X(fMac2->GetRes()); else - resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500); - - Double_t ressezd = 0; + kResRE.X(env.GetValue("Zd_ResRE[re/U_mot]", 1500)); + + kResSE.X(0); if (fZd1 && !fZd1->IsZombieNode()) - ressezd = fZd1->GetPhysRes(); + kResSE.X(fZd1->GetPhysRes()); else if (fZd2 && !fZd2->IsZombieNode()) - ressezd = fZd2->GetPhysRes(); + kResSE.X(fZd2->GetPhysRes()); else - ressezd = env.GetValue("Zd_ResSE[se/U_mot]", 16384); - - Double_t resseaz = 0; + kResSE.X(env.GetValue("Zd_ResSE[se/U_mot]", 16384)); + + kResSE.Y(0); if (fAz && !fAz->IsZombieNode()) - resseaz = fAz->GetPhysRes(); + kResSE.Y(fAz->GetPhysRes()); else - resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384); - - kGearRatio.Set (gzd*resrezd*4/ressezd, gaz*resreaz*4/resseaz); //[re/se] - kGearRatio2.Set(gzd*resrezd*4/360.0, gaz*resreaz*4/360.0); //[re/deg] + kResSE.Y(env.GetValue("Az_ResSE[se/U_mot]", 16384)); + + // believing the Macs manual '*4' shouldn't be necessary, but it is. + // Because the a RE is 4 quad counts. + // Calculating speeds we have to convert back to qc + kResRE *= 4; + kGearTot = kResRE*kGear; + cout << "done." << endl; cout << " * Setting Gear Ratios:" << endl; cout << " --------------------" << endl; - cout << " * X: " << gzd << "*" << resrezd << "/" << ressezd << "=4*" << kGearRatio.X() << endl; - cout << " * Y: " << gaz << "*" << resreaz << "/" << resseaz << "=4*" << kGearRatio.Y() << endl; + cout << " * X: " << kGear.X() << "*" << kResRE.X()/4 << "/" << kResSE.X() << "=4*" << kGearTot.X() << "/" << kResSE.X() << endl; + cout << " * Y: " << kGear.Y() << "*" << kResRE.Y()/4 << "/" << kResSE.Y() << "=4*" << kGearTot.Y() << "/" << kResSE.Y() << endl; } @@ -1632,133 +889,4 @@ fMac3->SetDeceleration(0.2*res); fMac3->StartPosSync(); -} - -void MCosy::TalkThreadTracking() -{ - if (fZd1->IsZombieNode() && fZd2->IsZombieNode()) - return; - - if (fAz->IsZombieNode()) - return; - - if (!fMac1 || !fMac2) - return; - - lout << "- Tracking Thread started..." << endl; - - SlaStars sla(fObservatory); - sla.Now(); - - ZdAz old; - ZdAz ist = GetSePos(); // [se] - - ZdAz time; - - ZdAz sollzd = sla.CalcZdAz(fRaDec); // [rad] - ZdAz sollaz = sollzd; // [rad] - - // - // only update fTrackingError while tracking - // - bool phca1=false; - bool phca2=false; - bool phcaz=false; - - while (fBackground==kBgdTracking) - { - // - // 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) - // - fZd1->ResetPosHasChanged(); - fZd2->ResetPosHasChanged(); - fAz->ResetPosHasChanged(); - do - { - phca1 = fZd1->PosHasChanged(); - phca2 = fZd2->PosHasChanged(); - phcaz = fAz->PosHasChanged(); - usleep(1); - } while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking); - - //---usleep(100000); // 0.1s - - // - // get position, where we are - // - old = ist; - ist = 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 = GetRePosPdo(); - - // - // Get time from last shaftencoder position change (position: ist) - // FIXME: I cannot take the avarage - // - // FIXME - //time.Zd(fZd1->GetMjd()); - /* OLD* */ - if (fZd1->GetMjd()>fZd2->GetMjd()) - time.Zd(fZd1->GetMjd()); - else - time.Zd(fZd2->GetMjd()); - - //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); - time.Az(fAz->GetMjd()); - - // - // if Shaftencoder changed position - // calculate were we should be - // - if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/) - { - sollzd = sla.CalcZdAz(fRaDec, time.Zd()); // [rad] - /* - ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); - sollzd = CorrectTarget(ist, dummy); // [se] - */ - fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight); - } - - if (phcaz /*(int)ist.Az() != (int)old.Az()*/) - { - sollaz = sla.CalcZdAz(fRaDec, time.Az()); // [rad] - /* - ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); - sollaz = CorrectTarget(ist, dummy); // [se] - */ - fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight); - } - - ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad] - - fZdAzSoll = AlignTrackingPos(soll); - - ist *= TMath::Pi()*2/16384; - soll = fBending(fZdAzSoll); - 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; - - //--- fout << endl << endl; } @@ -1813,5 +941,5 @@ } - Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384; + Double_t apos = (pos[0]-pos[1])/2 * TMath::TwoPi() / kResSE.X(); ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg; @@ -1865,5 +993,5 @@ ZdAz dre = re-re0; - if (fabs(dse.Zd())*144>16384) // Each 2.5deg (144) + if (fabs(dse.Zd())*144>kResSE.X()) // Each 2.5deg (144) { se0.Zd(se.Zd()); @@ -1872,9 +1000,9 @@ se -= dse/2; - ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg; + ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg; ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd()); } - if (fabs(dse.Az())*144>16384) // Each 2.5deg (144) + if (fabs(dse.Az())*144>kResSE.Y()) // Each 2.5deg (144) { se0.Az(se.Az()); @@ -1883,5 +1011,5 @@ se -= dse/2; - ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg; + ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*kRad2Deg; ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az()); } @@ -1927,9 +1055,9 @@ case kBgdNone: continue; - +/*#ifndef NEWALGO case kBgdTracking: TalkThreadTracking(); continue; - +#endif*/ case kBgdSeTest: TalkThreadSeTest(); @@ -1952,17 +1080,20 @@ // GetPointingPos [deg] - const ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se] - - //cout << seist.Zd()*kRad2Deg << " " << seist.Az()*kRad2Deg << endl; - - ZdAz back = fBending.CorrectBack(seist)*180/TMath::Pi(); - - //cout << back.Zd() << " " << back.Az() << endl; - - return back; + const ZdAz seist = GetSePos()*TMath::TwoPi()/kResSE; // [rad] + return fBending.CorrectBack(seist)*TMath::RadToDeg(); } Bool_t MCosy::HandleTimer(TTimer *t) { + const Int_t rc = fMutexGui.TryLock(); + if (rc==13) + cout << "MCosy::HandleTimer - mutex is already locked by this thread" << endl; + + if (rc) + { + lout << "* GUI update skipped due to locked mutex." << endl; + return kTRUE; + } + // // Update Gui, foremer MTGui. @@ -1975,6 +1106,4 @@ fAz->DisplayVal(); - ZdAz bendist = GetPointingPos(); - Byte_t avail = 0; @@ -1989,9 +1118,13 @@ SetStatus(MDriveCom::kError); - lout.UpdateGui(); - + ZdAz bendist = GetPointingPos(); + fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError); + + fWin->UpdateWeather(*fCom); fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/ fRaDec, fZdAzSoll, fStatus, avail); + lout.UpdateGui(); + const Bool_t trigger = fTriggerDisplay; fTriggerDisplay = kFALSE; @@ -2003,11 +1136,7 @@ DisplayHistGear(!trigger); - // FIXME: Not thread safe! - static int i=0; - if (i++==7) - { - fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError); - i=0; - } + if (fMutexGui.UnLock()==13) + cout << "MCosy::HandleTimer - tried to unlock mutex locked by other thread." << endl; + return kTRUE; } @@ -2195,4 +1324,8 @@ fAz =new ShaftEncoder(id6, "SE/Az", lout); + fZd1->SetReport(fOutRep); + fZd2->SetReport(fOutRep); + fAz->SetReport(fOutRep); + lout << "- Connecting devices to network." << endl; @@ -2265,6 +1398,23 @@ } */ + +TString MCosy::GetFileName(const char *fmt) +{ + // FIXME: Timeout missing + while (1) + { + MTime time(-1); + const TString name = Form(fmt, (const char*)time.GetFileName()); + if (gSystem->AccessPathName(name, kFileExists)) + return name; + break; + + usleep(1000); + } + return ""; +} + MCosy::MCosy(/*int mode,*/ const char *dev, const int baud, MLog &out) -: Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fStarguider(NULL), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone), fStatus(MDriveCom::kStopped) +: Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fStarguider(NULL), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0) { TEnv env(".cosyrc"); @@ -2275,4 +1425,9 @@ const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5 const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6 + + TString name = GetFileName("rep/cosy_%s.rep"); + cout << "Open Repfile: " << name << endl; + fOutRep = new MLog(name, kTRUE); + /* lout << "- Program in "; @@ -2300,28 +1455,5 @@ fAz->SetDisplay(fWin->GetLabel1()); - // - // open tpoint file - // - MTime time; - TString name; - while (1) - { - time.Now(); - name = Form("tpoint/tpoint_%s.txt", (const char*)time.GetFileName()); - if (gSystem->AccessPathName(name, kFileExists)) - break; - } - - cout << "TPoint File ********* " << name << " ********** " << endl; - - tpout = new ofstream(name); - *tpout << "Magic Model TPOINT data file" << endl; - *tpout << ": ALTAZ" << endl; - *tpout << "49 48 0 "; - *tpout << time.Year() << " " << time.Month() << " " << time.Day() << " "; - *tpout << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl; - // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m) - - fCom = new MDriveCom(this, out); + fCom = new MDriveCom(this, *fOutRep); fCom->Start(); } @@ -2349,7 +1481,10 @@ MCosy::~MCosy() { - *tpout << "END" << endl; - //streampos size = tpout.tellp(); - delete tpout; + if (fOutTp) + { + *fOutTp << "END" << endl; + delete fOutTp; + } + delete fOutRep; cout << "Deleting GUI timer." << endl; @@ -2359,4 +1494,8 @@ cout << "Deleting Nodes." << endl; + + fZd1->SetReport(0); + fZd2->SetReport(0); + fAz->SetReport(0); delete fAz; Index: trunk/MagicSoft/Cosy/main/MCosy.h =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.h (revision 3935) +++ trunk/MagicSoft/Cosy/main/MCosy.h (revision 4076) @@ -8,6 +8,9 @@ #include "MBending.h" -#ifndef MARS_Mobservatory +#ifndef MARS_MObservatory #include "MObservatory.h" +#endif +#ifndef ROOT_TMutex +#include #endif @@ -62,4 +65,6 @@ { friend class MTTalk; + friend class MPointing; + friend class MTracking; private: @@ -83,8 +88,10 @@ // with a generic interface to both... + TMutex fMutexGui; + enum BackgroundAction_t { kBgdNone, - kBgdTracking, + //kBgdTracking, kBgdSeTest, kBgdSeTestDispl, @@ -96,5 +103,4 @@ ZdAz fTrackingError; // [rad] Tracking Offset between SE and calc-pos - ZdAz fOffset; // Offset between se and re coordinate system [re] ZdAz fZdAzSoll; // [rad] Soll position when moving RaDec fRaDec; // Position to track @@ -107,6 +113,8 @@ Bool_t fTriggerDisplay; - XY kGearRatio; // describing the gear of the system [re/se] - XY kGearRatio2; // describing the gear of the system [re/deg] + XY kResSE; // describing the resolution of the system [se/U_tel] + XY kResRE; // describing the resolution of the system [re/U_mot] + XY kGear; // describing the resolution of the system [U_mot/U_tel] + XY kGearTot; // describing the resolution of the system [re/U_tel] MBending fBending; @@ -114,17 +122,11 @@ UInt_t fStatus; - ofstream *tpout; - - //Bool_t AlignTrackingPos(ZdAz pointing, ZdAz &za) const; + ofstream *fOutTp; + MLog *fOutRep; + ZdAz AlignTrackingPos(ZdAz pointing) const; Bool_t CheckRange(const ZdAz &d) const; Double_t Starguider(Double_t mjd, ZdAz &dest) const; - double Rad2SE(double rad) const; - double Rad2ZdRE(double rad) const; - double Rad2AzRE(double rad) const; - double Deg2ZdRE(double rad) const; - double Deg2AzRE(double rad) const; - void SetStatus(UInt_t stat) { fStatus = stat; } UInt_t GetStatus() const { return fStatus; } @@ -133,18 +135,10 @@ ZdAz GetRePosPdo(); ZdAz GetSePos() const; // [se] - - Bool_t RequestRePos(); - Bool_t SetVelocity(const ZdAz &v); - - void SetPosVelocity(const Float_t ratio, Float_t vel); - - void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2); + // FIXME: Must depend on the Shaftencoder mounted + ZdAz GetSePosRad() const { return GetSePos()*TMath::TwoPi()/16384; } // [rad] void InitSync(); - bool InitTracking(); - Bool_t LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const; void TalkThread(); - void TalkThreadTracking(); void TalkThreadSeTest(); void TalkThreadGear(); @@ -154,8 +148,7 @@ int SetPosition(const ZdAz &dst, Bool_t track=kFALSE); + void TrackPosition(const RaDec &dst); // ra, dec [rad] void TerminateApp(); - - void TrackPosition(const RaDec &dst); // ra, dec [rad] int StopWaitingForSDO() const; @@ -163,5 +156,4 @@ void StopMovement(); - void StopTracking(); void WaitForEndMovement(); @@ -188,5 +180,17 @@ ZdAz GetPointingPos(void) const; void SetStarguider(MStarguider *s) { fStarguider = s; } - + + static TString GetFileName(const char *name); + + MGCosy *GetWin() { return fWin; } + + AltAz GetAltAzDeg() const + { + ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE; + AltAz za1(TMath::Pi()/2-sepos.Zd(), sepos.Az()); + za1 *= kRad2Deg; + return za1; + } + // static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst); // static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0)); Index: trunk/MagicSoft/Cosy/main/MPointing.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MPointing.cc (revision 3935) +++ trunk/MagicSoft/Cosy/main/MPointing.cc (revision 4076) @@ -74,5 +74,5 @@ const int vr = mac->GetVelRes(); mac->SetAcceleration(acc*vr); - mac->SetAcceleration(dec*vr); + mac->SetDeceleration(dec*vr); return !mac->IsZombieNode(); } @@ -96,5 +96,4 @@ int MPointing::SetPosition(const ZdAz &dst, Bool_t track) // [rad] { - /* const ZdAz d = dst*kRad2Deg; @@ -105,5 +104,5 @@ // Calculate new target position (shortest distance to go) // - const ZdAz src = fCosy->GetSePos(); // [se] + //const ZdAz src = fCosy->GetSePos(); // [se] // @@ -123,5 +122,5 @@ ZdAz bend = fCosy->fBending(dst); // [rad] - const ZdAz dest = bend*16384/2/TMath::Pi(); // [se] + const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se] if (!fCosy->CheckRange(bend)) @@ -131,46 +130,35 @@ 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; + //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(); 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) { @@ -178,207 +166,25 @@ lout << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl; fCosy->SetStatus(MDriveCom::kStopped); - fCosy->fCom->Send("POSITION DONE"); + 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->kGearRatio; // [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->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) { @@ -391,5 +197,5 @@ else { - const Double_t y = 15*fCosy->kGearRatio.Y(); + const Double_t y = 15*fCosy->kGearTot.Y()*fCosy->kResSE.Y(); if (rd.Az()>-y && rd.Az()SetStatus(MDriveCom::kStopped); - fCosy->fCom->Send("POSITION DONE"); + fCosy->fCom->SendStatus("Tracking preposition reached."); return TRUE; } @@ -441,5 +243,5 @@ lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl; - fCosy->fCom->Send("POSITION FAILED"); + fCosy->fCom->SendStatus("Target position missed!"); return FALSE; Index: trunk/MagicSoft/Cosy/main/MTracking.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MTracking.cc (revision 3935) +++ trunk/MagicSoft/Cosy/main/MTracking.cc (revision 4076) @@ -13,4 +13,40 @@ //#define EXPERT #undef EXPERT + +// -------------------------------------------------------------------------- +// +// request the current positions from the rotary encoders. +// use GetRePos to get the psotions. If the request fails the function +// returns kFALSE, otherwise kTRUE +// +bool MTracking::RequestRePos() +{ + // + // Send request + // + fCosy->fMac2->RequestSDO(0x6004); + fCosy->fMac1->RequestSDO(0x6004); + + // + // Wait until the objects are received. + // + fCosy->fMac2->WaitForSdo(0x6004); + fCosy->fMac1->WaitForSdo(0x6004); + + // + // If waiting was not interrupted everything is ok. return. + // + if (!Break()) + return true; + + // + // If the waiting was interrupted due to a network error, + // print some logging message. + // + if (fCosy->HasError()) + lout << "Error while requesting re pos from Macs (SDO #6004)" << endl; + + return false; +} // -------------------------------------------------------------------------- @@ -49,5 +85,46 @@ return true; } - +/* +void MTracking::StopTracking() +{ + // + // Set status to Stopping + // + fCosy->SetStatus(MDriveCom::kStopping); + + // + // set deceleration to 50% + // + cout << "Stopping tracking (dec=20%)..." << endl; + fCosy->fMac1->SetDeceleration(0.2*fMac1->GetVelRes()); + fCosy->fMac2->SetDeceleration(0.2*fMac2->GetVelRes()); + + fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min] + fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min] + fCosy->fMac2->WaitForSdo(0x3006, 1); + fCosy->fMac1->WaitForSdo(0x3006, 1); + + cout << "Waiting for end of movement..." << endl; + fCosy->WaitForEndMovement(); + + // + // Wait for the objects to be OKed. + // + fCosy->fMac1->SetRpmMode(FALSE); + fCosy->fMac2->SetRpmMode(FALSE); + + // + // Wait for the movement to really be finished. + // + //cout << "Waiting for end of movement..." << endl; + //WaitForEndMovement(); + + // + // Check whether everything works fine. + // + fCosy->CheckForError(); + cout << "Movement stopped." << endl; +} +*/ // -------------------------------------------------------------------------- // @@ -58,6 +135,18 @@ // velocities are limited to the maximum velocity. // -Bool_t MTracking::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const -{ +Bool_t MTracking::LimitSpeed(ZdAz *vt, const SlaStars &sla) const +{ + // vt[re/min] + + // Calculate approximate velocity of both axis + ZdAz vcalc = sla.GetApproxVel(fCosy->fRaDec); // [rad/rad] + + //vcalc *= 1./(24*60); // [U_tel/min] + //vcalc *= fCosy->kGearTot; // [U_mot/min] + //vcalc *= fCosy->kResRE; // [re/min] + + vcalc *= fCosy->kGearTot*fCosy->kResRE/(24*60); // [re/min] + + // Set return code Bool_t rc = kFALSE; @@ -83,14 +172,16 @@ const Float_t maxtrack = 0.1; - if (fabs(vt->Az()) > maxtrack*vraz) - { + if (fabs(vt->Az()) > maxtrack*vraz*4) + { + vt->Az(maxtrack*vraz*4*sgn(vcalc.Az())); lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Az()) << " > " << maxtrack*vraz << ")... limited." << endl; - vt->Az(maxtrack*vraz*sgn(vcalc.Az())); + lout << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <Zd()) > maxtrack*vrzd) - { + if (fabs(vt->Zd()) > maxtrack*vrzd*4) + { + vt->Zd(maxtrack*vrzd*4*sgn(vcalc.Zd())); lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Zd()) <<" > " << maxtrack*vrzd << ")... limited." << endl; - vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd())); + lout << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <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 the star is culminating behind the zenith (South) we want to + // align the azimuth angle between -180 and 180deg. If the star is + // culminating before the zenith (north) we want the star to be + // aligned between -180 and 180deg (which is the default of CalcZdAz) + if (sla.GetPhi()>dst.Dec() && dest.Az()<0) + { + // align az from -180/180 to 0/360 + lout << "Star culminating behind zenith: Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl; + dest.Az(dest.Az() + TMath::TwoPi()); + } + + // Position the telescope to the current local position of the + // star. Do not reposition but start the tracking after the + // first positioning step if (!SetPosition(dest, kTRUE)) - //if (!SetPosition(dest, kFALSE)) { lout << "Error: Cannot start tracking, positioning failed." << endl; @@ -177,7 +260,6 @@ // calculate offset from present se position // - const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearRatio; - - if (!fCosy->RequestRePos()) + const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearTot/fCosy->kResSE; //[re] + if (!RequestRePos()) return; @@ -185,5 +267,5 @@ // Estimate Offset before starting to track // - fCosy->fOffset = sepos-fCosy->GetRePos(); + fOffset = sepos-fCosy->GetRePos(); /* @@ -202,18 +284,5 @@ } - 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; @@ -221,14 +290,12 @@ Start(); + // Get current nominal local position + sla.Now(); ZdAz pos = sla.CalcZdAz(fCosy->fRaDec); + // Some output + XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec())); 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] + // Request nominal position for this time in the future (To+dt) + // + const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad] + ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity + + //ZdAz vcalc = sla.GetApproxVel(fCosy->fRaDec); + //vcalc *= fCosy->kGearRatio2*4./60.; // [re/min] float dtime = -1; @@ -267,25 +324,22 @@ // dtime = Starguider(sla.GetMjd(), dest); + ZdAz repos; if (dtime<0) { - dest = fCosy->fBending(dest); // [rad] - - //lout << "DEST-BEND: " << dest.Zd()*kRad2Deg << " " <fBending(dest); // [rad] if (!fCosy->CheckRange(dest)) break; - dest *= 16384/TMath::Pi()/2; // [se] - dest *= fCosy->kGearRatio; // [re] - - *fCosy->fOutRep << "> ReqRePos1 " << endl; + dest *= fCosy->kGearTot/TMath::TwoPi(); // [re] + + //*fCosy->fOutRep << "> ReqRePos1 " << endl; // // Request absolute position of rotary encoder from Macs // - if (!fCosy->RequestRePos()) + if (!RequestRePos()) break; - *fCosy->fOutRep << "> ReqRePos2 " << endl; + //*fCosy->fOutRep << "> ReqRePos2 " << fOffset.Zd() << " " << fOffset.Az() << endl; // @@ -294,13 +348,8 @@ // fOffset does the synchronization between the // Shaft- and the rotary encoders - dest -= fCosy->GetRePos() + fCosy->fOffset; + repos = fCosy->GetRePos(); + dest -= repos + fOffset; //[re] 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; } @@ -309,5 +358,22 @@ // correct for the duration of RaDec2AltAz // - const ZdAz v = dest*60.0/(dtime/*-(fMac2->GetTime()-sla)*/); + /* --- OLD --- */ + ZdAz v = dest*60.0/dtime; //[re/min] + /* --- NEW --- seems to work worse! */ + //const Double_t dtaz = sla.GetTime() - fCosy->fMac1->GetPosTime(); + //const Double_t dtzd = sla.GetTime() - fCosy->fMac2->GetPosTime(); + // + //ZdAz v = dest*60.0; + //v.Zd(v.Zd()/dtzd); + //v.Az(v.Az()/dtaz); + /* --- END --- */ + + //*fCosy->fOutRep << "> Dt: " << dtaz << " " << dtzd << endl; + + if (LimitSpeed(&v, sla)) + { + lout << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl; + lout << "Dest: " << dest.Zd() << " " << dest.Az() << endl; + } // @@ -315,12 +381,8 @@ // 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" <fOutRep << "> SetVelocity1 " << endl; + //*fCosy->fOutRep << "> SetVelocity1: " << vt.Zd() << " " << vt.Az() << 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? - // - + //*fCosy->fOutRep << "> SetVelocity2 " << endl; + + // + // Now do 'unnecessary' things (timing) + // + fCosy->fVelocity = vt*4/fCosy->kGear; // [U_mot/min] + // *OLD* fVelocity = vt/kGearRatio2*4; + + if (fOut) + { + fOut->Lock(); + *fOut << "RE-REPORT " << MTime(-1) << " " << repos.Zd() << " " << repos.Az() <<" " << vt.Zd() << " " << vt.Az() << endl; + fOut->UnLock(); + } // @@ -363,17 +422,10 @@ // 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 +// *****FIXME**** cout << "." << flush; } @@ -385,5 +437,7 @@ fCosy->StopMovement(); - lout << sla.GetTime() << " - Tracking stopped." << endl; + lout << sla.GetTime() << " - Tracking stopped @ Zd="; + lout << fCosy->fZdAzSoll.Zd()*TMath::RadToDeg() <<"deg Az="; + lout << fCosy->fZdAzSoll.Az()*TMath::RadToDeg() <<"deg" << endl; } @@ -401,14 +455,12 @@ lout << "- Tracking Thread started..." << endl; + const XY re2se = fCosy->kGearTot/fCosy->kResSE; //[re/se] + 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] + ZdAz soll = sla.CalcZdAz(fCosy->fRaDec); // [rad] // @@ -421,18 +473,15 @@ 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; + const float weight = 1.; //0.3; + + // Check for changes of the shaftencoder values + //*fCosy->fOutRep << "> ResetPosHasChanged" << endl; fCosy->fZd1->ResetPosHasChanged(); fCosy->fZd2->ResetPosHasChanged(); fCosy->fAz->ResetPosHasChanged(); - *fCosy->fOutRep << "> Check for PosHasChanged" << endl; + //*fCosy->fOutRep << "> Check for PosHasChanged" << endl; do { @@ -443,31 +492,16 @@ } 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; - */ + //*fCosy->fOutRep << "> Do Calculation" << endl; + + // Get current position of motors (use last automatically sent + // position (PDO) - requesting the position results in problems + // with thread safty) ZdAz istre = fCosy->GetRePosPdo(); - // + // get current position of shaftencoders + ZdAz istse = fCosy->GetSePos(); // [se] + // Get time from last shaftencoder position change (position: ist) - // FIXME: I cannot take the avarage - // - // FIXME - //time.Zd(fZd1->GetMjd()); - /* OLD* */ + // FIXME: Is this correct? if (fCosy->fZd1->GetMjd()>fCosy->fZd2->GetMjd()) time.Zd(fCosy->fZd1->GetMjd()); @@ -475,43 +509,34 @@ 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); + // calculate offset for both axis (only one is needed) + const ZdAz offset = (istse*re2se - istre)*weight + fOffset*(weight-1); + + // if Shaftencoder changed position, calculate nominal position + if (phca1 || phca2) + { + const ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, time.Zd()); + soll.Zd(dummy.Zd()); // [rad] + fOffset.Zd(offset.Zd()); } - - 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); + if (phcaz) + { + const ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, time.Az()); + soll.Az(dummy.Az()); // [rad] + fOffset.Az(offset.Az()); } - ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad] - + // After calculation of fOffset is done we need 'ist' in rad + istse /= fCosy->kResSE/TMath::TwoPi(); // [rad] + + // Calculate the aligned tracking posotion from 'soll'-position 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; + /* --- OLD --- */ + //fCosy->fTrackingError = istse-fCosy->fBending(fCosy->fZdAzSoll); + /* --- NEW --- */ + fCosy->fTrackingError = fCosy->fBending.CorrectBack(istse)-fCosy->fZdAzSoll; + /* --- END --- */ } Index: trunk/MagicSoft/Cosy/main/MTracking.h =================================================================== --- trunk/MagicSoft/Cosy/main/MTracking.h (revision 3935) +++ trunk/MagicSoft/Cosy/main/MTracking.h (revision 4076) @@ -1,4 +1,6 @@ #ifndef COSY_MTracking #define COSY_MTracking + +#include "coord.h" #ifndef COSY_MPointing @@ -10,4 +12,6 @@ #endif +class SlaStars; + class MTracking : public MPointing, public MThread { @@ -16,16 +20,23 @@ Float_t fTrackDec; + ZdAz fOffset; // Offset between se and re coordinate system [re] + + MLog *fOut; + + bool RequestRePos(); bool SetVelocity(const ZdAz &v); + bool LimitSpeed(ZdAz *vt, const SlaStars &sla) const; bool InitTracking(); - bool LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const; + //void StopTracking(); void *Thread(); public: - MTracking(MCosy *cosy, const Log &log) : MPointing(cosy, log), MThread(kFALSE), fTrackAcc(0.1), fTrackDec(0.1) { } + MTracking(MCosy *cosy, const Log &log) : MPointing(cosy, log), MThread(kFALSE), fTrackAcc(0.1), fTrackDec(0.1), fOut(0) { } void TrackPosition(const RaDec &dst); // ra, dec [rad] void SetTrackAccDec(Float_t acc, Float_t dec) { fTrackAcc=0.1; fTrackDec=0.1; } + void SetOut(MLog *fout) { fOut = fout; } //void TalkThreadTracking();