source: trunk/MagicSoft/Cosy/main/MCosy.cc@ 1275

#include "MCosy.h"

#include <iomanip.h>
#include <fstream.h>
#include <iostream.h>

#include <TROOT.h>
#include <TEnv.h>
#include <TSystem.h>
#include <TApplication.h>
#include <TTimer.h>

#include "MGCosy.h"
#include "SlaStars.h"

#include "slalib/slalib.h"  // FIXME: REMOVE

#include "macs.h"
#include "base/timer.h"
#include "shaftencoder.h"

//#include <sys/resource.h>  // PRIO_PROCESS

ClassImp(MCosy);

typedef struct tm tm_t;

#define GEAR_RATIO_ALT  75.55 // 75.25 VERY IMPORTANT! unit=RE/SE
#define GEAR_RATIO_AZ  179.8  // VERY IMPORTANT! unit=RE/SE

const XY kGearRatio(GEAR_RATIO_ALT, GEAR_RATIO_AZ);
const XY kGearRatio2(GEAR_RATIO_ALT*16384.0/360.0, GEAR_RATIO_AZ*16384.0/360.0);

double Rad2SE(double rad)
{
    return 16384.0/k2Pi*rad;
}

double Rad2ZdRE(double rad)
{
    return 16384.0/k2Pi*rad*kGearRatio.X();
}

double Rad2AzRE(double rad)
{
    return 16384.0/k2Pi*rad*kGearRatio.Y();
}

double Deg2ZdRE(double rad)
{
    return rad*kGearRatio2.X();
}

double Deg2AzRE(double rad)
{
    return rad*kGearRatio2.Y();
}

ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
{
    // CorrectTarget [se]

    // src [se]
    // dst [rad]

    // fAltMax = 70
    // fAltMin = -105/110
    // fAzMin = -355
    // fAzMax =  355

    ZdAz source = src * 360.0/16384.0;
    ZdAz dest   = dst * kRad2Deg;

    if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
        return dst*(16384.0/k2Pi);

    const float fZdMin = -67;
    const float fZdMax =  67;
    const float fAzMin = -29;
    const float fAzMax = 423;

    //
    // This corrects to target for the shortest distance, not for the fastest move!
    //
    ZdAz s = source-dest;

    float min = s.Sqr();

    //
    // Is it enought to search inside one revolution?
    //
    ZdAz ret = dest;

    for (int i=-5; i<5+1; i++)
    {
        const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);

        //
        // Range Check
        //
        if (p.Zd()<fZdMin || p.Zd()>fZdMax)
            continue;

        if (p.Az()<fAzMin || p.Az()>fAzMax)
            continue;

        //
        // Calculate distance
        //
        s = source-p;

        const float dist = s.Sqr();

        if (dist > min)
            continue;

        //
        // New shortest distance
        //
        ret = p;
        min = dist;
    }
    return ret*(16384.0/360.0);
}

// --------------------------------------------------------------------------
//
//  GetSePos, reads the Shaftencoder positions from the Can-drivers
//  for the shaftencoders. The two shaft encoders at the elevation axis
//  are avaraged. The values are returned as a ZdAz object.
//
//  The positions are alway up-to-date because the shaftencoders are
//  sending all changes immediatly.
//
ZdAz MCosy::GetSePos()
{
    //
    // Get the values
    //
    const int p0 = fZd1->GetPos();
    const int p1 = fZd2->GetPos();
    const int p2 = fAz->GetPos();

    const int a0 = p0; //p0>8192?p0-16384:p0;
    const int a1 = p1; //p1>8192?p1-16384:p1;
    const int a2 = p2; //p2>8192?p2-16384:p2;

    //
    // interpolate shaft encoder positions
    //
    const float a = (float)(a0-a1)/2;

    //
    // calculate 'regelabweichung'
    //
    return ZdAz(a, a2);
}

// --------------------------------------------------------------------------
//
// 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.
    //
    WaitForSdos();

    //
    // If waitng was not interrupted everything is ok. return.
    //
    if (!StopWaitingForSDO())
        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.
//
//  The positions are returned as a ZdAz object. Use RequestRePos to request
//  the current positions first.
//
ZdAz MCosy::GetRePos()
{
    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
}

// --------------------------------------------------------------------------
//
//  reads the Rotary encoder positions from the Macs.
//
//  The positions are returned as a ZdAz object. The positions are the ones
//  which are send as PDOs to the computer. This is done at a given
//  frequency. Which means, that this positions are not ought to be
//  up-to-date.
//
ZdAz MCosy::GetRePosPdo()
{
    return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
}

