Index: trunk/MagicSoft/Cosy/main/MCosy.cc
===================================================================
--- trunk/MagicSoft/Cosy/main/MCosy.cc	(revision 1531)
+++ trunk/MagicSoft/Cosy/main/MCosy.cc	(revision 1691)
@@ -26,31 +26,40 @@
 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)
+/*
+#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]
+
+double MCosy::Rad2SE(double rad) const
 {
     return 16384.0/k2Pi*rad;
 }
 
-double Rad2ZdRE(double rad)
+double MCosy::Rad2ZdRE(double rad) const
 {
     return 16384.0/k2Pi*rad*kGearRatio.X();
 }
 
-double Rad2AzRE(double rad)
+double MCosy::Rad2AzRE(double rad) const
 {
     return 16384.0/k2Pi*rad*kGearRatio.Y();
 }
 
-double Deg2ZdRE(double rad)
+double MCosy::Deg2ZdRE(double rad) const
 {
     return rad*kGearRatio2.X();
 }
 
-double Deg2AzRE(double rad)
+double MCosy::Deg2AzRE(double rad) const
 {
     return rad*kGearRatio2.Y();
@@ -72,4 +81,7 @@
     ZdAz dest   = dst * kRad2Deg;
 
+    if (dest.Zd()>-3 && dest.Zd()<3)
+        dest.Zd(dest.Zd()<0?-3:3);
+
     if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
         return dst*(16384.0/k2Pi);
@@ -138,7 +150,7 @@
     // Get the values
     //
-    const int p0 = fZd1->GetPos();
-    const int p1 = fZd2->GetPos();
-    const int p2 = fAz->GetPos();
+    const int p0 = fZd1 ? fZd1->GetPos() : 0;
+    const int p1 = fZd2 ? fZd2->GetPos() : 0;
+    const int p2 = fAz  ? fAz->GetPos()  : 0;
 
     const int a0 = p0; //p0>8192?p0-16384:p0;
@@ -165,4 +177,7 @@
 Bool_t MCosy::RequestRePos()
 {
+    if (!(fMac1 && fMac2))
+        return kTRUE;
+
     //
     // Send request
@@ -201,5 +216,5 @@
 ZdAz MCosy::GetRePos()
 {
-    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
+    return fMac1 && fMac2 ? ZdAz(fMac2->GetPos(), fMac1->GetPos()) : ZdAz(0,0);
 }
 
@@ -363,14 +378,27 @@
     // FIXME: Correct by fOffset ?
 
+    if (!(fMac1 && fMac2 && fMac3))
+    {
+        cout << "SetPosition: No MACS!" << endl;
+        return TRUE;
+    }
+
     //
     // Calculate new target position (shortest distance to go)
     //
-    const ZdAz src  = GetSePos();
-    const ZdAz dest = CorrectTarget(src, dst);
+    const ZdAz src = GetSePos();//*TMath::Pi()*2/16384;;
+
+    //
+    // Because we agreed on I don't search for the shortest move
+    // anymore
+    //
+    // const ZdAz dest = CorrectTarget(src, dst);
+    //
+    const ZdAz dest = fBending(dst)*16384/2/TMath::Pi();
 
     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;
+    cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
 
     for (int i=0; i<10 && !StopWaitingForSDO(); i++)
@@ -484,4 +512,7 @@
 void MCosy::InitTracking()
 {
+    if (!(fMac1 && fMac2))
+        return;
+
     //
     // Start revolution mode
@@ -608,19 +639,28 @@
     InitTracking();
 
+    XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
+
     lout << "Start tracking:";
-    lout << " Ra: " << Rad2Deg(dst.Ra())  << "\xb0  ";
-    lout << "Dec: " << Rad2Deg(dst.Dec()) << "\xb0" << endl;
+    lout << " Ra: " << xy.X() << "h  " << "Dec: " << xy.Y() << "\xb0" << endl;
+
+    ofstream fout("coordinates.txt");
+    fout << xy;
+    fout.close();
 
     //
     // Initialize Tracker (slalib or starguider)
     //
-    fRaDec    = dst;
+    fRaDec = dst;
+
+    if (!(fMac1 && fMac2 && fMac3))
+        return;
+
     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;
+//---    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;
 
     //
@@ -646,7 +686,6 @@
         //
         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]
+        ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
+        dest = CorrectTarget(GetSePos(), dummy); // [se]
 
         //
@@ -676,4 +715,5 @@
         //
         ZdAz vt = v/4;
+        ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.;  // [re/min]
         LimitSpeed(&vt, vcalc);
         vt.Round();
@@ -700,8 +740,8 @@
         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;
+//---        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?
@@ -716,5 +756,5 @@
         // update time
         //
-        // usleep(50000); // 0.05s
+        usleep(50000); // 0.05s
     }
 
@@ -733,4 +773,6 @@
 void MCosy::StopMovement()
 {
+    if (!fMac1 || !fMac2)
+        return;
     //
     // Set status to stopped
@@ -780,17 +822,23 @@
     case WM_PRESET:
         cout << "WM_Preset: start." << endl;
-        fZd1->SetPreset();
-        fZd2->SetPreset();
-        fAz->SetPreset();
+        if (fZd1) fZd1->SetPreset();
+        if (fZd2) fZd2->SetPreset();
+        if (fAz)  fAz->SetPreset();
         cout << "WM_Preset: done. (return 0xaffe)" << endl;
         return (void*)0xaffe;
 
-    case WM_POLARIS:
+    case WM_CALIB:
         {
-            cout << "WM_Polaris: start." << endl;
+            cout << "WM_Calib: start." << endl;
             SlaStars sla;
             sla.SetMjd2Now();
 
-            RaDec rd(37.94, 89.2644);
+            RaDec rd = *((RaDec*)mp);
+
+            //RaDec rd(37.94, 89.2644);      // POLARIS
+            //RaDec rd(213.915417, 19.1825); // ACTURUS
+
+            cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
+
             ZdAz za=sla.CalcZdAz(rd*kDeg2Rad)*16384.0/k2Pi;
 
@@ -800,11 +848,39 @@
             cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
 
-            fZd1->SetPreset(za.Zd());
-            fZd2->SetPreset(-za.Zd());
-            fAz->SetPreset(za.Az());
+            if (fZd1)
+                fZd1->SetPreset(za.Zd());
+            if(fZd2)
+                fZd2->SetPreset(-za.Zd());
+            if(fAz)
+                fAz->SetPreset(za.Az());
 
             cout << "WM_Polaris: done. (return 0xaffe)" << endl;
         }
         return (void*)0xaffe;
+
+    case WM_TPOINT:
+        {
+            cout << "WM_TPoint: start." << endl;
+            SlaStars sla;
+            sla.SetMjd2Now();
+
+            RaDec rd = *((RaDec*)mp);
+            cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
+
+            AltAz za=sla.CalcAltAz(rd*kDeg2Rad)*kRad2Deg;
+
+            cout << "     Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
+            *tpout << za.Az() << " " << za.Alt() << " ";
+
+            ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;;
+            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() << endl;
+
+            cout << "WM_TPoint: done. (return 0xaffe)" << endl;
+        }
+        return (void*)0xca1b;
 
     case WM_POSITION:
@@ -827,9 +903,45 @@
         return (void*)0x8888;
 
+    case WM_NEWTRACK:
+        cout << "WM_NewTrack: START" << endl;
+        fRaDec = *((RaDec*)mp);
+        cout << "WM_NewTrack: done. (return 0x9999)" << endl;
+        return (void*)0x9999;
+
+    case WM_LOADBENDING:
+        cout << "WM_LoadBending: START" << endl;
+        fBending.Load("bending.txt");
+        cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
+        return (void*)0xbe0d;
+
+    case WM_CALCALTAZ:
+        {
+            cout << endl;
+
+            SlaStars sla;
+            sla.SetMjd2Now();
+
+            XY xy = *((XY*)mp);
+            RaDec rd(xy.X()*15., xy.Y());
+
+            ZdAz a0 = sla.CalcZdAz(rd*kDeg2Rad);
+            ZdAz a1 = fBending(a0);
+            ZdAz se = CorrectTarget(GetSePos(), a1);
+            a0 *= kRad2Deg;
+            a1 *= kRad2Deg;
+            ZdAz a2 = a1*16384/360;
+            cout << "Ra/Dec source: " << xy.X()  << "h " << xy.Y()  << "°" << endl;
+            cout << "Zd/Az source:  " << a0.Zd() << "° " << a0.Az() << "°" << endl;
+            cout << "Zd/Az bended:  " << a1.Zd() << "° " << a1.Az() << "°" << endl;
+            cout << "SE bended:     " << a2.Zd() << "  " << a2.Az() << endl;
+            cout << "SE target:     " << se.Zd() << "  " << se.Az() << endl;
+        }
+        return (void*)0xa17a;
+
     case WM_QUIT:
         cout << "WM_Quit: now." << endl;
         TerminateApp();
         cout << "WM_Quit: done." << endl;
-        return (void*)0x9999;
+        return (void*)0xaaaa;
     }
     cout << "Unknown Msg" << endl;
@@ -843,6 +955,70 @@
 }
 
+void MCosy::ReadConfig()
+{
+    cout << "Reading configuration file..." << flush;
+    TEnv env(".cosyrc");
+    cout << "done." << endl;
+
+    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;
+    if (fMac1)
+        resreaz = fMac1->GetRes();
+    else
+        if (fMac3)
+            resreaz = fMac3->GetRes();
+        else
+            resreaz = env.GetValue("Az_ResRE[re/U_mot]", 1500);
+
+    Double_t resrezd = 0;
+    if (fMac2)
+        resrezd = fMac2->GetRes();
+    else
+        resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500);
+
+    Double_t ressezd = 0;
+    if (fZd1)
+        ressezd = fZd1->GetPhysRes();
+    else
+        if (fZd2)
+            ressezd = fZd2->GetPhysRes();
+        else
+            ressezd = env.GetValue("Zd_ResSE[se/U_mot]", 16384);
+
+    Double_t resseaz = 0;
+    if (fAz)
+        resseaz = fAz->GetPhysRes();
+    else
+        resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384);
+
+    kGearRatio.Set (gzd*resrezd/ressezd, gaz*resreaz/resseaz);  //[re/se]
+    kGearRatio2.Set(gzd*resrezd/360.0,   gaz*resreaz/360.0);    //[re/deg]
+    cout << "done." << endl;
+}
+
 void MCosy::TalkThread()
 {
+    if (!(fMac1 && fMac2 && fMac3))
+        return;
+
+    fMac1->ReqPos();
+    fMac2->ReqPos();
+
+    const int res = fMac3->GetVelRes();
+
+    fMac3->SetVelocity(res);
+    fMac3->SetAcceleration(res);
+    fMac3->SetDeceleration(res);
+
+    fMac1->EnableTimeout(kTRUE, 500);
+    fMac2->EnableTimeout(kTRUE, 500);
+
+    if (!(fZd1 && fZd2 && fAz))
+        return;
+
+    /*
     //
     // Start the Network
@@ -872,13 +1048,10 @@
     //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 << 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;
@@ -893,21 +1066,28 @@
     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;
-*/
+    const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]");
+    const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]");
+
+    const Double_t resreaz = env.GetValue("Az_ResRE[re/U_mot]");
+    const Double_t resrezd = env.GetValue("Zd_ResRE[re/U_mot]");
+
+    kGearRatio.Set (gzd*resrezd/RES_SE, gaz*resreaz/RES_SE);   //[re/se]
+    kGearRatio2.Set(gzd*resrezd/360.0,  gaz*resreaz/360.0);    //[re/deg]
+
+    //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;
@@ -918,5 +1098,5 @@
     // dest = AltAz(-46.0, 210);
     // SetPosition(dest);
