Index: trunk/MagicSoft/Cosy/main/MCosy.cc
===================================================================
--- trunk/MagicSoft/Cosy/main/MCosy.cc	(revision 1702)
+++ trunk/MagicSoft/Cosy/main/MCosy.cc	(revision 1703)
@@ -41,4 +41,9 @@
 //const XY kGearRatio2(GEAR_RATIO_ALT*RES_RE/360.0,  GEAR_RATIO_AZ*RES_RE/360.0);    //[re/deg]
 
+/* +===================================+
+    FIXME: What if fMac3 (Sync) died?
+   +===================================+
+*/
+
 double MCosy::Rad2SE(double rad) const
 {
@@ -150,21 +155,17 @@
     // Get the values
     //
-    const int p0 = !fZd1->IsZombieNode() ? fZd1->GetPos() : 0;
-    const int p1 = !fZd2->IsZombieNode() ? fZd2->GetPos() : 0;
-    const int p2 = !fAz->IsZombieNode()  ? fAz->GetPos()  : 0;
-
-    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;
+    const int p0 = fZd1->GetPos();
+    const int p1 = fZd2->GetPos();
+    const int p2 = fAz->GetPos();
 
     //
     // interpolate shaft encoder positions
     //
-    const float a = (float)(a0-a1)/2;
+    const float p = (float)(p0-p1)/2;
 
     //
     // calculate 'regelabweichung'
     //
-    return ZdAz(a, a2);
+    return ZdAz(p, p2);
 }
 
@@ -177,7 +178,4 @@
 Bool_t MCosy::RequestRePos()
 {
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return kTRUE;
-
     //
     // Send request
@@ -189,4 +187,5 @@
     // Wait until the objects are received.
     //
+    // FIXME, what when waiting times out (Zombie)
     WaitForSdos();
 
@@ -194,5 +193,5 @@
     // If waiting was not interrupted everything is ok. return.
     //
-    if (!StopWaitingForSDO())
+    if (!StopWaitingForSDO() && !HasZombie())
         return kTRUE;
 
@@ -216,5 +215,5 @@
 ZdAz MCosy::GetRePos()
 {
-    return !fMac1->IsZombieNode() && !fMac2->IsZombieNode() ? ZdAz(fMac2->GetPos(), fMac1->GetPos()) : ZdAz(0,0);
+    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
 }
 
@@ -304,5 +303,5 @@
 int MCosy::StopWaitingForSDO() const
 {
-    return Break() || HasError() || HasZombie();
+    return Break() || HasError();
 }
 
@@ -316,7 +315,8 @@
 void MCosy::WaitForEndMovement()
 {
+    // FIXME, what when waiting times out (Zombie)
     WaitForSdos();
 
-    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO())
+    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO() && !HasZombie())
         usleep(1);
 }
@@ -378,4 +378,5 @@
     // FIXME: Correct by fOffset ?
 
+    /*
     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
     {
@@ -383,9 +384,18 @@
         return TRUE;
     }
+    */
+
+    //
+    // 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;
 
     //
     // Calculate new target position (shortest distance to go)
     //
-    const ZdAz src = GetSePos();//*TMath::Pi()*2/16384;;
+    const ZdAz src = GetSePos();
 
     //
@@ -404,5 +414,5 @@
     cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
 
