Changeset 4076 for trunk/MagicSoft/Cosy/main

Timestamp:

05/15/04 16:46:27 (21 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy/main

Files:

• MCaos.cc (modified) (3 diffs)
• MCaos.h (modified) (2 diffs)
• MCosy.cc (modified) (39 diffs)
• MCosy.h (modified) (10 diffs)
• MPointing.cc (modified) (11 diffs)
• MTracking.cc (modified) (21 diffs)
• MTracking.h (modified) (3 diffs)

trunk/MagicSoft/Cosy/main/MCaos.cc

@@ -307 +307 @@
 }
 
-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 +344 @@
 
     Rings rings;
-    rings.CalcRings(leds, 266, 272);
+    rings.CalcRings(leds, 266, 268);
 
     const Ring &center = 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 +374 @@
     }
 
+    return center;
     /*
         if (fCaosAnalyse->IsEntryEnabled(IDM_kStopAnalyse))

trunk/MagicSoft/Cosy/main/MCaos.h

@@ -4 +4 @@
 #ifndef CAOS_Leds
 #include "Leds.h"
+#endif
+#ifndef CAOS_Ring
+#include "Ring.h"
 #endif
 
@@ -68 +71 @@
     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);
 };
 

trunk/MagicSoft/Cosy/main/MCosy.cc

@@ -1 @@
-#include "MCosy.h"
 #include "MCosy.h"
 
@@ -22 +21 @@
 #include "MStarguider.h"
 #include "SlaStars.h"
+#include "MPointing.h"
+#include "MTracking.h"
 
 #include "slalib/slalib.h"  // FIXME: REMOVE
@@ -31 +32 @@
 
 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 +40 @@
 //#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 +152 @@
 // --------------------------------------------------------------------------
 //
-// 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 +178 @@
 // --------------------------------------------------------------------------
 //
-//  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 +205 @@
         return;
 
-    MTime t;
-    t.Now();
+    MTime t(-1);
     lout << t << " - MCosy::WaitForEndMovement aborted...";
     if (Break())
@@ -434 +297 @@
 }
 
-// --------------------------------------------------------------------------
-//
-// 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 +311 @@
     }
 
-    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 +371 @@
 
     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 +378 @@
 }
 
+// --------------------------------------------------------------------------
+//
+// 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=" <<xy.X() << "h Dec=";
-    lout << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;
-
-//---    ofstream fout("log/cosy.pos");
-//---    fout << "Tracking:";
-//---    fout << " Ra: " << Rad2Deg(dst.Ra())  << "\x9c  ";
-//---    fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
-//---    fout << "     Mjd/10ms    V/re/min/4" << endl;
-
-    //
-    // We want to reach the theoretical position exactly in about 0.5s
-    //
-    // *OLD*const float dt = 1;  // 1 second
-    const float dt = 5;//3;  // 2 second
-    while (!(Break() || HasError() || HasZombie()))
-    {
-        //
-        // Request Target position for this moment
-        //
-        sla.Now(dt);
-
-        //
-        // Request theoretical Position for a time in the future (To+dt) from CPU
-        //
-         const ZdAz pointing = sla.CalcZdAz(fRaDec); // soll pointing [rad]
-
-        //lout << sla.GetTime() << pointing.Zd()*kRad2Deg << " " << pointing.Az()*kRad2Deg << endl;
-        /*
-        ZdAz dest;
-        if (!AlignTrackingPos(pointing, dest))
-            break;
-            */
-        ZdAz dest = AlignTrackingPos(pointing);
-
-        // lout << "DEST: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
-
-        ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; 
-        //lout << "Vcalc: " << dest.Zd() << " " << dest.Az() << endl;
-        vcalc *= kGearRatio2*4./60.; // [re/min]
-
-        float dtime = -1;
-        if (kFALSE /*fUseStarguider*/)
-            dtime = Starguider(sla.GetMjd(), dest);
-
-        if (dtime<0)
-        {
-            dest = fBending(dest);       // [rad]
-
-            //lout << "DEST-BEND: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
-              
-            if (!CheckRange(dest))
-                break;
-
-            dest *= 16384/TMath::Pi()/2; // [se]
-            dest *= kGearRatio;          // [re]
-
-            //
-            // Request absolute position of rotary encoder from Macs
-            //
-            if (!RequestRePos())
-                break;
-
-            //
-            // distance between (To+dt) and To [re]
-            // position time difference < 5usec
-            // fOffset does the synchronization between the
-            // Shaft- and the rotary encoders
-            dest -= GetRePos() + fOffset;
-
-            dtime = dt;
-
-            ZdAz repos = 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" <<endl;
-            lout << "vt: " << vt.Zd() << " " << vt.Az() << "re/min" << endl;
-            lout << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
-        }              
-        vt.Round();
-
-        //
-        // check if the drive is fast enough to follow the star
-        //
-        if (vt.Zd()>.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 +480 @@
 }
 