-
+    */
     SlaStars sla;
     while (1)
@@ -928,10 +1108,10 @@
             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;
+//---        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;
@@ -1005,5 +1185,7 @@
             {
                 sla.SetMjd(time.Zd());
-                sollzd = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]
+
+                ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
+                sollzd = CorrectTarget(ist, dummy); // [se]
 
                 fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight);
@@ -1013,5 +1195,7 @@
             {
                 sla.SetMjd(time.Az());
-                sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]
+
+                ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
+                sollaz = CorrectTarget(ist, dummy); // [se]
 
                 fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
@@ -1021,11 +1205,11 @@
                                (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 << 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;
+//---        fout << endl << endl;
     }
 }
@@ -1036,7 +1220,7 @@
     // Update Gui, foremer MTGui.
     //
-    fZd1->DisplayVal();
-    fZd2->DisplayVal();
-    fAz->DisplayVal();
+    if (fZd1) fZd1->DisplayVal();
+    if (fZd2) fZd2->DisplayVal();
+    if (fAz)  fAz->DisplayVal();
 
     ZdAz ist = GetSePos()*(360.0/16384.0); // [se]
@@ -1063,4 +1247,25 @@
     Network::Start();
 
+    if (fMac1)
+        if (fMac1->IsZombieNode()) { delete fMac1; fMac1=NULL; }
+    if (fMac2)
+        if (fMac2->IsZombieNode()) { delete fMac2; fMac2=NULL; }
+    if (fMac3)
+        if (fMac3->IsZombieNode()) { delete fMac3; fMac3=NULL; }
+
+    if (fZd1)
+        if (fZd1->IsZombieNode())  { delete fZd1;  fZd1=NULL; }
+        else fZd1->SetDisplay(fWin->GetLabel2());
+
+    if (fZd2)
+        if (fZd2->IsZombieNode())  { delete fZd2;  fZd2=NULL; }
+        else fZd2->SetDisplay(fWin->GetLabel3());
+
+    if (fAz)
+        if (fAz->IsZombieNode())   { delete fAz;   fAz=NULL; }
+        else fAz->SetDisplay(fWin->GetLabel1());
+
+    ReadConfig();
+
     lout << "- Starting TX Thread." << endl;
     fTTalk = new MTTalk(this);
