Changeset 2019 for trunk/MagicSoft/Cosy/main/MCosy.cc

Timestamp:

04/28/03 11:04:17 (22 years ago)

Author:

tbretz

Message:

*** empty log message ***

File:

• trunk/MagicSoft/Cosy/main/MCosy.cc (modified) (46 diffs)

trunk/MagicSoft/Cosy/main/MCosy.cc

@@ -25 +25 @@
 #include "base/timer.h"
 #include "shaftencoder.h"
-
-//#include <sys/resource.h>  // PRIO_PROCESS
 
 ClassImp(MCosy);
@@ -52 +50 @@
 */
 
+#define EXPERT
+
 double MCosy::Rad2SE(double rad) const
 {
@@ -161 +161 @@
 //  sending all changes immediatly.
 //
-ZdAz MCosy::GetSePos()
+ZdAz MCosy::GetSePos() const
 {
     const int pa = fAz->GetPos();
@@ -206 +206 @@
     fMac1->WaitForSdo(0x6004);
 
-    // FIXME, what when waiting times out (Zombie)
-    // WaitForSdos();
-
     //
     // If waiting was not interrupted everything is ok. return.
     //
-    if (!(Break() || HasError()) && !HasZombie())
+    if (!(Break() || HasError() || HasZombie()))
         return kTRUE;
 
@@ -302 +299 @@
     if (axe1) fMac2->StartRelPos(rd.Zd());
     if (axe2) fMac1->StartRelPos(rd.Az());
-
+#ifdef EXPERT
     cout << "Waiting for positioning..." << flush;
-
+#endif
     if (axe1) fMac2->WaitForSdo(0x6004, 1);
     if (axe2) fMac1->WaitForSdo(0x6004, 1);
 
     WaitForEndMovement();
-
+#ifdef EXPERT
     cout << "done." << endl;
+#endif
 }
 
@@ -400 +398 @@
     if (d.Zd()<fMin.Zd() || d.Zd()>fMax.Zd())
     {
-        lout << "ERROR: Requested Zenith Angle (" << d.Zd() << "deg) not ";
-        lout << "inside allowed range." << endl;
+        lout << "ERROR: Requested Zenith Angle (" << d.Zd() << "deg, Az=";
+        lout << d.Az() << ") not inside allowed range." << endl;
         return kFALSE;
     }
@@ -407 +405 @@
     if (d.Az()<fMin.Az() || d.Az()>fMax.Az())
     {
-        lout << "ERROR: Requested Azimuth Angle (" << d.Az() << "deg) not ";
-        lout << "inside allowed range." << endl;
+        lout << "ERROR: Requested Azimuth Angle (" << d.Az() << "deg, Zd=";
+        lout << d.Zd() << ") not inside allowed range." << endl;
         return kFALSE;
     }
@@ -428 +426 @@
 int MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
 {
-    // FIXME: Correct by fOffset ?
-
     const ZdAz d = dst*kRad2Deg;
 
@@ -441 +437 @@
     //
     const ZdAz src = GetSePos(); // [se]
-
-    /*
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-    {
-        cout << "SetPosition: No connection to at least one of the MACS!" << endl;
-        return TRUE;
-    }
-    */
 
     //
@@ -457 +445 @@
     //if (fMac3->IsZombieNode())
     //    return false;
-
-    // ====================================
-    RequestRePos();
-    ZdAz before = GetRePos();
-    // ====================================
 
     //
@@ -477 +460 @@
     cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
 
-    /*
-     * re(se) = -28.1*cos(x*360/16384/se - 14.4) + 443.2*re/se
-     *
-     * int(re) = -1278.86*sin(0.021972656   *x/se - 14.4    )re + 443.2re x/se
-     * int(re) = -73273.4*sin(0.000383495197*x/se - .2513274)re + 443.2re x/se
-     *
-     * int (-75deg..95deg) = 3.28343e7 re
-     *
-     */
-
     //
     // Set velocities
@@ -493 +466 @@
 
     int i;
-    for (i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++)
+    for (i=0; i<10 && !(Break() || HasError() || HasZombie()); i++)
     {
 
@@ -506 +479 @@
         //
         ZdAz rd = dest-p; // [se]
-/*
-        if (rd.Az()>0 && rd.Az()<0.5)
-            rd.Az(1);
-        if (rd.Az()<0 && rd.Az()>-0.5)
-            rd.Az(-1);
-*/
-        cout << "Deviation: " << rd.Zd() << "se  Az:" << rd.Az() << "se" << endl;
-
 
         // ===========================================
-        Float_t k1 = dest.Zd()*360/16384 - 14.3979; //[deg]
-        Float_t k0 = p.Zd()*360/16384    - 14.3979; //[deg]
-
-        //
-        // -28re/se*cos((x/deg-14.4)*pi/180) + 443.2re/se     x
-        // 28re/se*2*pi/360*sin((x/deg-14.4)*pi/180)*deg + 443.2re/se*x/deg
-        //
-
-        Float_t dk = 443.152*(dest.Zd()-p.Zd());
-        Float_t dx = 28.0148/2/TMath::Pi()*(sin(k1/kRad2Deg)-sin(k0/kRad2Deg));
-
-        cout << "Using: " << rd.Zd()*kGearRatio.X() << endl; // [re]
-        cout << "Estimated1: " << 16384*dx + dk << endl;
-        cout << "Estimated2: " << (16384*dx + dk)/1.085949688 << endl;
-        cout << "Estimated3: " << rd.Zd()*432 << endl;
+        const ZdAz ist = dst-rd*TMath::Pi()/8192;
+
+        const double p1 = ist.Zd()-19.0605/kRad2Deg;
+        const double p2 = dst.Zd()-19.0605/kRad2Deg;
+
+        const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*8192/TMath::Pi();
+        const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*8192/TMath::Pi();
         // ===========================================
-
 
         ZdAz cd = rd;     // [se]
         cd.Round();
-/*
-        if (rd.Az()>0 && rd.Az()<0.5)
-            rd.Az(0.5);
-        if (rd.Az()<0 && rd.Az()>-0.5)
-            rd.Az(-0.5);
-*/
+
         //
         // Check if there is a control deviation on the axis
@@ -555 +506 @@
             lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl;
             SetStatus(MCosy::kStopped);
-
-            // ====================================
-            RequestRePos();
-            ZdAz after = GetRePos();
-            // ====================================
-
-            after -= before;
-
-            cout << "Drove: " << after.Zd() << " " << after.Az() << endl;
             return TRUE;
         }
@@ -571 +513 @@
         //
         rd *= kGearRatio; // [re]
+        rd.Zd(f2-f1);
 
         //
@@ -590 +533 @@
             if (rd.Az()>-15*kGearRatio.Y() && rd.Az()<15*kGearRatio.Y())
             {
+#ifdef EXPERT
                 cout << " -------------- LO ---------------- " << endl;
+#endif
                 fMac1->SetAcceleration(0.05*vr);
                 fMac1->SetDeceleration(0.05*vr);
@@ -596 +541 @@
             else
             {
+#ifdef EXPERT
                 cout << " -------------- HI ---------------- " << endl;
+                fMac1->SetAcceleration(0.4*vr);// 0.4
+                fMac1->SetDeceleration(0.4*vr);// 0.4
+#else
                 fMac1->SetAcceleration(0.2*vr);
                 fMac1->SetDeceleration(0.1*vr);
+#endif
             }
 
+#ifdef EXPERT
+            fMac2->SetAcceleration(0.4*vr);// 0.4
+            fMac2->SetDeceleration(0.4*vr);// 0.4
+            SetPosVelocity(fabs(rd.Ratio()), 0.2); // 0.175
+#else
             fMac2->SetAcceleration(0.2*vr);
             fMac2->SetDeceleration(0.1*vr);
-
-            // vel(0.6)  acc(0.5)
-            SetPosVelocity(fabs(rd.Ratio()), 0.175);
-            //            SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3);
+            SetPosVelocity(fabs(rd.Ratio()), 0.1);
+#endif
         }
 
@@ -632 +585 @@
     if (i<10)
         StopMovement();
+    else
+        SetStatus(MCosy::kStopped);
 
     lout << "Warning: Requested position not reached (i=" << i << ")" << endl;
@@ -653 +608 @@
 
     //
-    // Wait for the objects to be OKed.
-    //
-
+    // Wait for the objects to be acknoledged.
+    //
     fMac2->WaitForSdo(0x3006, 1);
     fMac1->WaitForSdo(0x3006, 1);
 
-    // FIXME, what when waiting times out (Zombie)
-    // WaitForSdos();
-
     //
     // If the waiting for the objects wasn't interrupted return kTRUE
     //
-    if (!(Break() || HasError()) && !HasZombie())
+    if (!(Break() || HasError() || HasZombie()))
         return kTRUE;
 
@@ -694 +645 @@
     // Start revolution mode
     //
-    fMac2->SetAcceleration(0.2*fMac2->GetVelRes());
-    fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
+    fMac2->SetAcceleration(0.1*fMac2->GetVelRes());
+    fMac2->SetDeceleration(0.1*fMac2->GetVelRes());
     if (fMac2->IsZombieNode())
         return false;
 
-    fMac1->SetAcceleration(0.2*fMac1->GetVelRes());
-    fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
+    fMac1->SetAcceleration(0.1*fMac1->GetVelRes());
+    fMac1->SetDeceleration(0.1*fMac1->GetVelRes());
     if (fMac1->IsZombieNode())
         return false;
@@ -732 +683 @@
     // we don't want fast changes!
     //
-
     ULong_t vrzd = fMac1->GetVelRes();
     ULong_t vraz = fMac2->GetVelRes();
@@ -738 +688 @@
 #define sgn(x) (x<0?-1:1)
 
-    const Float_t limit = 0.25;
+    //
+    // When speed changes sign, the maximum allowed speed
+    // is 25% of the |v|
+    //
+    const Float_t limit    = 0.25;
+
+    //
+    // The maximum allowed speed while tracking is 10%
+    //
+    const Float_t maxtrack = 0.1;
 /*
     if (sgn(vt->Az()) != sgn(vcalc.Az()))
@@ -766 +725 @@
         && fabs(vt->Az()) < limit*fabs(vcalc.Az())
        )
-        vt->Az(0);
+        {
+            lout << "Warning: Negative Azimuth speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
+            vt->Az(0);
+        }
     else
-        if (fabs(vt->Az()) > 0.9*vraz)
-        {
-            lout << "Warning: Azimuth speed limit exceeded. Limiting speed to 90% max." << endl;
-            vt->Az(0.9*vraz*sgn(vt->Az()));
+        if (fabs(vt->Az()) > maxtrack*vraz)
+        {
+            lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded... limited." << endl;
+            vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
         }
 
@@ -777 +739 @@
         && fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
        )
-        vt->Zd(0);
+        {
+            lout << "Warning: Negative Altitude speed limit (" << limit*100 << "%) exceeded... set to 0." << endl;
+            vt->Zd(0);
+        }
     else
-        if (fabs(vt->Zd()) > 0.9*vrzd)
-        {
-            lout << "Warning: Altitude speed limit exceeded. Limiting speed to 90% max." << endl;
-            vt->Zd(0.9*vrzd*sgn(vt->Zd()));
-        }
-}
-
-Bool_t MCosy::AlignTrackingPos(ZdAz pointing, ZdAz &za)
+        if (fabs(vt->Zd()) > maxtrack*vrzd)
+        {
+            lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded... limited." << endl;
+            vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
+        }
+}
+
+Bool_t MCosy::AlignTrackingPos(ZdAz pointing, ZdAz &za) const
 {
     // pointing [deg]
-
     if (pointing.Zd()<0)
     {
@@ -796 +760 @@
     }
 
-    const ZdAz se = GetSePos()*360/16384; // [deg]
-    const ZdAz unbendedse = fBending.CorrectBack(se); // ist pointing
+    const ZdAz se = GetSePos()*2*TMath::Pi()/16384;   // [rad]
+    const ZdAz unbendedse = fBending.CorrectBack(se)*kRad2Deg; // ist pointing
 
     do
@@ -805 +769 @@
             break;
 
-        pointing.Az(pointing.Az()+TMath::Sign(360., -d));
+        pointing.Az(pointing.Az()+TMath::Sign(360., d));
     } while (1);
-  /*
-    while (pointing.Az()<fMin.Az())
-        pointing.Az(pointing.Az() + 360);
-
-    while (pointing.Az()>fMax.Az())
-        pointing.Az(pointing.Az() - 360);
-        */
+
     const Bool_t rc = CheckRange(pointing);
-    za = fBending(pointing/kRad2Deg); // [rad]
+    za = pointing/kRad2Deg; // [rad]
 
     if (!rc)
@@ -821 +779 @@
 
     return rc;
+}
+
+Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
+{
+    ifstream fin("pointingpos.txt");
+    if (!fin)
+        return -1;
+
+    Double_t mjd0, zd, az;
+    fin >> mjd0 >> zd >> az;
+
+    mjd0 += 52000;
+
+    if (mjd0+1./24/60 <mjd)
+        return -1;
+
+    ZdAz point;
+    if (!AlignTrackingPos(ZdAz(zd, az), point))
+    {
+        cout << "Starguider position couldn't be aligned..." << endl;
+        return -1;
+    }
+
+    const ZdAz diff = (dest-point)*kRad2Deg;
+
+    if (diff.Zd()>5 || diff.Az()>5)
+    {
+        cout << "Starguider deviation too large... dZd=" << diff.Zd() <<" dAz="<<diff.Az() << endl;
+        return -1;
+    }
+
+    dest -= point;
+    dest *= 16384/TMath::Pi()/2; // [se]
+    dest *= -kGearRatio;         // [re]
+
+    cout << "Using Starguider... dZd=" << dest.Zd() << " dAz=" << dest.Az() << endl;
+
+    return (mjd-mjd0) * (24*60*60); // [s]
 }
 
@@ -877 +873 @@
     //
     if (!InitTracking())
+    {
+        StopMovement();
         return;
+    }
 
     XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
@@ -883 +882 @@
     lout << "Start tracking:";
     lout << " Ra: " << xy.X() << "h  " << "Dec: " << xy.Y() << "\xb0" << endl;
-
+#ifdef EXPERT
     ofstream fout("coordinates.txt");
     fout << xy;
     fout.close();
-
+#endif
     //
     // Initialize Tracker (slalib or starguider)
@@ -922 +921 @@
             break;
 
-        dest *= 16384/TMath::Pi()/2;  // [se]
-
-        /*
-        ZdAz dummy1 = sla.CalcZdAz(fRaDec);
-
-        if (dummy1.Az()<-TMath::Pi()/2)
-            dummy1.Az(dummy1.Az() + TMath::Pi()*2);
-        if (dummy1.Az()>3*TMath::Pi()/2)
-            dummy1.Az(dummy1.Az() -TMath::Pi()*2);
-
-        if (!CheckRange(dummy1*kRad2Deg))
-        {
-            lout << "Error: Tracking position out of Range." << endl;
-            break;
-        }
-
-        ZdAz dummy = fBending(dummy1);
-        dest = CorrectTarget(GetSePos(), dummy); // [se]
-        */
-
-//*LP*      const ZdAz d = dest*360./16384; // [deg]
-        dest *= kGearRatio;  // [re]
-
-//*LP*      ZdAz min = fBending(fMin/kRad2Deg)*kRad2Deg;
-//*LP*      ZdAz max = fBending(fMax/kRad2Deg)*kRad2Deg;
-
         ZdAz vcalc = sla.GetApproxVel(fRaDec) * 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 -= GetRePos() + fOffset;
+        float dtime = -1;
+        if (kFALSE /*fUseStarguider*/)
+            dtime = Starguider(mjd, dest);
+
+        if (dtime<0)
+        {
+            dest = fBending(dest);       // [rad]
+            dest *= 16384/TMath::Pi()/2; // [se]
+            dest *= kGearRatio;          // [re]
+
+            //
+            // 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 -= GetRePos() + fOffset;
+
+            dtime = dt;
+        }
 
         //
@@ -968 +953 @@
         // correct for the duration of RaDec2AltAz
         //
-        const ZdAz v = dest*60.0/(dt/*-(fMac2->GetTime()-sla)*/);
+        const ZdAz v = dest*60.0/(dtime/*-(fMac2->GetTime()-sla)*/);
 
         //
@@ -1020 +1005 @@
         // (This is important on fast machines >500MHz)
         //
+        /*
+        MTimeout t(1000);
+        while (!t.HasTimedOut())
+            usleep(1);
+         */
         usleep(1000000); // 1s
         //usleep(50000); // 0.05s
@@ -1047 +1037 @@
     //
     cout << "Stopping movement (dec=30%)..." << endl;
-
+#ifdef EXPERT
+    fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
+    fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
+#else
     fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
     fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
+#endif
     fMac1->SetRpmMode(FALSE);
     fMac2->SetRpmMode(FALSE);
@@ -1068 +1062 @@
     // Wait for the movement to really be finished.
     //
+#ifdef EXPERT
     cout << "Waiting for end of movement..." << endl;
+#endif
     WaitForEndMovement();
 
@@ -1075 +1071 @@
     //
     CheckForError();
+#ifdef EXPERT
     cout << "Movement stopped." << endl;
+#endif
 }
 
