Index: trunk/MagicSoft/Cosy/MCosy.cc
===================================================================
--- trunk/MagicSoft/Cosy/MCosy.cc	(revision 1393)
+++ 	(revision )
@@ -1,930 +1,0 @@
-#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
-
-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/D2PI*rad;
-}
-
-double Rad2ZdRE(double rad)
-{
-    return 16384.0/D2PI*rad*kGearRatio.X();
-}
-
-double Rad2AzRE(double rad)
-{
-    return 16384.0/D2PI*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 * 360.0/D2PI;
-
-    if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
-        return dst*(16384.0/D2PI);
-
-    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);
-}
-
-
-ZdAz MCosy::GetSePos()
-{
-    const int p0 = fAlt1->GetPos();
-    const int p1 = fAlt2->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);
-}
-
-ZdAz MCosy::GetRePos()
-{
-    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
-}
-
-ZdAz MCosy::GetRePosPdo()
-{
-    return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
-}
-
-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);
-    }
-}
-
-void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
-{
-    SetStatus(kMoving);
-
-    if (axe1) fMac2->StartRelPos(rd.Zd());
-    if (axe2) fMac1->StartRelPos(rd.Az());
-
-    cout << "Waiting for positioning..." << flush;
-
-    WaitForEndMovement();
-
-    cout << "done." << endl;
-}
-
-void MCosy::CheckForError()
-{
-    if (!HasError())
-    {
-        SetStatus(kStopped);
-        return;
-    }
-
-    SetStatus(kError);
-    fMac1->HandleError();
-    fMac2->HandleError();
-    if (HasError())
-        return;
-
-    SetStatus(kStopped);
-}
-
-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 with " << i << "manuvers." << endl;
-            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;
-}
-
-Bool_t MCosy::RequestRePos()
-{
-
-    fMac2->RequestSDO(0x6004);
-    fMac1->RequestSDO(0x6004);
-    WaitForSdos();
-    if (!StopWaitingForSDO())
-        return kTRUE;
-
-    if (HasError())
-        lout << "Error #6004 (requesting re pos from Macs) happened." << endl;
-
-    return kFALSE;
-}
-
-Bool_t MCosy::SetVelocity(ZdAz v)
-{
-    fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
-    fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]
-    WaitForSdos();
-    if (!StopWaitingForSDO())
-        return kTRUE;
-
-    if (HasError())
-        lout << "Error #3006 (setting velocity of Macs) happened." << endl;
-
-    return kFALSE;
-}
-
-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(kMoving | kTracking);
-
-    fMac2->SetRpmMode(TRUE);
-    fMac1->SetRpmMode(TRUE);
-}
-
-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()) &&
-        fabs(vt->Az()) < limit*fabs(vcalc.Az()))
-        vt->Az(0);
-    else
-        if (fabs(vt->Az()) > 0.9*vraz)
-            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)
-            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("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())
-    {
-        //
-        // 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(15) << 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;
-}
-
-int MCosy::IsPositioning() const
-{
-    return (fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO();
-}
-
-void MCosy::WaitForEndMovement()
-{
-    WaitForSdos();
-
-    while (IsPositioning())
-        usleep(1);
-}
-
-void MCosy::StopMovement()
-{
-    SetStatus(kStopping);
-
-    cout << "Stopping  positioning..." << endl;
-    fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
-    fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
-
-    cout << "Stoping possible RPM mode..." << endl;
-    fMac1->SetRpmMode(FALSE);
-    fMac2->SetRpmMode(FALSE);
-
-    cout << "Waiting for silence..." << endl;
-    WaitForEndMovement();
-
-    CheckForError();
-
-    cout << "Movement stopped." << endl;
-}
-
-void *MCosy::Proc(int msg, void *mp)
-{
-    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;
-        fAlt1->SetPreset();
-        fAlt2->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*D2PI/360.0)*16384.0/D2PI;
-
-            cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
-
-            ZdAz sepos = GetSePos();
-            cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
-
-            fAlt1->SetPreset(za.Zd());
-            fAlt2->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*D2PI/360.0);
-        }
-        cout << "WM_Position: done. (return 0x7777)" << endl;
-        return (void*)0x7777;
-
-    case WM_TRACK:
-        cout << "WM_Track: START" << endl;
-        {
-            RaDec dest = *((RaDec*)mp);
-            TrackPosition(dest*D2PI/360.0);
-        }
-        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;
-
-/*
-    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 && !fTTalk->HasStopFlag())
-            usleep(1);
-
-        if (fTTalk->HasStopFlag())
-            break;
-
-        ofstream fout("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
-        //
-        while (fTracking && !fTTalk->HasStopFlag())
-        {
-            //
-            // Make changes (eg wind) smoother - attenuation of control function
-            //
-            const float weight = 0.3;
-
-            ZdAz offset(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight,
-                        fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
-
-            fOffset = offset;
-
-            //
-            // This is the time constant which defines how fast
-            // you correct for external influences (like wind)
-            //
-            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((fAlt1->GetMjd()+fAlt2->GetMjd())/2.0);
-            time.Az(fAz->GetMjd());
-
-            //
-            // if Shaftencoder changed position
-            // calculate were we should be
-            //
-            if ((int)ist.Zd() != (int)old.Zd())
-            {
-                sla.SetMjd(time.Zd());
-                sollzd = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]
-            }
-
-            if ((int)ist.Az() != (int)old.Az())
-            {
-                sla.SetMjd(time.Az());
-                sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]
-            }
-
-            fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
-                               (ist.Az()-sollaz.Az())*kGearRatio.Y());
-
-            fout << setprecision(15) << setw(15) << time.Zd()*60.*60.*24. << " ";
-            fout << setprecision(5)  << setw(5)  << fTrackingError.Zd() << "  ";
-            fout << setprecision(15) << setw(15) << time.Az()*60.*60.*24. << " ";
-            fout << setprecision(5)  << setw(5)  << fTrackingError.Az() << endl;
-        }
-
-        fout << endl << endl;
-    }
-}
-
-Bool_t MCosy::HandleTimer(TTimer *t)
-{
-    //
-    // Update Gui, foremer MTGui.
-    //
-    fAlt1->DisplayVal();
-    fAlt2->DisplayVal();
-    fAz->DisplayVal();
-
-    ZdAz ist = GetSePos()*(360.0/16384.0); // [se]
-
-    fWin->Update(ist, fTrackingError/kGearRatio2,
-                 fVelocity, fOffset/*/kGearRatio2*/,
-                 fStatus);
-
-    return kTRUE;
-}
-
-
-int MCosy::StopWaitingForSDO() const
-{
-    return Break() || HasError();
-}
-
-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);
-}
-
-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
-    //
-    fMac1=new Macs(1, lout);
-    fMac2=new Macs(2, lout);
-    fMac3=new Macs(3, lout);
-    fAlt1=new ShaftEncoder(4, lout);
-    fAlt2=new ShaftEncoder(5, lout);
-    fAz  =new ShaftEncoder(6, lout);
-
-    //
-    // Connect the devices to the network
-    //
-    SetNode(fMac1);
-    SetNode(fMac2);
-    SetNode(fMac3);
-    SetNode(fAlt1);
-    SetNode(fAlt2);
-    SetNode(fAz);
-
-    //
-    // Create Gui Event timer and Gui
-    //
-    fUpdateGui = new TTimer(this, 100); // 100ms
-
-    fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
-
-    fAz->SetDisplay(fWin->GetLabel1());
-    fAlt1->SetDisplay(fWin->GetLabel2());
-    fAlt2->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);
-*/
-
-    gApplication->Terminate(0);
-}
-
-MCosy::~MCosy()
-{
-    cout << "Deleting GUI timer." << endl;
-
-    delete fUpdateGui;
-
-    cout << "Deleting Nodes." << endl;
-
-    delete fAz;
-    delete fAlt2;
-    delete fAlt1;
-    delete fMac1;
-    delete fMac2;
-    delete fMac3;
-
-    cout << "Deleting MGCosy." << endl;
-
-    lout.DisableOutputDevice(MLog::eGui);
-
-    delete fWin;
-
-    cout << "MGCosy destructed." << endl;
-}
Index: trunk/MagicSoft/Cosy/MCosy.h
===================================================================
--- trunk/MagicSoft/Cosy/MCosy.h	(revision 1393)
+++ 	(revision )
@@ -1,123 +1,0 @@
-#ifndef MCOSY_H
-#define MCOSY_H
-
-#include "coord.h"
-#include "msgqueue.h"
-#include "network.h"
-#include "MThread.h"
-
-#define kDEG ((char)0x9c)  // Linux 'ø'
-
-#define WM_WAIT      WM_NULL
-#define WM_PRESET    0x1000
-#define WM_POSITION  0x1001
-#define WM_TRACK     0x1002
-#define WM_STOP      0x1003
-#define WM_POLARIS   0x1004
-#define WM_QUIT      0x1005
-
-class ShaftEncoder;
-class Macs;
-class MGCosy;
-class MCosy;
-
-class MTTalk : public MThread
-{
-private:
-    MCosy *fCosy;
-
-    void *Thread();
-
-public:
-    MTTalk(MCosy *cosy) : MThread(false), fCosy(cosy)
-    {
-        SetPriority(10);
-        Start();
-    }
-};
-
-#define kError     0x01
-#define kMoving    0x02
-#define kTracking  0x04
-#define kStopping  0x08
-#define kStopped   0x10
-
-class TTimer;
-class MCosy : public Network, public MsgQueue, public TObject
-{
-    friend class MTGui;
-    friend class MTTalk;
-
-private:
-    ShaftEncoder *fAlt1;
-    ShaftEncoder *fAlt2;
-    ShaftEncoder *fAz;
-
-    Macs *fMac1;
-    Macs *fMac2;
-    Macs *fMac3;
-
-    MGCosy *fWin;
-
-    TTimer *fUpdateGui;
-    MTTalk *fTTalk;       // should be outsourced, like the starguider.
-                          // with a generic interface to both...
-
-    ZdAz  fTrackingError; // Tracking Offset between SE and calc-pos [re]
-    ZdAz  fOffset;        // Offset between se and re coordinate system [re]
-    RaDec fRaDec;         // Position to track
-    int   fTracking;      // Flag for present tracking action
-    ZdAz  fAccuracy;      // Actual accuracy of Tracking
-    ZdAz  fVelocity;      // Actual velocity of Tracking
-
-    UInt_t fStatus;
-
-    void SetStatus(UInt_t stat) { fStatus = stat; }
-    UInt_t GetStatus() const { return fStatus; }
-
-    ZdAz GetRePos();
-    ZdAz GetRePosPdo();
-    ZdAz GetSePos();     // [se]
-
-    Bool_t RequestRePos();
-    Bool_t SetVelocity(ZdAz v);
-    void SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc);
-
-    void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
-
-    void InitTracking();
-    void LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const;
-
-    void TalkThread();
-    void GuiThread(MTGui *t);
-
-    int  SetPosition(const ZdAz &dst);
-
-    void TerminateApp();
-
-    void TrackPosition(const RaDec &dst); // ra, dec [rad]
-
-    int StopWaitingForSDO() const;
-    int IsPositioning() const;
-    void CheckForError();
-
-    void StopMovement();
-    void WaitForEndMovement();
-
-public:
-    MCosy(const char *dev, const int baud, MLog &out=gLog);
-    ~MCosy();
-
-    void Start();
-    void Stop();
-
-    void *Proc(int msg, void *mp);
-
-    Bool_t HandleTimer(TTimer *t);
-
-    static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst);
-//    static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0));
-
-};
-
-#endif