// --------------------------------------------------------------------------
//
//  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, Float_t acc)
{
    //
    // Set velocities
    //
    const int vr = fMac1->GetVelRes();

    vel *= vr;
    acc *= vr;

    if (ratio <1)
    {
        fMac1->SetVelocity(vel);
        fMac1->SetAcceleration(acc);
        fMac1->SetDeceleration(acc);

        fMac2->SetVelocity(vel*ratio);
        fMac2->SetAcceleration(acc*ratio);
        fMac2->SetDeceleration(acc*ratio);
    }
    else
    {
        fMac1->SetVelocity(vel/ratio);
        fMac1->SetAcceleration(acc/ratio);
        fMac1->SetDeceleration(acc/ratio);

        fMac2->SetVelocity(vel);
        fMac2->SetAcceleration(acc);
        fMac2->SetDeceleration(acc);
    }
}

// --------------------------------------------------------------------------
//
// 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)
{
    SetStatus(MCosy::kMoving);

    if (axe1) fMac2->StartRelPos(rd.Zd());
    if (axe2) fMac1->StartRelPos(rd.Az());

    cout << "Waiting for positioning..." << flush;

    WaitForEndMovement();

    cout << "done." << endl;
}

// --------------------------------------------------------------------------
//
// check for a break-signal (from the msgqueue) and errors.
//
int MCosy::StopWaitingForSDO() const
{
    return Break() || HasError();
}

// --------------------------------------------------------------------------
//
// Waits for a movement to become finished.
//
// First waits for all peding Sdos, then waits until both motors are stopped
// or waiting for SDOs was stopped (either by an error or by Break)
//
void MCosy::WaitForEndMovement()
{
    WaitForSdos();

    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO())
        usleep(1);
}

// --------------------------------------------------------------------------
//
// Check for an error...
//
// This is ment for usage after the Action: All Motors Stop.
//
void MCosy::CheckForError()
{
    //
    // Check all Can-Nodes for an Error. If there is no error the motor
    // status is set to stopped.
    //
    if (!HasError())
    {
        SetStatus(MCosy::kStopped);
        return;
    }

    //
    // If there is an error, the error status is set to Error.
    //
    SetStatus(MCosy::kError);

    //
    // No try to handle the error.
    //
    fMac1->HandleError();
    fMac2->HandleError();

    //
    // If the error couldn't get solved return
    //
    if (HasError())
        return;

    //
    // Set motor status to stopped
    //
    SetStatus(MCosy::kStopped);
}