-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 +519 @@
 
     case WM_STOP:
-        cout << "MCosy::Proc: Stop." << endl;
+        //cout << "MCosy::Proc: Stop." << endl;
         if (!CheckNetwork())
             return (void*)0xebb0;
@@ -1331 +569 @@
     case WM_TPOINT:
         {
-            cout << "WM_TPoint: start." << endl;
+            //cout << "WM_TPoint: start." << endl;
             SlaStars sla(fObservatory);
             sla.Now();
@@ -1340 +578 @@
             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 +631 @@
 
             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 +645 @@
             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 +657 @@
             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 +688 @@
             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 +738 @@
             */
         }
-        cout << "WM_Home: done. (return 0x403e)" << endl;
+        //cout << "WM_Home: done. (return 0x403e)" << endl;
         return (void*)0x403e;
 
@@ -1510 +763 @@
             a1 *= kRad2Deg;
 
-            const ZdAz a2 = a1*16384/360;
+            const ZdAz a2 = a1*kResSE/360;
 
             cout << "Zd/Az bended:  " << a1.Zd() << "° " << a1.Az() << "°" << endl;
@@ -1519 +772 @@
     case WM_ENDSWITCH:
         {
-            ZdAz pos = GetSePos()*TMath::Pi()*2/16384;
+            ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE;
             pos = fBending.SubtractOffsets(pos)*kRad2Deg;
 
@@ -1575 +828 @@
 
     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 +889 @@
     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 +941 @@
         }
 
-        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 +993 @@
         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 +1000 @@
             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 +1011 @@
             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 +1055 @@
         case kBgdNone:
             continue;
-
+/*#ifndef NEWALGO
         case kBgdTracking:
             TalkThreadTracking();
             continue;
-
+#endif*/
         case kBgdSeTest:
             TalkThreadSeTest();
@@ -1952 +1080 @@
 
     // 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 +1106 @@
         fAz->DisplayVal();
 
-    ZdAz bendist = GetPointingPos();
-
     Byte_t avail = 0;
 
@@ -1989 +1118 @@
         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 +1136 @@
         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 +1324 @@
     fAz =new ShaftEncoder(id6, "SE/Az",  lout);
 
+    fZd1->SetReport(fOutRep);
+    fZd2->SetReport(fOutRep);
+    fAz->SetReport(fOutRep);
+
     lout << "- Connecting devices to network." << endl;
 
@@ -2265 +1398 @@
 }
 */
+
+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 +1425 @@
     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 +1455 @@
     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 +1481 @@
 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 +1494 @@
 
     cout << "Deleting Nodes." << endl;
+
+    fZd1->SetReport(0);
+    fZd2->SetReport(0);
+    fAz->SetReport(0);
 
     delete fAz;

trunk/MagicSoft/Cosy/main/MCosy.h

@@ -8 +8 @@
 #include "MBending.h"
 
-#ifndef MARS_Mobservatory
+#ifndef MARS_MObservatory
 #include "MObservatory.h"
+#endif
+#ifndef ROOT_TMutex
+#include <TMutex.h>
 #endif
 
@@ -62 +65 @@
 {
     friend class MTTalk;
+    friend class MPointing;
+    friend class MTracking;
 
 private:
@@ -83 +88 @@
                           // with a generic interface to both...
 
+    TMutex fMutexGui;
+
     enum BackgroundAction_t
     {
         kBgdNone,
-        kBgdTracking,
+        //kBgdTracking,
         kBgdSeTest,
         kBgdSeTestDispl,
@@ -96 +103 @@
 
     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 +113 @@
     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 +122 @@
     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 +135 @@
     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 +148 @@
 
     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 +156 @@
 
     void StopMovement();
-    void StopTracking();
 
     void WaitForEndMovement();
@@ -188 +180 @@
     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));

trunk/MagicSoft/Cosy/main/MPointing.cc