-    for (int i=0; i<10 && !StopWaitingForSDO(); i++)
+    for (int i=0; i<10 && !StopWaitingForSDO() && !HasZombie(); i++)
     {
         //
@@ -450,4 +460,6 @@
 
         rd.Round();
+
+        // FIXME? Check for Error or Zombie?
 
         /*
@@ -477,5 +489,5 @@
 // in case of success, kFALSE in case of failure.
 //
-Bool_t MCosy::SetVelocity(ZdAz v)
+Bool_t MCosy::SetVelocity(const ZdAz &v)
 {
     //
@@ -488,4 +500,5 @@
     // Wait for the objects to be OKed.
     //
+    // FIXME, what when waiting times out (Zombie)
     WaitForSdos();
 
@@ -493,5 +506,5 @@
     // If the waiting for the objects wasn't interrupted return kTRUE
     //
-    if (!StopWaitingForSDO())
+    if (!StopWaitingForSDO() && !HasZombie())
         return kTRUE;
 
@@ -513,8 +526,9 @@
 // revolution mode.
 //
-void MCosy::InitTracking()
-{
+bool MCosy::InitTracking()
+{
+    // FIXME? Handling of Zombie OK?
     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return;
+        return false;
 
     //
@@ -523,12 +537,23 @@
     fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
     fMac2->SetDeceleration(0.90*fMac2->GetVelRes());
+    if (fMac2->IsZombieNode())
+        return false;
 
     fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
     fMac1->SetDeceleration(0.90*fMac1->GetVelRes());
+    if (fMac1->IsZombieNode())
+        return false;
 
     SetStatus(MCosy::kMoving | MCosy::kTracking);
 
     fMac2->SetRpmMode(TRUE);
+    if (fMac2->IsZombieNode())
+        return false;
+
     fMac1->SetRpmMode(TRUE);
+    if (fMac1->IsZombieNode())
+        return false;
+
+    return true;
 }
 
@@ -640,5 +665,6 @@
     // Init accelerations and Rpm Mode
     //
-    InitTracking();
+    if (!InitTracking())
+        return;
 
     XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
@@ -655,8 +681,4 @@
     //
     fRaDec = dst;
-
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return;
-
     fTracking = kTRUE;
 
@@ -671,5 +693,5 @@
     //
     const float dt = 1;  // 1 second
-    while (!StopWaitingForSDO())
+    while (!StopWaitingForSDO() && !HasZombie())
     {
         //
@@ -774,7 +796,4 @@
 void MCosy::StopMovement()
 {
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return;
-
     //
     // Set status to Stopping
@@ -785,14 +804,12 @@
     // set deceleration to 50%
     //
-    cout << "Stopping  positioning..." << endl;
+    cout << "Stopping..." << endl;
     fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
+    if (!fMac1->IsZombieNode())
+        fMac1->SetRpmMode(FALSE);
+
     fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
-
-    //
-    // Stop revolution mode (movement)
-    //
-    cout << "Stopping possibleRPM mode..." << endl;
-    fMac1->SetRpmMode(FALSE);
-    fMac2->SetRpmMode(FALSE);
+    if (!fMac2->IsZombieNode())
+        fMac2->SetRpmMode(FALSE);
 
     //
@@ -809,14 +826,18 @@
 }
 
-void *MCosy::Proc(int msg, void *mp)
-{
-    lout << "Checking for Zombies" << endl;
+bool MCosy::CheckNetwork()
+{
+    CheckConnections();
     if (HasZombie())
     {
-        lout << "Found Zombies" << endl;
+        lout << "- Found Zombies in Network..." << endl;
         if (!RebootZombies())
-            return (void*)0xebb0;
+            return false;
     }
-
+    return true;
+}
+
+void *MCosy::Proc(int msg, void *mp)
+{
     switch (msg)
     {
@@ -827,4 +848,6 @@
     case WM_STOP:
         cout << "MCosy::Proc: Stop." << endl;
+        if (!CheckNetwork())
+            return (void*)0xebb0;
         StopMovement();
         return NULL;
@@ -832,7 +855,9 @@
     case WM_PRESET:
         cout << "WM_Preset: start." << endl;
-        if (!fZd1->IsZombieNode()) fZd1->SetPreset();
-        if (!fZd2->IsZombieNode()) fZd2->SetPreset();
-        if (!fAz->IsZombieNode())  fAz->SetPreset();
+        if (!CheckNetwork())
+            return (void*)0xebb0;
+        fZd1->SetPreset();
+        fZd2->SetPreset();
+        fAz->SetPreset();
         cout << "WM_Preset: done. (return 0xaffe)" << endl;
         return (void*)0xaffe;
@@ -841,4 +866,7 @@
         {
             cout << "WM_Calib: start." << endl;
+            if (!CheckNetwork())
+                return (void*)0xebb0;
+
             SlaStars sla;
             sla.SetMjd2Now();
@@ -858,10 +886,7 @@
             cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
 
-            if (!fZd1->IsZombieNode())
-                fZd1->SetPreset(za.Zd());
-            if (!fZd2->IsZombieNode())
-                fZd2->SetPreset(-za.Zd());
-            if (!fAz->IsZombieNode())
-                fAz->SetPreset(za.Az());
+            fZd1->SetPreset(za.Zd());
+            fZd2->SetPreset(-za.Zd());
+            fAz->SetPreset(za.Az());
 
             cout << "WM_Calib: done. (return 0xaffe)" << endl;
@@ -897,6 +922,8 @@
         cout << "WM_Position: start." << endl;
         {
+            if (!CheckNetwork())
+                return (void*)0xebb0;
+
             ZdAz dest = *((ZdAz*)mp);
-
             SetPosition(dest*kDeg2Rad);
         }
@@ -907,4 +934,7 @@
         cout << "WM_Track: START" << endl;
         {
+            if (!CheckNetwork())
+                return (void*)0xebb0;
+
             RaDec dest = *((RaDec*)mp);
             TrackPosition(dest*kDeg2Rad);
@@ -933,5 +963,7 @@
     case WM_HOME:
         cout << "WM_Home: START" << endl;
-        if (!fMac1->IsZombieNode() && !fMac2->IsZombieNode())
+        if (!CheckNetwork())
+            return (void*)0xebb0;
+        else
         {
             cout << "Going Home..." << endl;
@@ -976,4 +1008,9 @@
     case WM_QUIT:
         cout << "WM_Quit: now." << endl;
+        if (!CheckNetwork())
+        {
+            lout << "ERROR: Cannot shutdown CANbus network." << endl;
+            return (void*)0xebb0;
+        }
         TerminateApp();
         cout << "WM_Quit: done." << endl;
@@ -1004,5 +1041,5 @@
         resreaz = fMac1->GetRes();
     else
-        if (!fMac3->IsZombieNode())
+        if (fMac3 && !fMac3->IsZombieNode())
             resreaz = fMac3->GetRes();
         else
@@ -1040,21 +1077,25 @@
 }
 
+void MCosy::InitSync()
+{
+    if (!fMac3)
+        return;
+
+    const int res = fMac3->GetVelRes();
+
+    fMac3->SetVelocity(res);
+    fMac3->SetAcceleration(res);
+    fMac3->SetDeceleration(res);
+    fMac3->StartPosSync();
+}
+
 void MCosy::TalkThread()
 {
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
+    /* ========== FIXME? =============
+     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
         return;
-
+     */
     fMac1->ReqPos();
     fMac2->ReqPos();
-
-    if (fMac3)
-    {
-        const int res = fMac3->GetVelRes();
-
-        fMac3->SetVelocity(res);
-        fMac3->SetAcceleration(res);
-        fMac3->SetDeceleration(res);
-        fMac3->StartPosSync();
-    }
 
     if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
@@ -1227,7 +1268,7 @@
     // Update Gui, foremer MTGui.
     //
-    if (!fZd1->IsZombieNode()) fZd1->DisplayVal();
-    if (!fZd2->IsZombieNode()) fZd2->DisplayVal();
-    if (!fAz->IsZombieNode())  fAz->DisplayVal();
+    fZd1->DisplayVal();
+    fZd2->DisplayVal();
+    fAz->DisplayVal();
 
     ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
@@ -1238,5 +1279,5 @@
     avail |= !fMac1->IsZombieNode() ? 0x01 : 0;
     avail |= !fMac2->IsZombieNode() ? 0x02 : 0;
-    avail |= !fMac3->IsZombieNode() ? 0x04 : 0;
+    avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
     avail |= !fZd1->IsZombieNode()  ? 0x08 : 0;
     avail |= !fZd2->IsZombieNode()  ? 0x10 : 0;
@@ -1268,23 +1309,4 @@
     // Don't call this function twice!
     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();
@@ -1292,5 +1314,4 @@
     lout << "- Starting TX Thread." << endl;
     fTTalk = new MTTalk(this);
-    //    fTGui = new MTGui(this);
 
     lout << "- Starting GUI update." << endl;
@@ -1361,10 +1382,4 @@
     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);
 }
 
@@ -1397,10 +1412,4 @@
     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);
 }
 
@@ -1420,6 +1429,4 @@
     lout << "- Starting GUI." << endl;
     fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
-
-    lout.SetOutputGui(fWin->GetLog(), kTRUE);
 }
 
@@ -1451,4 +1458,10 @@
     }
 
+    lout.SetOutputGui(fWin->GetLog(), kTRUE);
+
+    fZd1->SetDisplay(fWin->GetLabel2());
+    fZd2->SetDisplay(fWin->GetLabel3());
+    fAz->SetDisplay(fWin->GetLabel1());
+
     int i=0;
     char name[100];
@@ -1503,10 +1516,11 @@
     cout << "Deleting Nodes." << endl;
 
-    if (fAz)   delete fAz;
-    if (fZd1)  delete fZd1;
-    if (fZd2)  delete fZd2;
-    if (fMac1) delete fMac1;
-    if (fMac2) delete fMac2;
-    if (fMac3) delete fMac3;
+    delete fAz;
+    delete fZd1;
+    delete fZd2;
+    delete fMac1;
+    delete fMac2;
+    if (fMac3)
+        delete fMac3;
 
     cout << "Deleting MGCosy." << endl;