// --------------------------------------------------------------------------
//
// 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) // [rad]
{
    // FIXME: Correct by fOffset ?

    //
    // Calculate new target position (shortest distance to go)
    //
    const ZdAz src  = GetSePos();
    const ZdAz dest = CorrectTarget(src, dst);

    lout << "Positioning to Target..." << 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 << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;

    for (int i=0; i<10 && !StopWaitingForSDO(); i++)
    {
        //
        // Get Shaft Encoder Positions
        //
        const ZdAz p=GetSePos();

        //
        // calculate control deviation and rounded cd
        //
        ZdAz rd = dest-p; // [se]

        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)
        {
            lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl;
            SetStatus(MCosy::kStopped);
            return TRUE;
        }

        //
        // change units from se to re
        //
        rd *= kGearRatio; // [re]

        //
        // Initialize Velocities so that we reach both positions
        // at the same time
        //
        if (i)
            SetPosVelocity(1.0, 0.1, 0.1);
        else
            SetPosVelocity(fabs(rd.Ratio()), 0.9, 0.5);

        rd.Round();

        /*
         cout << " + " << cdzd << " " << 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;
         */

        //
        // repositioning (relative)
        //
        DoRelPos(rd, cdzd, cdaz);
    }

    StopMovement();
    lout << "Warning: Requested position not reached." << 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(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 OKed.
    //
    WaitForSdos();

    //
    // If the waiting for the objects wasn't interrupted return kTRUE
    //
    if (!StopWaitingForSDO())
        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.
//
void MCosy::InitTracking()
{
    //
    // Start revolution mode
    //
    fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
    fMac2->SetDeceleration(0.90*fMac2->GetVelRes());

    fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
    fMac1->SetDeceleration(0.90*fMac1->GetVelRes());

    SetStatus(MCosy::kMoving | MCosy::kTracking);

    fMac2->SetRpmMode(TRUE);
    fMac1->SetRpmMode(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.
//
void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
{
    //
    // 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)

    const Float_t limit = 0.25;
/*
    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())
       )
        vt->Az(0);
    else
        if (fabs(vt->Az()) > 0.9*vraz)
        {
            lout << "Warning: Azimuth speed limit exceeded. Limiting speed to 90% max." << endl;
            vt->Az(0.9*vraz*sgn(vt->Az()));
        }

    if (sgn(vt->Zd()) != sgn(vcalc.Zd())
        && fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
       )
        vt->Zd(0);
    else
        if (fabs(vt->Zd()) > 0.9*vrzd)
        {
            lout << "Warning: Altitude speed limit exceeded. Limiting speed to 90% max." << endl;
            vt->Zd(0.9*vrzd*sgn(vt->Zd()));
        }
}

void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
{
    SlaStars sla;

    //
    // Position to actual position
    //
    sla.SetMjd2Now();
    ZdAz dest = sla.CalcZdAz(dst);

    if (!SetPosition(dest))
    {
        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
    //
    InitTracking();

    lout << "Start tracking:";
    lout << " Ra: " << Rad2Deg(dst.Ra())  << "\xb0  ";
    lout << "Dec: " << Rad2Deg(dst.Dec()) << "\xb0" << endl;

    //
    // Initialize Tracker (slalib or starguider)
    //
    fRaDec    = dst;
    fTracking = kTRUE;

    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
    //
    const float dt = 1;  // 1 second
    while (!StopWaitingForSDO())
    {
        //
        // 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)
        //
        //usleep(100000000);

        //
        // Request Target position for this moment
        //
        sla.Now();

        //
        // Request theoretical Position for a time in the future (To+dt) from CPU
        //
        sla.SetMjd(sla.CalcMjd()+dt/(60*60*24));
        dest = CorrectTarget(GetSePos(), sla.CalcZdAz(dst)); // [se]

        ZdAz vcalc = sla.GetApproxVel(dst) * kGearRatio2*4./60.;  // [re/min]

        //
        // 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 *= kGearRatio;  // [re]
        dest -= GetRePos() + fOffset;

        //
        // 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/(dt-(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;
        LimitSpeed(&vt, vcalc);
        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
        //
        // usleep(50000); // 0.05s
    }

    fTracking = kFALSE;
    StopMovement();
    lout << "Tracking stopped." << endl;
}

// --------------------------------------------------------------------------
//
// Stops the movement of both motors.
//
// Sets the status to stopping. Sets the deceleration to 50% of the maximum.
// stops. Quits the revolution mode and wait for the end of the movement.
//
void MCosy::StopMovement()
{
    //
    // Set status to stopped
    //
    SetStatus(MCosy::kStopping);

    //
    // set deceleration to 50%
    //
    cout << "Stopping  positioning..." << endl;
    fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
    fMac2->SetDeceleration(0.5*fMac2->GetVelRes());

    //
    // Stop revolution mode (movement)
    //
    cout << "Stopping possibleRPM mode..." << endl;
    fMac1->SetRpmMode(FALSE);
    fMac2->SetRpmMode(FALSE);

    //
    // Wait for the movement to really be finished.
    //
    cout << "Waiting for silence..." << endl;
    WaitForEndMovement();

    //
    // Check whether everything works fine.
    //
    CheckForError();
    cout << "Movement stopped." << endl;
}

void *MCosy::Proc(int msg, void *mp)
{
    cout << "MCosy::Proc: 0x" << msg << endl;
    switch (msg)
    {
    case WM_WAIT:
        cout << "Wait for execution of Proc(WM_*, ): done." << endl;
        return NULL;

    case WM_STOP:
        StopMovement();
        return NULL;

    case WM_PRESET:
        cout << "WM_Preset: start." << endl;
        fZd1->SetPreset();
        fZd2->SetPreset();
        fAz->SetPreset();
        cout << "WM_Preset: done. (return 0xaffe)" << endl;
        return (void*)0xaffe;

    case WM_POLARIS:
        {
            cout << "WM_Polaris: start." << endl;
            SlaStars sla;
            sla.SetMjd2Now();

            RaDec rd(37.94, 89.2644);
            ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;

            cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;

            ZdAz sepos = GetSePos();
            cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;

            fZd1->SetPreset(za.Zd());
            fZd2->SetPreset(-za.Zd());
            fAz->SetPreset(za.Az());

            cout << "WM_Polaris: done. (return 0xaffe)" << endl;
        }
        return (void*)0xaffe;

    case WM_POSITION:
        cout << "WM_Position: start." << endl;
        {
            ZdAz dest = *((ZdAz*)mp);

            SetPosition(dest*kDeg2Rad);
        }
        cout << "WM_Position: done. (return 0x7777)" << endl;
        return (void*)0x7777;

    case WM_TRACK:
        cout << "WM_Track: START" << endl;
        {
            RaDec dest = *((RaDec*)mp);
            TrackPosition(dest*kDeg2Rad);
        }
        cout << "WM_Track: done. (return 0x8888)" << endl;
        return (void*)0x8888;

    case WM_QUIT:
        cout << "WM_Quit: now." << endl;
        TerminateApp();
        cout << "WM_Quit: done." << endl;
        return (void*)0x9999;
    }
    cout << "Unknown Msg" << endl;
    return (void*)0xffffffff;
}

void *MTTalk::Thread()
{
    fCosy->TalkThread();
    return NULL;
}

void MCosy::TalkThread()
{
    //
    // Start the Network
    //
    cout << "Reading configuration file..." << flush;
    TEnv env(".cosyrc");
    cout << "done." << endl;

    const int res = fMac3->GetVelRes();

    fMac3->SetVelocity(res);
    fMac3->SetAcceleration(res);
    fMac3->SetDeceleration(res);

    fMac3->StartPosSync();

    cout << "Going Home..." << endl;
    fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
    fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
    PostMsg(WM_PRESET, 0, 0);
    PostMsg(WM_WAIT,   0, 0);

    fMac1->ReqPos();
    fMac2->ReqPos();

    //const ZdAz repos=GetRePos();
    //cout << "APOS: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;

    /*
     cout << Deg2AzRE(env.GetValue("MinAz[Deg]", -1.0)) << " < Az < "
     << Deg2AzRE(env.GetValue("MaxAz[Deg]", +1.0)) << "RE" << endl;
     cout << env.GetValue("MinAz[Deg]", -1.0) << " < Az < "
     << env.GetValue("MaxAz[Deg]", +1.0) << kDEG << endl;
     cout << Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)) << "RE < Zd < "
     << Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)) << "RE" << endl;
     */


    cout << "Setting up software endswitch..." << flush;
    fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0)));
    fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0)));

    fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0)));
    fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0)));
    cout << "done." << endl;

    fMac1->EnableTimeout(kTRUE, 500);
    fMac2->EnableTimeout(kTRUE, 500);