@@ -74 +74 @@
     const int vr = mac->GetVelRes();
     mac->SetAcceleration(acc*vr);
-    mac->SetAcceleration(dec*vr);
+    mac->SetDeceleration(dec*vr);
     return !mac->IsZombieNode();
 }
@@ -96 +96 @@
 int MPointing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
 {
-    /*
     const ZdAz d = dst*kRad2Deg;
 
@@ -105 +104 @@
     // Calculate new target position (shortest distance to go)
     //
-    const ZdAz src = fCosy->GetSePos(); // [se]
+    //const ZdAz src = fCosy->GetSePos(); // [se]
 
     //
@@ -123 +122 @@
     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 +130 @@
     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 +166 @@
             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()<y)
-            {
-                lout << "--- LO-SPEED Mac1 ---" << endl;
-                SetAccDec(fCosy->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 +197 @@
         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()<y)
             {
@@ -409 +215 @@
         // 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;
@@ -416 +221 @@
         // 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);
@@ -429 +231 @@
         lout << t << " - Positioning done." << endl;
         fCosy->SetStatus(MDriveCom::kStopped);
-        fCosy->fCom->Send("POSITION DONE");
+        fCosy->fCom->SendStatus("Tracking preposition reached.");
         return TRUE;
     }
@@ -441 +243 @@
     lout << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl;
 
-    fCosy->fCom->Send("POSITION FAILED");
+    fCosy->fCom->SendStatus("Target position missed!");
 
     return FALSE;

trunk/MagicSoft/Cosy/main/MTracking.cc

@@ -13 +13 @@
 //#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 +85 @@
     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 +135 @@
 // 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 +172 @@
     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" <<endl;
         rc=kTRUE;
     }
-    if (fabs(vt->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" <<endl;
         rc=kTRUE;
     }
@@ -146 +237 @@
     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 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 +260 @@
     // 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 +267 @@
     // Estimate Offset before starting to track
     //
-    fCosy->fOffset = sepos-fCosy->GetRePos();
+    fOffset = sepos-fCosy->GetRePos();
 
     /*
@@ -202 +284 @@
     }
 
-    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 +290 @@
     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=" <<xy.X() << "h Dec=";
     lout << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;
-
-//---    ofstream fout("log/cosy.pos");
-//---    fout << "Tracking:";
-//---    fout << " Ra: " << Rad2Deg(dst.Ra())  << "\x9c  ";
-//---    fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
-//---    fout << "     Mjd/10ms    V/re/min/4" << endl;
 
     //
@@ -240 +307 @@
     {
         //
-        // Request Target position for this moment
+        // Request Target position for Now+dt
         //
         sla.Now(dt);
 
         //
-        // Request theoretical Position for a time in the future (To+dt) from CPU
-        //
-        const ZdAz pointing = sla.CalcZdAz(fCosy->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 << " " <<dest.Az()*kRad2Deg << endl;
-
-        ZdAz vcalc = sla.GetApproxVel(fCosy->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 +324 @@
         //    dtime = Starguider(sla.GetMjd(), dest);
 
+        ZdAz repos;
         if (dtime<0)
         {
-            dest = fCosy->fBending(dest);       // [rad]
-
-            //lout << "DEST-BEND: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
-              
+            dest = fCosy->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 +348 @@
             // 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 +358 @@
         // 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 +381 @@
         // 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" <<endl;
-            lout << "vt: " << vt.Zd() << " " << vt.Az() << "re/min" << endl;
-            lout << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
-        }              
+        ZdAz vt = v/4; //[re'/min]
+        //lout << " " << vt.Zd() << " " << vt.Az() << " ";
         vt.Round();
+        //lout << " " << vt.Zd() << " " << vt.Az() << endl;
 
         //
@@ -337 +399 @@
         // Maybe we should attenuate the changes
         //
-        *fCosy->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 +422 @@
         // 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 +437 @@
     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 +455 @@
     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 +473 @@
     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 +492 @@
         } 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 +509 @@
             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 --- */
     }
 

trunk/MagicSoft/Cosy/main/MTracking.h

@@ -1 +1 @@
 #ifndef COSY_MTracking
 #define COSY_MTracking
+
+#include "coord.h"
 
 #ifndef COSY_MPointing
@@ -10 +12 @@
 #endif
 
+class SlaStars;
+
 class MTracking : public MPointing, public MThread
 {
@@ -16 +20 @@
     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();