@@ -1203 +1201 @@
         return (void*)0xaffe;
         */
-
+#ifdef EXPERT
     case WM_TPOINT:
         {
@@ -1229 +1227 @@
         }
         return (void*)0xca1b;
+#endif
 
     case WM_TRACKPOS:
@@ -1398 +1397 @@
 
     cout << "Reading telescope range..." << flush;
-    const Double_t amin = env.GetValue("Az_Min[deg]", -90.0);
-    const Double_t zmin = env.GetValue("Zd_Min[deg]", -74.5);
+    const Double_t amin = env.GetValue("Az_Min[deg]", -95.0);
+    const Double_t zmin = env.GetValue("Zd_Min[deg]", -75.0);
     fMin.Set(zmin, amin);
 
     cout << " Min: " << zmin << "deg  " << amin << "deg" << endl;
 
-    const Double_t amax = env.GetValue("Az_Max[deg]", 318.0);
-    const Double_t zmax = env.GetValue("Zd_Max[deg]", 100.5);
+    const Double_t amax = env.GetValue("Az_Max[deg]", 305.0);
+    const Double_t zmax = env.GetValue("Zd_Max[deg]", 98.25);
     fMax.Set(zmax, amax);
 
@@ -1450 +1449 @@
     cout << " * Setting Gear Ratios:" << endl;
     cout << "   --------------------" << endl;