@@ -1092,6 +1297,5 @@
 }
 
-MCosy::MCosy(const char *dev, const int baud, MLog &out)
-: Network(dev, baud, out), fTracking(kFALSE)
+void MCosy::Constructor()
 {
     //
@@ -1101,10 +1305,12 @@
 
     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);
+    fMac3=new Macs(2, "Mac.3/Az-Sync", lout);
+    fMac2=new Macs(3, "Mac.2/Zd",      lout);
+
+    fZd1=new ShaftEncoder(5, "SE.5/Zd1", lout);
+    fZd2=new ShaftEncoder(6, "SE.6/Zd2", lout);
+    fAz =new ShaftEncoder(7, "SE.7/Az",  lout);
+
+    lout << "- Connecting devices to network." << endl;
 
     //
@@ -1132,4 +1338,125 @@
 
     lout.SetOutputGui(fWin->GetLog(), kTRUE);
+}
+
+void MCosy::ConstructorSE()
+{
+    //
+    // Create Nodes
+    //
+    lout << "- Setting up network." << endl;
+
+    fZd1=new ShaftEncoder(5, "SE.5/Zd1", lout);
+    fZd2=new ShaftEncoder(6, "SE.6/Zd2", lout);
+    fAz =new ShaftEncoder(7, "SE.7/Az",  lout);
+
+    lout << "- Connecting devices to network." << endl;
+
+    //
+    // Connect the devices to the network
+    //
+    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::ConstructorDemo()
+{
+    //
+    // 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);
+
+    lout << "- Connecting devices to network." << endl;
+
+    //
+    // 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);
+}
+
+MCosy::MCosy(int mode, const char *dev, const int baud, MLog &out)
+: Network(dev, baud, out), fTracking(kFALSE)
+{
+    lout << "- Program in ";
+    switch (mode)
+    {
+    case 0:
+        lout << "<<Stanard mode>>" << endl;
+        Constructor();
+        break;
+    case 1:
+        lout << "<<SE mode>>" << endl;
+        ConstructorSE();
+        break;
+    default:
+        lout << "<<Demo mode>>" << endl;
+        ConstructorDemo();
+    }
+
+    int i=0;
+    char name[100];
+    while (1)
+    {
+        sprintf(name, "/home/tbretz/TPoint/tpoint%03d.txt", i++);
+        cout << "Testing: " << name << endl;
+        if (gSystem->AccessPathName(name, kFileExists))
+            break;
+    }
+
+    Timer time;
+    time.Now();
+
+    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)
 }
 