/*
    fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)));
    fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)));
*/
//    fMac3->StartVelSync();
/*
    cout << "PostMsg(WM_PRESET)" << endl;
    void *rc =
    cout << hex << "WM_PRESET: ret=" << rc << endl;

    RaDec dest = RaDec(45.0, 30.0)*D2PI/360.0;

    cout << "PostMsg(WM_TRACK)" << endl;
    cout << sizeof(RaDec) << "==" << sizeof(dest) << endl;
    rc=PostMsg(WM_TRACK, &dest, sizeof(dest));
    cout << "DEST killed." << endl;
*/
    // AltAz dest = AltAz(45.0, 30.0);
    // double ra, dec;
    // slaDaf2r( 71, 0, 0, &ra,  &status); // 0 WARNING: RANGE
    // slaDaf2r( 89, 0, 0, &dec, &status); // 49
    // cout << "Start tracking: Ra: " << Rad2Deg(ra) << kDEG << "  Dec: " << Rad2Deg(dec) << kDEG << endl;

    // dest = AltAz(-46.0, 210);
    // SetPosition(dest);

    SlaStars sla;
    while (1)
    {
        //
        // wait until a tracking session is started
        //
        while (!fTracking)
            usleep(1);

        ofstream fout("log/cosy.err");
        fout << "Tracking:";
        fout << " Ra: " << Rad2Deg(fRaDec.Ra())  << "\x9c  ";
        fout << "Dec: " << Rad2Deg(fRaDec.Dec()) << "\x9c" << endl << endl;
        fout << "     MjdZd/10ms    ErrZd/re";
        fout << "     MjdAz/10ms    ErrAd/re" << endl;

        ZdAz old;
        ZdAz ist;

        ZdAz sollzd;
        ZdAz sollaz;

        ZdAz istre = -fOffset;                // [re]
        ZdAz time;

        //
        // only update fTrackingError while tracking
        //
        bool phca1=false;
        bool phca2=false;
        bool phcaz=false;

        while (fTracking)
        {
            //
            // 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 && fTracking);

            //---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;
            */
            istre = GetRePosPdo();

            //
            // Get time from last shaftencoder position change (position: ist)
            // FIXME: I cannot take the avarage
            //
            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()*/)
            {
                sla.SetMjd(time.Zd());
                sollzd = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]

                fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight);
            }

            if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
            {
                sla.SetMjd(time.Az());
                sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]

                fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
            }

            fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
                               (ist.Az()-sollaz.Az())*kGearRatio.Y());

            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;
        }

        fout << endl << endl;
    }
}