-    cout << " *  X: " << gzd << "*" << resrezd << "/" << ressezd << "=" << kGearRatio.X() << endl;
-    cout << " *  Y: " << gaz << "*" << resreaz << "/" << resseaz << "=" << kGearRatio.Y() << endl;
+    cout << " *  X: " << gzd << "*" << resrezd << "/" << ressezd << "=4*" << kGearRatio.X() << endl;
+    cout << " *  Y: " << gaz << "*" << resreaz << "/" << resseaz << "=4*" << kGearRatio.Y() << endl;
 }
 
@@ -1489 +1488 @@
     ZdAz ist = GetSePos();              // [se]
 
-//    ZdAz istre = -fOffset;              // [re]
     ZdAz time;
 
@@ -1583 +1581 @@
         ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad]
 
-        AlignTrackingPos(soll*360/16384, fZdAzSoll);
-        /*
-        fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384);
-
-        // FIXME?
-        if (fZdAzSoll.Az()<-TMath::Pi()/2)
-            fZdAzSoll.Az(fZdAzSoll.Az() + TMath::Pi()*2);
-        if (fZdAzSoll.Az()>3*TMath::Pi()/2)
-            fZdAzSoll.Az(fZdAzSoll.Az() -TMath::Pi()*2);
-
-        // FIXME?
-        while (ist.Az()>3*16384/4/2)
-            ist.Az(ist.Az() - 16384);
-        while (soll.Az()>3*16384/4/2)
-            soll.Az(soll.Az() - 16384);
-        while (ist.Az()<-16384/4)
-            ist.Az(ist.Az() + 16384);
-        while (soll.Az()<-16384/4)
-            soll.Az(soll.Az() + 16384);
-            */
+        AlignTrackingPos(soll*kRad2Deg, fZdAzSoll);
+
         ist *= TMath::Pi()*2/16384;
