Changeset 6923 for trunk/MagicSoft/Cosy/main/MTracking.cc

Timestamp:

04/11/05 10:58:30 (20 years ago)

Author:

tbretz

Message:

*** empty log message ***

File:

• trunk/MagicSoft/Cosy/main/MTracking.cc (modified) (6 diffs)

trunk/MagicSoft/Cosy/main/MTracking.cc

@@ -22 +22 @@
 bool MTracking::RequestRePos()
 {
-    //
-    // Send request
-    //
-    fCosy->fMac2->RequestSDO(0x6004);
-    fCosy->fMac1->RequestSDO(0x6004);
-
-    //
-    // Wait until the objects are received.
-    //
-    fCosy->fMac2->WaitForSdo(0x6004);
-    fCosy->fMac1->WaitForSdo(0x6004);
-
-    //
-    // If waiting was not interrupted everything is ok. return.
-    //
-    if (!Break())
-        return true;
+    for (int i=0; i<2; i++)
+    {
+        //
+        // Send request
+        //
+        fCosy->fMac2->RequestSDO(0x6004);
+        fCosy->fMac1->RequestSDO(0x6004);
+
+        //
+        // Wait until the objects are received.
+        //
+        fCosy->fMac2->WaitForSdo(0x6004, 0, 500, i>0);
+        fCosy->fMac1->WaitForSdo(0x6004, 0, 500, i>0);
+
+        //
+        // If waiting was not interrupted everything is ok. return.
+        //
+        if (!Break())
+            return true;
+
+        fCosy->PrintError();
+    }
 
     //
@@ -48 +53 @@
 
     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 MTracking::SetVelocity(const ZdAz &v)
+{
+    for (int i=0; i<2; i++)
+    {
+        //
+        // Send the new velocities for both axes.
+        //
+        fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
+        fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]
+
+        //
+        // Wait for the objects to be acknoledged.
+        //
+        fCosy->fMac2->WaitForSdo(0x3006, 1, 500, i>0);
+        fCosy->fMac1->WaitForSdo(0x3006, 1, 500, i>0);
+
+        //
+        // If the waiting for the objects wasn't interrupted return kTRUE
+        //
+        if (!Break())
+            return kTRUE;
+
+        fCosy->PrintError();
+    }
+
+    //
+    // print a message if the interruption was due to a Can-node Error
+    //
+    if (fCosy->HasError())
+        lout << "Error while setting tracking velocity (SDO #3006)" << endl;
+
+    return kFALSE;
 }
 
@@ -189 +235 @@
 }
 
-// --------------------------------------------------------------------------
-//
-// 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 MTracking::SetVelocity(const ZdAz &v)
-{
-    //
-    // Send the new velocities for both axes.
-    //
-    fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
-    fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]
-
-    //
-    // Wait for the objects to be acknoledged.
-    //
-    fCosy->fMac2->WaitForSdo(0x3006, 1);
-    fCosy->fMac1->WaitForSdo(0x3006, 1);
-
-    //
-    // If the waiting for the objects wasn't interrupted return kTRUE
-    //
-    if (!Break())
-        return kTRUE;
-
-    //
-    // print a message if the interruption was due to a Can-node Error
-    //
-    if (fCosy->HasError())
-        lout << "Error while setting tracking velocity (SDO #3006)" << endl;
-
-    return kFALSE;
-}
-
 // --------------------------------------------------------------------
 //
@@ -237 +247 @@
     const XY re = fCosy->kGearTot/fCosy->kResSE; //[re/se]
 
+    Int_t pzd1 = fCosy->fZd1->GetPos();
+    Int_t pzd2 = fCosy->fZd2->GetPos();
+    Int_t paz  = fCosy->fAz->GetPos();
+
+    if (fCosy->fZd1->DirHasChanged() != fCosy->fZd2->DirHasChanged())
+    {
+        pzd1 -= fCosy->fZd1->GetDirection();
+        pzd2 -= fCosy->fZd2->GetDirection();
+    }
+    else
+    {
+        fCosy->fZd1->ResetDirHasChanged();
+        fCosy->fZd2->ResetDirHasChanged();
+    }
+
     // Get current shaftencoder position of the telescope
-    Double_t seposzd1 = ((fCosy->fZd1->GetPos()+8192)%16384)*re.X();
-    Double_t seposzd2 = ((fCosy->fZd2->GetPos()+8192)%16384)*re.X();
-    Double_t seposaz  =   fCosy->fAz->GetPos() *re.Y();
+    Double_t seposzd1 = ((pzd1+8192)%16384)*re.X();
+    Double_t seposzd2 = ((pzd2+8192)%16384)*re.X();
+    Double_t seposaz  =   paz              *re.Y();
 
     // distance between (To+dt) and To [re]
@@ -503 +528 @@
         return (void*)3;
 
-    lout << "- Tracking Thread started..." << endl;
+    lout << "- Tracking Thread started (" << MTime() << ")" << endl;
 
     //const XY re2se = fCosy->kGearTot/fCosy->kResSE; //[re/se]
@@ -590 +615 @@
     }
 
-    lout << "- Tracking Thread done." << endl;
-
+    lout << "- Tracking Thread done. (" << MTIme() << ")" << endl;
     return 0;
 }