Changeset 1760 for trunk/MagicSoft/Cosy/main

Timestamp:

02/16/03 17:02:06 (22 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy/main

Files:

• MCosy.cc (modified) (23 diffs)
• MCosy.h (modified) (1 diff)

trunk/MagicSoft/Cosy/main/MCosy.cc

@@ +1 @@
+#include "MCosy.h"
 #include "MCosy.h"
 
@@ -307 +308 @@
     if (HasZombie())
         return;
-
     SetStatus(MCosy::kMoving);
 
@@ -408 +408 @@
 
     const ZdAz d = dst*kRad2Deg;
-    if (d.Zd()<fMin.Zd() || d.Zd()>fMax.Zd() || d.Az()<fMin.Az() || d.Az()>fMax.Az())
-    {
-        lout << "ERROR: Requested Position not inside allowed range." << endl;
+
+    lout << "Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
+
+    if (d.Zd()<fMin.Zd() || d.Zd()>fMax.Zd())
+    {
+        lout << "ERROR: Requested Zenith Angle (" << d.Zd() << "deg) not ";
+        lout << "inside allowed range." << endl;
+        return kFALSE;
+    }
+
+    if (d.Az()<fMin.Az() || d.Az()>fMax.Az())
+    {
+        lout << "ERROR: Requested Azimuth Angle (" << d.Az() << "deg) not ";
+        lout << "inside allowed range." << endl;
         return kFALSE;
     }
@@ -445 +456 @@
     fZdAzSoll = 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;
@@ -451 +461 @@
     cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
 
-    for (int i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++)
-    {
+    int i;
+    for (i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++)
+    {
+
+        lout << "Step #" << i << endl;
         //
         // Get Shaft Encoder Positions
@@ -491 +504 @@
         // at the same time
         //
+        lout << "SetVelocity" << endl;
         if (i)
             SetPosVelocity(1.0, 0.1, 0.1);
         else
-            SetPosVelocity(fabs(rd.Ratio()), 0.9, 0.5);
+            SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3);
 
         rd.Round();
@@ -511 +525 @@
         // repositioning (relative)
         //
+        lout << "Do Relative Positioning Done" << endl;
         DoRelPos(rd, cdzd, cdaz);
+
+        lout << "Relative Positioning Done" << endl;
     }
 
     StopMovement();
-    lout << "Warning: Requested position not reached." << endl;
+    lout << "Warning: Requested position not reached (i=" << i << ")" << endl;
     return FALSE;
 }
@@ -670 +687 @@
 void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
 {
+    lout << "Track Position: " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg << "deg (Ra/Dec)" << endl;
+
     SlaStars sla(fObservatory);
 
@@ -733 +752 @@
     // We want to reach the theoretical position exactly in about 0.5s
     //
-    const float dt = 1;  // 1 second
+    // *OLD*const float dt = 1;  // 1 second
+    const float dt = 3;  // 1 second
     while (!(Break() || HasError()) && !HasZombie())
     {
@@ -751 +771 @@
         dest *= kGearRatio;  // [re]
 
-        if (d.Zd()<fMin.Zd() || d.Zd()>fMax.Zd() || d.Az()<fMin.Az() || d.Az()>fMax.Az())
+        ZdAz min = fBending(fMin/kRad2Deg)*kRad2Deg;
+        ZdAz max = fBending(fMax/kRad2Deg)*kRad2Deg;
+
+        if (d.Zd()<min.Zd() || d.Az()<min.Az())
         {
             lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
+            lout << "<  " << d.Zd() << " " << min.Zd() << "  " << d.Az() << " " << min.Az()  << endl;
+            break;
+        }
+        if (d.Zd()>max.Zd() || d.Az()>max.Az())
+        {
+            lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
+            lout << ">  " <<  d.Zd() << " " << max.Zd() << "  " << d.Az() << " " << max.Az()  << endl;
             break;
         }
@@ -777 +807 @@
         // correct for the duration of RaDec2AltAz
         //
-        const ZdAz v = dest*60.0/(dt-(fMac2->GetTime()-sla));
+        const ZdAz v = dest*60.0/(dt/*-(fMac2->GetTime()-sla)*/);
 
         //
@@ -830 +860 @@
         //
         usleep(50000); // 0.05s
+        //usleep(500000); // 0.5s
     }
 
@@ -876 +907 @@
 bool MCosy::CheckNetwork()
 {
+    return kTRUE;
     //CheckConnections();
+
     if (HasZombie())
     {
@@ -1034 +1067 @@
         else
         {
+            cout << "HOME NOT ALLOWED... for Magic." << endl;
+            /*
             cout << "Going Home..." << endl;
             TEnv env(".cosyrc");
@@ -1053 +1088 @@
             fMac2->ReqPos();
             fMac3->StopMotor();
+            */
         }
         cout << "WM_Home: done. (return 0x403e)" << endl;
@@ -1065 +1101 @@
 
             XY xy = *((XY*)mp);
-            RaDec rd(xy.X()*15., xy.Y());
+            RaDec rd(xy.X()*15., xy.Y()); // [deg]
 
             ZdAz a0 = sla.CalcZdAz(rd*kDeg2Rad);
@@ -1109 +1145 @@
 
     cout << "Reading telescope range..." << flush;
-    const Double_t amin = env.GetValue("Az_Min", -90.0);
-    const Double_t zmin = env.GetValue("Zd_Min", -90.0);
+    const Double_t amin = env.GetValue("Az_Min[deg]", -74.5);
+    const Double_t zmin = env.GetValue("Zd_Min[deg]", -90.0);
     fMin.Set(zmin, amin);
 
     cout << " Min: " << zmin << "deg  " << amin << "deg" << endl;
 
-    const Double_t amax = env.GetValue("Az_Max", 318.0);
-    const Double_t zmax = env.GetValue("Zd_Max", 100.0);
+    const Double_t amax = env.GetValue("Az_Max[deg]", 318.0);
+    const Double_t zmax = env.GetValue("Zd_Max[deg]", 100.5);
     fMax.Set(zmax, amax);
 
@@ -1155 +1191 @@
         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]
+    kGearRatio.Set (gzd*resrezd*4/ressezd, gaz*resreaz*4/resseaz);  //[re/se]
+    kGearRatio2.Set(gzd*resrezd*4/360.0,   gaz*resreaz*4/360.0);    //[re/deg]
     cout << "done." << endl;
 
@@ -1183 +1219 @@
 void MCosy::TalkThreadTracking()
 {
-    if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
+    if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
         return;
+
+    if (fAz->IsZombieNode())
+        return;
+
     if (!fMac1 || !fMac2)
         return;
+
+    lout << "Tracking Thread started..." << endl;
 
     SlaStars sla(fObservatory);
@@ -1223 +1265 @@
         {
             phca1 = fZd1->PosHasChanged();
-            phca2 = fZd2->PosHasChanged();
+//            phca2 = fZd2->PosHasChanged();
             phcaz = fAz->PosHasChanged();
             usleep(1);
@@ -1250 +1292 @@
         // FIXME: I cannot take the avarage
         //
+        // FIXME
+        time.Zd(fZd1->GetMjd());
+        /* OLD*
         if (fZd1->GetMjd()>fZd2->GetMjd())
             time.Zd(fZd1->GetMjd());
         else
             time.Zd(fZd2->GetMjd());
+        */
         //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
         time.Az(fAz->GetMjd());
@@ -1281 +1327 @@
         }
 
-        ZdAz soll(sollzd.Zd(), sollaz.Az());
+        ZdAz soll(sollzd.Zd(), sollaz.Az()); // [se]
         fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384);
 
@@ -1603 +1649 @@
     lout << "- Starting GUI." << endl;
     fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
+}
+
+bool MCosy::HasZombie() const
+{
+    bool ses = fZd1->IsZombieNode() & fZd2->IsZombieNode();
+
+    return fMac1->IsZombieNode() | fMac2->IsZombieNode() | fAz->IsZombieNode() | ses;
 }
 

trunk/MagicSoft/Cosy/main/MCosy.h

@@ -161 +161 @@
     bool CheckNetwork();
 
+    virtual bool HasZombie() const;
+
 public:
     MCosy(int mode, const char *dev, const int baud, MLog &out=gLog);