-        fTrackingError.Set(ist.Zd()-fZdAzSoll.Zd(), ist.Az()-fZdAzSoll.Az());
+        soll = fBending(fZdAzSoll);
+        fTrackingError.Set(ist.Zd()-soll.Zd(), ist.Az()-soll.Az());
 
         //---            fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
@@ -1724 +1705 @@
             re0.Zd(re.Zd());
 
+            se -= dse/2;
+
             ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
             ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
@@ -1732 +1715 @@
             se0.Az(se.Az());
             re0.Az(re.Az());
+
+            se -= dse/2;
 
             ZdAz bend = fBending.CorrectBack(se*2*TMath::Pi()/16384)*kRad2Deg;
@@ -2124 +2109 @@
     fZd2->SetDisplay(fWin->GetLabel3());
     fAz->SetDisplay(fWin->GetLabel1());
-
+#ifdef EXPERT
     int i=0;
     char name[100];
@@ -2145 +2130 @@
     *tpout << time.Year() << " " << time.Month() << " " << time.Day() << " ";
     *tpout << /*"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
+#endif
     // temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
 }
@@ -2170 +2156 @@
 MCosy::~MCosy()
 {
+#ifdef EXPERT
     *tpout << "END" << endl;
     delete tpout;
+#endif
 
     cout << "Deleting GUI timer." << endl;