Changeset 12770 for trunk/FACT++/src/drivectrl.cc

Timestamp:

01/26/12 10:05:38 (13 years ago)

Author:

tbretz

Message:

Added a new dim service SOURCE_POSITION and a new command WOBBLE.

File:

• trunk/FACT++/src/drivectrl.cc (modified) (11 diffs)

trunk/FACT++/src/drivectrl.cc

@@ -136 +136 @@
     }
 
+public:
+    virtual void UpdateSource()
+    {
+    }
+    virtual void UpdateSource(const array<double, 6> &)
+    {
+    }
 
 protected:
@@ -587 +594 @@
 {
 private:
-
     DimDescribedService fDimPointing;
     DimDescribedService fDimTracking;
+    DimDescribedService fDimSource;
     DimDescribedService fDimTPoint;
     DimDescribedService fDimStatus;
 
-    virtual void UpdatePointing(const Time &t,
-                                const array<double, 2> &arr)
+    void UpdatePointing(const Time &t, const array<double, 2> &arr)
     {
         fDimPointing.setData(arr);
@@ -600 +606 @@
     }
 
-    virtual void UpdateTracking(const Time &t,
-                                const array<double, 7> &arr)
+    void UpdateTracking(const Time &t,const array<double, 7> &arr)
     {
         fDimTracking.setData(arr);
         fDimTracking.Update(t);
     }
-    virtual void UpdateStatus(const Time &t,
-                              const array<uint8_t, 3> &arr)
+
+    void UpdateStatus(const Time &t, const array<uint8_t, 3> &arr)
     {
         fDimStatus.setData(arr);
         fDimStatus.Update(t);
     }
-    virtual void UpdateTPoint(const Time &t,
-                              const Drive::DimTPoint &data,
-                              const string &name)
+
+    void UpdateTPoint(const Time &t, const Drive::DimTPoint &data,
+                      const string &name)
     {
         vector<char> dim(sizeof(data)+name.length()+1);
@@ -630 +635 @@
                      "|Zd[deg]:Zenith distance (encoder readout)"
                      "|Az[deg]:Azimuth angle (encoder readout)"),
-        fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1",
+        fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1",
                      "|Ra[h]:Command right ascension"
                      "|Dec[deg]:Command declination"
@@ -638 +643 @@
                      "|dZd[deg]:Control deviation Zd"
                      "|dAz[deg]:Control deviation Az"),
+        fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1",
+                     "|Ra_cmd[h]:Command right ascension"
+                     "|Dec_cmd[deg]:Command declination"
+                     "|Ra_src[h]:Source right ascension"
+                     "|Dec_src[deg]:Source declination"
+                     "|Offfset[deg]:Wobble offset"
+                     "|Angle[deg]:Wobble angle"),
         fDimTPoint("DRIVE_CONTROL/TPOINT", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;S:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;C",
                    "|Ra[h]:Command right ascension"
@@ -661 +673 @@
     }
 
+    void UpdateSource()
+    {
+        const array<double, 6> arr = {{ 0, 0, 0, 0, 0, 0 }};
+        fDimSource.setQuality(0);
+        fDimSource.Update(arr);
+    }
+
+    void UpdateSource(const array<double, 6> &arr)
+    {
+        fDimSource.setQuality(1);
+        fDimSource.Update(arr);
+    }
+
     // A B [C] [D] E [F] G H [I] J K [L] M N O P Q R [S] T U V W [X] Y Z
 };
@@ -739 +764 @@
     }
 
-    int SendCommand(const string &str)
+    int SendCommand(const string &str, bool upd=true)
     {
         fDrive.PostMessage(str);
         T::Message("Sending: "+str);
+
+        if (upd)
+            fDrive.UpdateSource();
+
         return T::GetCurrentState();
     }
@@ -762 +791 @@
         }
 
+        if (type!=kPoint)
+        {
+            const array<double, 6> dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }};
+            fDrive.UpdateSource(dim);
+        }
+
         command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
-
-        return SendCommand(command);
+        return SendCommand(command, type==kPoint);
+    }
+
+    int Wobble(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "Wobble", 32))
+            return T::kSM_FatalError;
+
+        const double *dat = evt.Ptr<double>();
+
+        const double ra  = dat[0]*M_PI/12;
+        const double dec = dat[1]*M_PI/180;
+        const double off = dat[2]*M_PI/180;
+        const double dir = dat[3]*M_PI/180;
+
+        const double cosdir = cos(dir);
+        const double sindir = sin(dir);
+        const double cosoff = cos(off);
+        const double sinoff = sin(off);
+        const double cosdec = cos(dec);
+        const double sindec = sin(dec);
+
+        if (off==0)
+        {
+            const array<double, 6> dim = {{ ra, dec, ra, dec, 0, 0 }};
+            fDrive.UpdateSource(dim);
+
+            string command = "RADEC ";
+            command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]);
+            return SendCommand(command, false);
+        }
+
+        const double costheta = cosdec*cosoff + sindec*sinoff*cosdir;
+        if (costheta >= 1)
+        {
+            T::Error("cos(Zd) > 1");
+            return T::GetCurrentState();
+        }
+
+        const double sintheta = sqrt(1 - costheta*costheta);
+
+        const double cosdeltara = (cosoff - cosdec*costheta)/(sindec*sintheta);
+        const double sindeltara = sindir*sinoff/sintheta;
+
+        const double ndec = acos(costheta)*180/M_PI;
+        const double nra  = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
+
+        const array<double, 6> dim = {{ ra, dec, nra, ndec, off, dir }};
+        fDrive.UpdateSource(dim);
+
+        string command = "RADEC ";
+        command += AngleToStr(nra) + ' ' + AngleToStr(ndec);
+        return SendCommand(command, false);
     }
 
@@ -776 +862 @@
         ostringstream cmd;
         cmd << "LEDS " << led[0] << " " << led[1];
-        return SendCommand(cmd.str());
+        return SendCommand(cmd.str(), false);
     }
 
@@ -906 +992 @@
              "|Dec[deg]:Declination");
 
+        T::AddEvent("WOBBLE", "D:4", kStateArmed)   // ->RADEC/GRB
+            (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
+            ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
+             "|Ra[h]:Right ascension"
+             "|Dec[deg]:Declination"
+             "|Offset[deg]:Wobble offset"
+             "|Angle[deg]:Wobble angle");
+
         T::AddEvent("MOON", kStateArmed)
-            (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0"))
+            (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
             ("Start tracking the moon");
         T::AddEvent("VENUS", kStateArmed)
-            (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0"))
+            (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
             ("Start tracking Venus");
         T::AddEvent("MARS", kStateArmed)
-            (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0"))
+            (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
             ("Start tracking Mars");
         T::AddEvent("JUPITER", kStateArmed)
-            (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0"))
+            (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
             ("Start tracking Jupiter");
         T::AddEvent("SATURN", kStateArmed)
-            (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0"))
+            (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
             ("Start tracking Saturn");
 
         T::AddEvent("TAKE_TPOINT")
-            (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0"))
+            (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
             ("Take a TPoint");
 
@@ -933 +1027 @@
 
         T::AddEvent("LEDS_OFF")
-            (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0"))
+            (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false))
             ("Switch off TPoint LEDs");
 
         T::AddEvent("STOP")
-            (bind(&StateMachineDrive::SendCommand, this, "STOP!"))
+            (bind(&StateMachineDrive::SendCommand, this, "STOP!", true))
             ("Stop any kind of movement.");