Bool_t MCosy::HandleTimer(TTimer *t)
{
    //
    // Update Gui, foremer MTGui.
    //
    fZd1->DisplayVal();
    fZd2->DisplayVal();
    fAz->DisplayVal();

    ZdAz ist = GetSePos()*(360.0/16384.0); // [se]

    fWin->Update(ist, fTrackingError/kGearRatio2,
                 fVelocity, fOffset/*/kGearRatio2*/,
                 fStatus);

    return kTRUE;
}


// --------------------------------------------------------------------------
//
// Start the work of the application:
//
// Start the Can-Network.
// Start the MCosy::TalkThread thread.
// turn on the gui update
//
void MCosy::Start()
{
    // Don't call this function twice!
    Network::Start();

    lout << "- Starting TX Thread." << endl;
    fTTalk = new MTTalk(this);

    lout << "- Starting GUI update." << endl;
    fUpdateGui->TurnOn();
    //    fTGui = new MTGui(this);
}

// --------------------------------------------------------------------------
//
// Start the work of the application:
//
// Turn of the gui update
// stop the MCosy::TalkThread thread.
// Stop the network
//
void MCosy::Stop()
{

    lout << "- Stopping GUI update." << endl;
    fUpdateGui->TurnOff();
    lout << "- GUI Update stopped." << endl;

    delete fTTalk;
    lout << "- TX Thread stopped." << endl;

    Network::Stop();
}

MCosy::MCosy(const char *dev, const int baud, MLog &out)
: Network(dev, baud, out), fTracking(kFALSE)
{
    //
    // Create Nodes
    //
    lout << "- Setting up network." << endl;

    fMac1=new Macs(1, "Mac.1/Az",      lout);
    fMac2=new Macs(2, "Mac.2/Zd",      lout);
    fMac3=new Macs(3, "Mac.3/Az-Sync", lout);

    fZd1=new ShaftEncoder(4, "SE.4/Zd1", lout);
    fZd2=new ShaftEncoder(5, "SE.5/Zd2", lout);
    fAz =new ShaftEncoder(6, "SE.6/Az",  lout);

    //
    // Connect the devices to the network
    //
    SetNode(fMac1);
    SetNode(fMac2);
    SetNode(fMac3);
    SetNode(fZd1);
    SetNode(fZd2);
    SetNode(fAz);

    //
    // Create Gui Event timer and Gui
    //
    lout << "- Initializing GUI Timer." << endl;
    fUpdateGui = new TTimer(this, 100); // 100ms

    lout << "- Starting GUI." << endl;
    fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);

    fAz->SetDisplay(fWin->GetLabel1());
    fZd1->SetDisplay(fWin->GetLabel2());
    fZd2->SetDisplay(fWin->GetLabel3());

    lout.SetOutputGui(fWin->GetLog(), kTRUE);
}

void MCosy::TerminateApp()
{
    cout << "MCosy::TerminateApp()" << endl;
/*
    Int_t rc;
    TGMessageBox msg(this, gClient->GetRoot(),
                     "Information",
                     "Cosy is shutting down the system - this may take wa while!",
                     kMBIconExclamation,
                     kMBOK, //kMBClose
                     &rc, 0);
*/

    lout.DisableOutputDevice(MLog::eGui);
    lout.SetOutputGui(NULL, kFALSE);

    gApplication->Terminate(0);
}

MCosy::~MCosy()
{
    cout << "Deleting GUI timer." << endl;

    delete fUpdateGui;

    cout << "Deleting Nodes." << endl;

    delete fAz;
    delete fZd2;
    delete fZd1;
    delete fMac1;
    delete fMac2;
    delete fMac3;

    cout << "Deleting MGCosy." << endl;

    lout.DisableOutputDevice(MLog::eGui);

    delete fWin;

    cout << "MGCosy destructed." << endl;
}