@@ -1155,4 +1482,7 @@
 MCosy::~MCosy()
 {
+    *tpout << "END" << endl;
+    delete tpout;
+
     cout << "Deleting GUI timer." << endl;
 
@@ -1161,10 +1491,10 @@
     cout << "Deleting Nodes." << endl;
 
-    delete fAz;
-    delete fZd2;
-    delete fZd1;
-    delete fMac1;
-    delete fMac2;
-    delete fMac3;
+    if (fAz)   delete fAz;
+    if (fZd1)  delete fZd1;
+    if (fZd2)  delete fZd2;
+    if (fMac1) delete fMac1;
+    if (fMac2) delete fMac2;
+    if (fMac3) delete fMac3;
 
     cout << "Deleting MGCosy." << endl;
Index: trunk/MagicSoft/Cosy/main/MCosy.h
===================================================================
--- trunk/MagicSoft/Cosy/main/MCosy.h	(revision 1531)
+++ trunk/MagicSoft/Cosy/main/MCosy.h	(revision 1691)
@@ -6,13 +6,18 @@
 #include "network.h"
 #include "MThread.h"
+#include "MBending.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_WAIT        WM_NULL
+#define WM_PRESET      0x1000
+#define WM_POSITION    0x1001
+#define WM_TRACK       0x1002
+#define WM_STOP        0x1003
+#define WM_CALIB       0x1004
+#define WM_TPOINT      0x1005
+#define WM_NEWTRACK    0x1006
+#define WM_LOADBENDING 0x1007
+#define WM_CALCALTAZ   0x1008
 
 class ShaftEncoder;
@@ -75,5 +80,18 @@
     ZdAz  fVelocity;      // Actual velocity of Tracking
 
+    XY kGearRatio;        // describing the gear of the system [re/se]
+    XY kGearRatio2;       // describing the gear of the system [re/deg]
+
+    MBending fBending;
+
     UInt_t fStatus;
+
+    ofstream *tpout;
+
+    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; }
@@ -107,6 +125,12 @@
     void WaitForEndMovement();
 
+    void Constructor();
+    void ConstructorSE();
+    void ConstructorDemo();
+
+    void ReadConfig();
+
 public:
-    MCosy(const char *dev, const int baud, MLog &out=gLog);
+    MCosy(int mode, const char *dev, const int baud, MLog &out=gLog);
     ~MCosy();
 
