Changeset 921 for trunk/MagicSoft/Cosy/MCosy.cc

Timestamp:

08/30/01 16:02:20 (23 years ago)

Author:

tbretz

Message:

*** empty log message ***

File:

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

trunk/MagicSoft/Cosy/MCosy.cc

@@ -54 +54 @@
     return rad*kGearRatio2.Y();
 }
-
-//double Rad2Deg(double rad)
-//{
-//    return 360.0/D2PI*rad;
-//}
 
 ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
@@ -200 +195 @@
     cout << "Waiting for positioning..." << flush;
 
-    WaitForSdos();
-
-    cout << "SDO..." << flush;
-
-    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO())
-        usleep(1);
+    WaitForEndMovement();
 
     cout << "done." << endl;
-
-    SetStatus(kStopped);
 }
 
@@ -215 +203 @@
 {
     if (!HasError())
+    {
+        SetStatus(kStopped);
         return;
+    }
 
     SetStatus(kError);
@@ -228 +219 @@
 int MCosy::SetPosition(const ZdAz &dst) // [rad]
 {
-    // FIXME: CORRECT BY fTrackingError !!!!!!!!!!
+    // FIXME: Correct by fOffset ?
 
     //
@@ -253 +244 @@
         ZdAz rd = dest-p; // [se]
 
-        ZdAz cd = rd;   // [se]
+        ZdAz cd = rd;     // [se]
         cd.Round();
 
@@ -300 +291 @@
     }
 
-    CheckForError();
-
+    StopMovement();
     lout << "Warning: Requested position not reached." << endl;
     return FALSE;
@@ -354 +344 @@
 void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
 {
-    return;
-
     //
     // How to limit the speed. If the wind comes and blowes
@@ -367 +355 @@
 #define sgn(x) (x<0?-1:1)
 
-    if (sgn(vt->Az()) != sgn(vcalc.Az()))
-        vt->Az(0.1*vcalc.Az());
+    const Float_t limit = 0.25;
+
+    if (sgn(vt->Az()) != sgn(vcalc.Az()) &&
+        fabs(vt->Az()) < limit*fabs(vcalc.Az()))
+        vt->Az(0);
     else
         if (fabs(vt->Az()) > 0.9*vraz)
             vt->Az(0.9*vraz*sgn(vt->Az()));
 
-    if (sgn(vt->Zd()) != sgn(vcalc.Zd()))
-        vt->Zd(0.1*vcalc.Zd());
+    if (sgn(vt->Zd()) != sgn(vcalc.Zd()) &&
+        fabs(vt->Zd()) < limit*fabs(vcalc.Zd()))
+        vt->Zd(0);
     else
         if (fabs(vt->Zd()) > 0.9*vrzd)
@@ -392 +384 @@
     if (!SetPosition(dest))
     {
-        lout << "ERROR: Cannot start tracking, unable to reach requested position." << endl;
+        lout << "Error: Cannot start tracking, positioning failed." << endl;
         return;
     }
@@ -421 +413 @@
 
     lout << "Start tracking:";
-    lout << " Ra: " << Rad2Deg(dst.Ra())  << "deg  ";
-    lout << "Dec: " << Rad2Deg(dst.Dec()) << "deg" << endl;
+    lout << " Ra: " << Rad2Deg(dst.Ra())  << "\xb0  ";
+    lout << "Dec: " << Rad2Deg(dst.Dec()) << "\xb0" << endl;
 
     //
@@ -431 +423 @@
 
     ofstream fout("cosy.pos");
-    fout << "   Mjd/10ms    V/re/min/4" << endl;
+    fout << "Tracking:";
+    fout << " Ra: " << Rad2Deg(dst.Ra())  << "\x9c  ";
+    fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
+    fout << "     Mjd/10ms    V/re/min/4" << endl;
 
     //
@@ -450 +445 @@
         dest = CorrectTarget(GetSePos(), sla.CalcZdAz(dst)); // [se]
 
-        //ZdAz vcalc = sla.GetApproxVel(dst) * kGearRatio2*4./60.;  // [re/min]
+        ZdAz vcalc = sla.GetApproxVel(dst) * kGearRatio2*4./60.;  // [re/min]
 
         //
@@ -478 +473 @@
         //
         ZdAz vt = v/4;
-        //LimitSpeed(&vt, vcalc);
+        LimitSpeed(&vt, vcalc);
         vt.Round();
 
@@ -484 +479 @@
         // check if the drive is fast enough to follow the star
         //
-        if (v.Zd()>.9*fMac1->GetVelRes() || v.Az()>.9*fMac2->GetVelRes())
+        if (vt.Zd()>.9*fMac1->GetVelRes() || vt.Az()>.9*fMac2->GetVelRes())
         {
-            lout << "Error: Tracking speed faster than possible maximum velocity." << endl;
+            lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
             break;
         }
@@ -506 +501 @@
         fout << setw(4) << vt.Zd() << " ";
         fout << setw(4) << vt.Az() << endl;
+        //
+        // FIXME? Calculate an accuracy for the tracking system?
+        // How good do we reach the calculated position in 'real'
+        // re valus?
+        //
+
 
         //
@@ -516 +517 @@
 
     fTracking = kFALSE;
-
-    //
-    // Stop revolution mode
-    //
+    StopMovement();
+    lout << "Tracking stopped." << endl;
+}
+
+int MCosy::IsPositioning() const
+{
+    return (fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO();
+}
+
+void MCosy::WaitForEndMovement()
+{
+    WaitForSdos();
+
+    while (IsPositioning())
+        usleep(1);
+}
+
+void MCosy::StopMovement()
+{
     SetStatus(kStopping);
 
+    cout << "Stopping  positioning..." << endl;
+    fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
+    fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
+
+    cout << "Stoping possible RPM mode..." << endl;
+    fMac1->SetRpmMode(FALSE);
     fMac2->SetRpmMode(FALSE);
-    fMac1->SetRpmMode(FALSE);
-
-    SetStatus(kStopped);
-
-    lout << "Tracking stopped." << endl;
+
+    cout << "Waiting for silence..." << endl;
+    WaitForEndMovement();
 
     CheckForError();
+
+    cout << "Movement stopped." << endl;
 }
 
@@ -541 +563 @@
 
     case WM_STOP:
-        cout << "Stopping  positioning..." << endl;
-        SetStatus(kStopping);
-        fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
-        fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
-        cout << "Stoping possible RPM mode..." << endl;
-        fMac1->SetRpmMode(FALSE);
-        fMac2->SetRpmMode(FALSE);
-        cout << "Stopping commands sent." << endl;
-        while (fMac1->IsPositioning() || fMac2->IsPositioning())
-            usleep(1);
-        SetStatus(kStopped);
-        cout << "Positioning stopped." << endl;
+        StopMovement();
         return NULL;
 
@@ -698 +709 @@
         // wait until a tracking session is started
         //
-        while (!fTracking)
+        while (!fTracking && !fTTalk->HasStopFlag())
             usleep(1);
 
+        if (fTTalk->HasStopFlag())
+            break;
+
+        ofstream fout("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;
-        ZdAz ist   = fTrackingError/kGearRatio;     // [se]
-        ZdAz istre = fOffset;                       // [re]
-
-        //
-        // only update fTrackingError while tracking
-        //
+        ZdAz ist;
+
         ZdAz sollzd;
         ZdAz sollaz;
-        while (fTracking)
+
+        ZdAz istre = -fOffset;                // [re]
+        ZdAz time;
+
+        //
+        // only update fTrackingError while tracking
+        //
+        while (fTracking && !fTTalk->HasStopFlag())
         {
+            //
+            // Make changes (eg wind) smoother - attenuation of control function
+            //
+            const float weight = 0.3;
+
+            ZdAz offset(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight,
+                        fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
+
+            fOffset = offset;
+
             //
             // This is the time constant which defines how fast
@@ -719 +753 @@
 
             //
-            // Make changes (eg wind) smoother - attenuation of control function
-            //
-            fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
-                               (ist.Az()-sollaz.Az())*kGearRatio.Y());
-
-
-            const float weight = 0.3;
-
-            ZdAz offset(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight,
-                        fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight);
-
-            fOffset = offset;
-            // fTrackingError.Zd(((offset.Zd()>1000)||(offset.Zd()<-1000))?0:offset.Zd());
-            // fTrackingError.Az(((offset.Az()>1000)||(offset.Az()<-1000))?0:offset.Az());
-
-            //
             // get position, where we are
             //
+            old = ist;
             ist = GetSePos(); // [se]
 
@@ -750 +769 @@
             //
             // Get time from last shaftencoder position change (position: ist)
-            //
-            const double tzd = (fAlt1->GetMjd()+fAlt2->GetMjd())/2.0;
-            const double taz = fAz->GetMjd();
+            // FIXME: I cannot take the avarage
+            //
+            time.Zd((fAlt1->GetMjd()+fAlt2->GetMjd())/2.0);
+            time.Az(fAz->GetMjd());
 
             //
@@ -760 +780 @@
             if ((int)ist.Zd() != (int)old.Zd())
             {
-                sla.SetMjd(tzd);
+                sla.SetMjd(time.Zd());
                 sollzd = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]
             }
@@ -766 +786 @@
             if ((int)ist.Az() != (int)old.Az())
             {
-                sla.SetMjd(taz);
+                sla.SetMjd(time.Az());
                 sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se]
             }
 
-            old = ist;
+            fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
+                               (ist.Az()-sollaz.Az())*kGearRatio.Y());
+
+            fout << setprecision(15) << setw(15) << time.Zd()*60.*60.*24. << " ";
+            fout << setprecision(5)  << setw(5)  << fTrackingError.Zd() << "  ";
+            fout << setprecision(15) << setw(15) << time.Az()*60.*60.*24. << " ";
+            fout << setprecision(5)  << setw(5)  << fTrackingError.Az() << endl;
         }
+
+        fout << endl << endl;
     }
 }
@@ -814 +842 @@
 void MCosy::Stop()
 {
-    cout << "- Stopping GUI update." << endl;
+
+    lout << "- Stopping GUI update." << endl;
     fUpdateGui->TurnOff();
-    //    delete fTGui;
-    lout << "- GUI Thread stopped." << endl;
+    lout << "- GUI Update stopped." << endl;
 
     delete fTTalk;