Changeset 9132

Timestamp:

08/26/08 12:11:32 (16 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy

Files:

• Changelog (modified) (1 diff)
• candrv/canopen.cc (modified) (1 diff)
• candrv/canopen.h (modified) (1 diff)
• candrv/network.cc (modified) (2 diffs)
• candrv/nodedrv.cc (modified) (1 diff)
• cosy.cc (modified) (2 diffs)
• devdrv/DevdrvLinkDef.h (modified) (1 diff)
• devdrv/Makefile (modified) (1 diff)
• devdrv/dkc.cc (added)
• devdrv/dkc.h (added)
• gui/MGCosy.cc (modified) (11 diffs)
• gui/MGCosy.h (modified) (6 diffs)
• gui/MGSkyPosition.cc (modified) (5 diffs)
• gui/MGSkyPosition.h (modified) (1 diff)
• main/MCosy.cc (modified) (42 diffs)
• main/MCosy.h (modified) (8 diffs)
• main/MSlewing.cc (modified) (10 diffs)
• main/MSlewing.h (modified) (2 diffs)
• main/MStarguider.cc (modified) (7 diffs)
• main/MTracking.cc (modified) (18 diffs)
• main/MTracking.h (modified) (1 diff)

trunk/MagicSoft/Cosy/Changelog

@@ -1 +1 @@
                                                                   -*-*- END -*-*-
+
+ 2008/08/26 Thomas Bretz (La Palma)
+
+ * cosy.cc:
+     - changed ceco to 151.72.130.60
+
+   * candrv/canopen.[h,cc]:
+     - added HasConnection
+
+   * candrv/network.cc:
+     - check for connection before trying to reboot zombies
+
+   * candrv/nodedrv.cc:
+     - some improvement to output
+
+   * devdrv/DevdrvLinkDef.h, devdrv/Makefile:
+     - added Dkc
+
+   * gui/MGCosy.[h,cc]:
+     - change to layout
+     - display DKC information
+     - display cabinet information
+
+   * gui/MGSkyPosition.[h,cc]:
+     - for convinience added Polaris to display
+
+   * main/MCosy.[h,cc]:
+     - removed shaftencoder and macs
+     - removed a lot of old obsolete code
+     - added Dkc
+     - set new velocity and acceleration values
+     - removed a lot of expert stuff
+     - removed ambiguity between feedback 1 and 2
+
+   * main/MSlewing.[h,cc]:
+     - replaced Macs by Dkc
+     - fixed to use new units
+     - scale velocity even with different maximum speeds
+     - changed from relative positionuing to absolute positioning
+     - implemented a new correction for the non-uniformity of the el-axis
+
+   * main/MStarguider.cc:
+     - changed usage of MString
+     - use GetSePos now instead of GetTrackingPosRaw
+
+   * main/MTracking.[h,cc]:
+     - replaced Macs by Dkc
+     - removed ambiguity of feedback 1 and feedback 2
+     - adapted definition of control deviation
+
+
 
  2008/07/13 Thomas Bretz

trunk/MagicSoft/Cosy/candrv/canopen.cc

@@ -78 +78 @@
     if (fInterface)
         fInterface->Stop();
+}
+
+Bool_t CanOpen::HasConnection() const
+{
+    return fInterface ? fInterface->HasConnection() : kFALSE;
 }
 

trunk/MagicSoft/Cosy/candrv/canopen.h

@@ -101 +101 @@
 
     void SetInterface(Interface *f) { fInterface=f; } // Set Handle to the interface
+    Bool_t HasConnection() const;
 
     // Send a Process Data Object (PDO) to the CanOpen bus

trunk/MagicSoft/Cosy/candrv/network.cc

@@ -356 +356 @@
 bool Network::RebootZombies()
 {
+    if (!HasConnection())
+    {
+        gLog << warn << "- No connection to network." << endl;
+        return false;
+    }
+
     bool rc = true;
 
-    gLog << "- Trying to reboot all Zombies..." << endl;
+    gLog << inf2 << "- Trying to reboot all Zombies..." << endl;
     for (int i=0; i<32; i++)
         if (fNodes[i])
@@ -364 +370 @@
                 if (!fNodes[i]->Reboot())
                 {
-                    gLog << "- Failed to reboot " << fNodes[i]->GetNodeName() << "." << endl;
+                    gLog << err << "- Failed to reboot " << fNodes[i]->GetNodeName() << "." << endl;
                     rc = false;
                 }

trunk/MagicSoft/Cosy/candrv/nodedrv.cc

@@ -427 +427 @@
         return true;
 
-    gLog << inf2 << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " --> ZOMBIE! " << MTime(-1) << endl;
+    gLog << inf2 << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " ";
     if (zombie)
+    {
+        gLog << "--> ZOMBIE! " << endl;
         SetZombie();
+     }
+
+    gLog << MTime(-1) << endl;
+
     return false;
 }

trunk/MagicSoft/Cosy/cosy.cc

@@ -92 +92 @@
     const Bool_t  kDebugThreads = arg.HasOnlyAndRemove("--debug-threads");
     const TString sps           = arg.GetStringAndRemove("--sps=", "sps");
-    const TString ceco          = arg.GetStringAndRemove("--cc=", "ceco"); // ceco
+    const TString ceco          = arg.GetStringAndRemove("--cc=", "161.72.130.60"); // ceco
     const Int_t   ceco_tx       = arg.GetIntAndRemove("--cc-tx=", 7314);//7304);
     const Int_t   ceco_rx       = arg.GetIntAndRemove("--cc-rx=", 7414);//7404);
@@ -197 +197 @@
 
     MDriveCom *com = new MDriveCom(ceco, ceco_tx, ceco_rx);
+    com->SetTelescope(2);
 
     MCosy *cosy = new MCosy(env, com, pointing);

trunk/MagicSoft/Cosy/devdrv/DevdrvLinkDef.h

@@ -5 +5 @@
 #pragma link off all functions;
 
+#pragma link C++ class Dkc+;
 #pragma link C++ class Macs+;
 #pragma link C++ class ShaftEncoder+;

trunk/MagicSoft/Cosy/devdrv/Makefile

@@ -17 +17 @@
 #------------------------------------------------------------------------------
 
-SRCFILES = shaftencoder.cc macs.cc
+SRCFILES = shaftencoder.cc macs.cc dkc.cc
 
 ############################################################

trunk/MagicSoft/Cosy/gui/MGCosy.cc

@@ -131 +131 @@
 {
     const int x = 180;
-    const int y = 25;
+    const int y = 20;
 
     TGLabel *l;
@@ -150 +150 @@
     fList->Add(fMjd);
 
-    l = new TGLabel(f, "SE-Az:");
+    l = new TGLabel(f, "Encoder-Az:");
+    l->Move(x-60, y+57);
+    fList->Add(l);
+
+    l = new TGLabel(f, "Encoder-Zd:");
     l->Move(x-60, y+40);
     fList->Add(l);
 
-    l = new TGLabel(f, "SE-Zd1:");
-    l->Move(x-60, y+57);
-    fList->Add(l);
-
-    l = new TGLabel(f, "SE-Zd2:");
-    l->Move(x-60, y+74);
-    fList->Add(l);
-
     fWeather = new TGLabel(f, "");
-    fWeather->MoveResize(10, y+280, 280, 16);
+    fWeather->MoveResize(10, y+272, 280, 16);
     fList->Add(fWeather);
 
-    fLabel1 = new TGLabel(f, "00000000"); // Max: 16384
-    fLabel2 = new TGLabel(f, "00000000");
-    fLabel3 = new TGLabel(f, "00000000");
+    fLabel1 = new TGLabel(f, "0000000000");
+    fLabel2 = new TGLabel(f, "0000000000");
     fLabel1->SetTextJustify(kTextRight);
     fLabel2->SetTextJustify(kTextRight);
-    fLabel3->SetTextJustify(kTextRight);
-    fLabel1->Move(x, y+40);
-    fLabel2->Move(x, y+57);
-    fLabel3->Move(x, y+74);
+    fLabel1->Move(x+30, y+57);
+    fLabel2->Move(x+30, y+40);
     fList->Add(fLabel1);
     fList->Add(fLabel2);
-    fList->Add(fLabel3);
-/*
-    l = new TGLabel(f, "Offset-Zd:");
-    l->Move(x-60, y+102);
+
+    l = new TGLabel(f, "Ra (estimated):");
+    l->Move(x-60, y+57+23);
     fList->Add(l);
 
-    l = new TGLabel(f, "Offset-Az:");
-    l->Move(x-60, y+119);
+    l = new TGLabel(f, "Ra (nominal):");
+    l->Move(x-60, y+57+40);
     fList->Add(l);
 
-    fOffsetZd = new TGLabel(f, "000000000");
-    fOffsetAz = new TGLabel(f, "000000000");
-    fOffsetZd->SetTextJustify(kTextRight);
-    fOffsetAz->SetTextJustify(kTextRight);
-    fOffsetZd->Move(x+15, y+102);
-    fOffsetAz->Move(x+15, y+119);
-    fList->Add(fOffsetZd);
-    fList->Add(fOffsetAz);
-*/
-    l = new TGLabel(f, "Ra (estimated):");
-    l->Move(x-60, y+142-20);
+    l = new TGLabel(f, "Dec (estimated):");
+    l->Move(x-60, y+57+40+23);
     fList->Add(l);
 
-    l = new TGLabel(f, "Ra (nominal):");
-    l->Move(x-60, y+159-20);
+    l = new TGLabel(f, "Dec (nominal):");
+    l->Move(x-60, y+57+40+40);
     fList->Add(l);
 
-    l = new TGLabel(f, "Dec (estimated):");
-    l->Move(x-60, y+182-20);
+    l = new TGLabel(f, "Zd (nominal):");
+    l->Move(x-60, y+57+80+23);
     fList->Add(l);
 
-    l = new TGLabel(f, "Dec (nominal):");
-    l->Move(x-60, y+199-20);
-    fList->Add(l);
-
-    l = new TGLabel(f, "Zd (nominal):");
-    l->Move(x-60, y+222);
-    fList->Add(l);
-
     l = new TGLabel(f, "Az (nominal):");
-    l->Move(x-60, y+239);
+    l->Move(x-60, y+57+80+40);
     fList->Add(l);
 
@@ -232 +207 @@
     fZdSoll->SetTextJustify(kTextRight);
     fAzSoll->SetTextJustify(kTextRight);
-    fRaEst->Move(x+30, y+142-20);
-    fRaSoll->Move(x+30, y+159-20);
-    fDecEst->Move(x+30, y+182-20);
-    fDecSoll->Move(x+30, y+199-20);
-    fZdSoll->Move(x+30, y+222);
-    fAzSoll->Move(x+30, y+239);
+    fRaEst->Move(x+30, y+57+23);
+    fRaSoll->Move(x+30, y+57+40);
+    fDecEst->Move(x+30, y+57+40+23);
+    fDecSoll->Move(x+30, y+57+40+40);
+    fZdSoll->Move(x+30, y+57+80+23);
+    fAzSoll->Move(x+30, y+57+80+40);
     fList->Add(fRaEst);
     fList->Add(fDecEst);
@@ -251 +226 @@
     fStopping  = new TGLabel(f, "Stopping");
     fStopped   = new TGLabel(f, "Stopped");
-    fAvailMac1 = new TGLabel(f, "- MACS1 -");
-    fAvailMac2 = new TGLabel(f, "- MACS2 -");
-    //fAvailMac3 = new TGLabel(f, "- MAC3 -");
-    fAvailSe1  = new TGLabel(f, "-SE/Zd1-");
-    fAvailSe2  = new TGLabel(f, "-SE/Zd2-");
-    fAvailSe3  = new TGLabel(f, "- SE/Az -");
+    //fAvailDkc1 = new TGLabel(f, "- DKC/Zd -");
+    //fAvailDkc2 = new TGLabel(f, "- DKC/Az -");
+    fAvailTcp  = new TGLabel(f, "- TCP/IP -");
+
+    fDkcReady1    = new TGLabel(f, "bb");
+    fDkcReady2    = new TGLabel(f, "bb");
+    fDkcEmergency = new TGLabel(f, "Emergency");
+    fDkcPcMode    = new TGLabel(f, "PC mode");
+    fDkcRF1       = new TGLabel(f, "AF");
+    fDkcRF2       = new TGLabel(f, "AF");
+    fDkcBrake     = new TGLabel(f, "Zd Brake");
+    fDkcPower     = new TGLabel(f, "Power on");
+
+    fMsgAz     = new TGLabel(f, " xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx");
+    fMsgZd     = new TGLabel(f, " xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx");
 
     ULong_t color;
@@ -264 +248 @@
     gClient->GetColorByName("Red", color);
     fError->SetBackgroundColor(color);
-    fAvailMac1->SetBackgroundColor(color);
-    fAvailMac2->SetBackgroundColor(color);
-    //fAvailMac3->SetBackgroundColor(color);
-    fAvailSe1->SetBackgroundColor(color);
-    fAvailSe2->SetBackgroundColor(color);
-    fAvailSe3->SetBackgroundColor(color);
+    fDkcReady1->SetBackgroundColor(color);
+    fDkcReady2->SetBackgroundColor(color);
+    fDkcEmergency->SetBackgroundColor(color);
+    fDkcPcMode->SetBackgroundColor(color);
+    fDkcRF1->SetBackgroundColor(color);
+    fDkcRF2->SetBackgroundColor(color);
+    fDkcBrake->SetBackgroundColor(color);
+    fDkcPower->SetBackgroundColor(color);
+    fAvailTcp->SetBackgroundColor(color);
     gClient->GetColorByName("LightBlue", color);
     fMoving->SetBackgroundColor(color);
@@ -279 +266 @@
     fStopped->SetBackgroundColor(color);
 
-    fArmed   ->MoveResize(10,  25,        70, 20);
-    fError   ->MoveResize(10,  25+25,     70, 20);
-    fMoving  ->MoveResize(10,  25+25+20,  70, 20);
-    fTracking->MoveResize(10,  25+25+40,  70, 20);
-    fStopping->MoveResize(10,  25+25+60,  70, 20);
-    fStopped ->MoveResize(10,  25+25+80,  70, 20);
-    fAvailMac1->MoveResize(10, 25+25+120, 70, 20);
-    fAvailMac2->MoveResize(10, 25+25+140, 70, 20);
-    //fAvailMac3->Move(10, 25+160);
-    fAvailSe1->MoveResize(10,  25+25+200, 70, 20);
-    fAvailSe2->MoveResize(10,  25+25+220, 70, 20);
-    fAvailSe3->MoveResize(10,  25+25+180, 70, 20);
+    gClient->GetColorByName("White", color);
+    fMsgAz->SetBackgroundColor(color);
+    fMsgZd->SetBackgroundColor(color);
+
+    fArmed   ->MoveResize(10,  20,        70, 20);
+    fError   ->MoveResize(10,  20+25,     70, 20);
+    fMoving  ->MoveResize(10,  20+25+20,  70, 20);
+    fTracking->MoveResize(10,  20+25+40,  70, 20);
+    fStopping->MoveResize(10,  20+25+60,  70, 20);
+    fStopped ->MoveResize(10,  20+25+80,  70, 20);
+
+    fAvailTcp    ->MoveResize( 10, 80+20+102, 75, 20);
+
+    fDkcEmergency->MoveResize( 10, 80+20+124, 75, 20);
+    fDkcPcMode   ->MoveResize( 89, 80+20+124, 75, 20);
+    fDkcPower    ->MoveResize(168, 80+20+124, 75, 20);
+    fDkcBrake    ->MoveResize(247, 80+20+124, 75, 20);
+
+    fDkcReady1->MoveResize( 10,    248+2, 16, 16);
+    fDkcReady2->MoveResize( 10,    268+0, 16, 16);
+    fDkcRF1   ->MoveResize( 10+18, 248+2, 18, 16);
+    fDkcRF2   ->MoveResize( 10+18, 268+0, 18, 16);
+
+    fMsgAz->MoveResize(10+38, 248+2, 310-34, 16); // x-95
+    fMsgZd->MoveResize(10+38, 268+0, 310-34, 16); // x-95
+
+    fMsgAz->SetTextJustify(kTextLeft);
+    fMsgZd->SetTextJustify(kTextLeft);
 
     fList->Add(fArmed);
@@ -298 +301 @@
     fList->Add(fStopping);
     fList->Add(fStopped);
-    fList->Add(fAvailMac1);
-    fList->Add(fAvailMac2);
-    //fList->Add(fAvailMac3);
-    fList->Add(fAvailSe1);
-    fList->Add(fAvailSe2);
-    fList->Add(fAvailSe3);
+    //fList->Add(fAvailDkc1);
+    //fList->Add(fAvailDkc2);
+    fList->Add(fAvailTcp);
+
+    fList->Add(fDkcReady1   );
+    fList->Add(fDkcReady2   );
+    fList->Add(fDkcEmergency);
+    fList->Add(fDkcPcMode   );
+    fList->Add(fDkcRF1      );
+    fList->Add(fDkcRF2      );
+    fList->Add(fDkcBrake    );
+    fList->Add(fDkcPower    );
+
+    fList->Add(fMsgZd);
+    fList->Add(fMsgAz);
 }
 
@@ -716 +728 @@
 }
 
-MGCosy::MGCosy(MObservatory::LocationName_t key, MsgQueue *q, const TGWindow *p, UInt_t w, UInt_t h)
-    : TGMainFrame(p, w, h), fObservatory(key), fQueue(q)
+MGCosy::MGCosy(MObservatory::LocationName_t key, MsgQueue *q)
+    : TGMainFrame(gClient->GetRoot(), 1, 1), fObservatory(key), fQueue(q)
 {
     // This means that all objects added with AddFrame are deleted
@@ -806 +818 @@
     CreateLabel(frame);
 
+    
     //
     //   Map the window, set up the layout, etc.
     //
     const Int_t ww = 1010;
-    const Int_t hh =  700;
+    const Int_t hh =  680;
     SetWMSizeHints(ww, hh, ww, hh, 1, 1);  // set the smallest and biggest size of the Main frame
 
@@ -1038 +1051 @@
 }
 
-void MGCosy::Update(ZdAz pos, ZdAz acc, ZdAz vel, /*ZdAz off,*/ RaDec radec,
-                    ZdAz soll, UInt_t stat, UInt_t stat2, Bool_t armed)
+void MGCosy::Update(ZdAz pos, ZdAz acc, /*ZdAz vel, ZdAz off,*/ RaDec radec,
+                    ZdAz soll, UInt_t stat, UInt_t pdo3, UInt_t stat2, Bool_t armed, const TString &statzd, const TString &stataz)
 {
     // acc [rad]
@@ -1060 +1073 @@
     EnableLabel(fAzSoll,   stat&MDriveCom::kMoving);
 
-    SetLabelColor(fAvailMac1, stat2&0x01);
-    SetLabelColor(fAvailMac2, stat2&0x02);
-    //SetLabelColor(fAvailMac3, stat2&0x04);
-    SetLabelColor(fAvailSe1,  stat2&0x08);
-    SetLabelColor(fAvailSe2,  stat2&0x10);
-    SetLabelColor(fAvailSe3,  stat2&0x20);
-
-    EnableLabel(fArmed, armed);//stat2&0x40);
+    SetLabelColor(fAvailTcp, stat2);
+
+    if (!stat2)
+        pdo3 = 0;
+
+    SetLabelColor(fDkcReady1,       pdo3&0x0001);
+    SetLabelColor(fDkcReady2,       pdo3&0x0100);
+    SetLabelColor(fDkcEmergency,   (pdo3&0x0404)==0x0404);
+    SetLabelColor(fDkcPcMode,      (pdo3&0x1010)!=0x1010 && pdo3);
+    SetLabelColor(fDkcRF1,          pdo3&0x0020);
+    SetLabelColor(fDkcRF2,          pdo3&0x2000);
+    SetLabelColor(fDkcBrake,       (pdo3&0x4040)==0x4040);
+    SetLabelColor(fDkcPower,       (pdo3&0x8080)==0x8080);
+
+    EnableLabel(fArmed, armed);
+
+    fMsgZd->SetText(new TGString(TString(" Zd: ")+statzd));
+    fMsgAz->SetText(new TGString(TString(" Az: ")+stataz));
+
 
     if (stat&MDriveCom::kTracking)

trunk/MagicSoft/Cosy/gui/MGCosy.h

@@ -49 +49 @@
     TGLabel       *fLabel1;
     TGLabel       *fLabel2;
-    TGLabel       *fLabel3;
+    // TGLabel       *fLabel3;
 
     // TGLabel       *fOffsetZd;
@@ -62 +62 @@
     TGLabel       *fZdSoll;
     TGLabel       *fAzSoll;
+
+    TGLabel       *fMsgZd;
+    TGLabel       *fMsgAz;
 
     TGLabel       *fUtc;
@@ -77 +80 @@
     MGSkyPosition  *fSkyPosition;
     MGAccuracy     *fAccuracy;
-    //MGAccuracyTime *fAccuracyTime;
     MGVelocity     *fVelocity;
     MGVelocity     *fOffset;
@@ -93 +95 @@
     TGLabel *fStopping;
     TGLabel *fStopped;
-    TGLabel *fAvailMac1;
-    TGLabel *fAvailMac2;
-    TGLabel *fAvailMac3;
-    TGLabel *fAvailSe1;
-    TGLabel *fAvailSe2;
-    TGLabel *fAvailSe3;
+    //TGLabel *fAvailDkc1;
+    //TGLabel *fAvailDkc2;
+    TGLabel *fAvailTcp;
+
+    TGLabel *fDkcReady1;
+    TGLabel *fDkcReady2;
+    TGLabel *fDkcEmergency;
+    TGLabel *fDkcPcMode;
+    TGLabel *fDkcRF1;
+    TGLabel *fDkcRF2;
+    TGLabel *fDkcBrake;
+    TGLabel *fDkcPower;
 
     TGTextView *fLog;
@@ -125 +133 @@
 
 public:
-    MGCosy(MObservatory::LocationName_t key, MsgQueue *q, const TGWindow *p, UInt_t w, UInt_t h);
+    MGCosy(MObservatory::LocationName_t key, MsgQueue *q);
     ~MGCosy();
 
@@ -132 +140 @@
     TGLabel *GetLabel1() const { return fLabel1; }
     TGLabel *GetLabel2() const { return fLabel2; }
-    TGLabel *GetLabel3() const { return fLabel3; }
+    //TGLabel *GetLabel3() const { return fLabel3; }
 
     TGTextView *GetLog() const { return fLog; }
     MGImage    *GetImage()     { return fImage; }
 
-    void Update(ZdAz pos, ZdAz acc, ZdAz vel, /*ZdAz off,*/ RaDec radec, ZdAz soll, UInt_t stat, UInt_t stat2, Bool_t armed);
+    void Update(ZdAz pos, ZdAz acc, /*ZdAz vel, ZdAz off,*/ RaDec radec, ZdAz soll, UInt_t stat, UInt_t pdo3, UInt_t stat2, Bool_t armed, const TString &statzd, const TString &stataz);
     void UpdateWeather(const MCeCoCom &com);
 

trunk/MagicSoft/Cosy/gui/MGSkyPosition.cc

@@ -74 +74 @@
     InitArc(fSaturn,  1001, kYellow/*17*/, 16);
 
-    for (int i=0; i<14; i++)
+    for (int i=0; i<15; i++)
     {
         fStars[i] = new TArc(0, 0, 1);
@@ -115 +115 @@
 }
 
-static const RaDec stars[14] =
+static const RaDec stars[15] =
 {
     // 32349   Sirius           -1.44 1.45    2.64      0.009   A0m
@@ -138 +138 @@
     (RaDec(MAstro::Hms2Rad(  5, 16,  0), MAstro::Dms2Rad( 46, 00,  0))),
     (RaDec(MAstro::Hms2Rad(  7, 39,  0), MAstro::Dms2Rad(  5, 14,  0))),
-    (RaDec(MAstro::Hms2Rad(  5, 55,  0), MAstro::Dms2Rad(  7, 24,  0)))
+    (RaDec(MAstro::Hms2Rad(  5, 55,  0), MAstro::Dms2Rad(  7, 24,  0))),
+    //         Polaris           2.02
+    (RaDec(MAstro::Hms2Rad(  2, 31, 49), MAstro::Dms2Rad( 89, 15, 51))),
 };
 
 void MGSkyPosition::UpdateStars()
 {
-    for (int i=0; i<14; i++)
+    for (int i=0; i<15; i++)
         SetDot(fStars[i], stars[i], 0);
 }
@@ -370 +372 @@
     fSlaStar->SetMjd(mjd);
 
+    UpdateStars();
+
     UpdatePlanet(kESun,     fSun);
     UpdatePlanet(kEMoon,    fMoon);
@@ -377 +381 @@
     UpdatePlanet(kESaturn,  fSaturn);
 
-    UpdateStars();
-
     RaDec radec = fSlaStar->CalcRaDecFast(pos*TMath::DegToRad());
     UpdatePosition(radec, pos.Zd(), pos.Az());

trunk/MagicSoft/Cosy/gui/MGSkyPosition.h

@@ -35 +35 @@
     TArc       *fSaturn;
 
-    TArc       *fStars[14];
+    TArc       *fStars[15];
 
     TArc       *fSaturnRing;

trunk/MagicSoft/Cosy/main/MCosy.cc

@@ -7 +7 @@
 #include <TTimer.h>
 #include <TApplication.h>
-
-#include <TH2.h>
-#include <TH3.h>
-#include <TProfile.h>
-#include <TCanvas.h>
 
 //#include "MLog.h"
@@ -26 +21 @@
 #include "MTracking.h"
 
-#include "macs.h"
-#include "shaftencoder.h"
+#include "dkc.h"
 
 ClassImp(MCosy);
@@ -34 +28 @@
 
 typedef struct tm tm_t;
-
-/* +===================================+
-    FIXME: What if fMac3 (Sync) died?
-   +===================================+
-*/
 
 //#define EXPERT
@@ -114 +103 @@
 }
 */
-
-MTTalk::~MTTalk()
-{
-    gLog << inf2 << "~MTTalk::CancelThread" << std::endl;
-    CancelThread();
-    gLog << inf2 << "~MTTalk::MTTalk" << std::endl;
-}
-
 // --------------------------------------------------------------------------
 //
@@ -136 +117 @@
 ZdAz MCosy::GetSePos() const
 {
-    const int pa = fAz->GetPos();
-    if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
-        return ZdAz(0, pa);
-
-    //
-    // Get the values          (FIXME!)
-    //
-    //int p1 =  (fZd1->GetPos()+fZd1->GetPhysRes()/2)%fZd1->GetPhysRes();
-    //int p2 = -(fZd2->GetPos()+fZd2->GetPhysRes()/2)%fZd2->GetPhysRes();
-
-    int p1 =  fZd1->GetPos();//+fZd1->GetPhysRes()/2)%fZd1->GetPhysRes();
-    int p2 = -fZd2->GetPos();//+fZd2->GetPhysRes()/2)%fZd2->GetPhysRes();
-
-    if (fZd1->IsZombieNode())
-        return ZdAz(p2, pa);
-    if (fZd2->IsZombieNode())
-        return ZdAz(p1, pa);
-
-    //
-    // interpolate shaft encoder positions
-    //
-    float p = (float)(p1+p2)/2;
-
-    return ZdAz(p, pa);
-}
-
-// --------------------------------------------------------------------------
-//
-//  reads the Rotary encoder positions from the last request of the Macs.
-//
-//  The positions are returned as a ZdAz object. Use RequestRePos to request
-//  the current positions first.
-//
-ZdAz MCosy::GetRePos()
-{
-    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
-}
-
-// --------------------------------------------------------------------------
-//
-//  reads the Rotary encoder positions from the Macs.
-//
-//  The positions are returned as a ZdAz object. The positions are the ones
-//  which are send as PDOs to the computer. This is done at a given
-//  frequency. Which means, that this positions are not ought to be
-//  up-to-date.
-//
-ZdAz MCosy::GetRePosPdo()
-{
-    return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
+    const Double_t pa = fMac1 ? (Double_t)fMac1->GetPdoPos2()/fMac1->GetPosRes()  : 0;
+    const Double_t p1 = fMac2 ? (Double_t)fMac2->GetPdoPos2()/fMac2->GetPosRes() : 0;
+
+    return ZdAz(p1, pa);
 }
 
@@ -323 +258 @@
     }
 
-    const ZdAz se = GetSePos()*TMath::TwoPi()/kResSE;   // [rad]
+    const ZdAz se = GetSePos()*TMath::TwoPi();   // [rad]
     const ZdAz unbendedse = fBending.CorrectBack(se)*TMath::RadToDeg(); // ist pointing
 
@@ -350 +285 @@
 }
 
+/*
 Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
 {
@@ -365 +301 @@
 
     ZdAz point=AlignTrackingPos(ZdAz(zd, az)/TMath::RadToDeg());
-    /*
-    if (!AlignTrackingPos(ZdAz(zd, az), point))
-    {
-        cout << "Starguider position couldn't be aligned..." << endl;
-        return -1;
-    }*/
-
-    // FIXME: Check Range missing!
 
     const ZdAz diff = (dest-point)*TMath::RadToDeg();
@@ -389 +317 @@
     return (mjd-mjd0) * (24*60*60); // [s]
 }
+*/
 
 // --------------------------------------------------------------------------
@@ -405 +334 @@
     MSlewing point(this);
 
-//#ifdef EXPERT
-//    point.SetPointAccDec(0.25, 0.25);
-//    point.SetPointVelocity(0.4);
-//#else
-
-// original settings
-//
-    point.SetPointAccDec(0.2, 0.1);
-    point.SetPointVelocity(0.1);
-
-
-//#endif
-
-// original
+    // Default: point.SetPointAcc(0.03, 0.01);
+    point.SetPointAcc(0.03, 0.01);
+    point.SetPointVelocity(0.3);
+
     return point.SetPosition(dst, track);
-
-// test
-//    return point.SetPosition(dst, kTRUE);
-}
-
-void MCosy::SetTrackingPosRE(ZdAz za)
-{
-    za /= kGearTot;                        // [U_tel]
-    za *= TMath::TwoPi();                  // [rad]
-    //cout << "RE1:  " << za.Zd()*180/3.1415 << " " << za.Az()*180/3.1415 << endl;
-    fTrackingPosRaw = za*TMath::RadToDeg();
-    fTrackingPos    = fBending.CorrectBack(za)*TMath::RadToDeg();
-    //cout << "RE2:  " << fTrackingPos.Zd() << " " << fTrackingPos.Az() << endl;
 }
 
@@ -439 +345 @@
     MTracking track(this);
     track.SetOut(fOutRep);
-//#ifdef EXPERT
-//    track.SetPointAccDec(0.4, 0.4);
-//    track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
-//#else
-
-
-
-    track.SetPointAccDec(0.2, 0.1);
-    track.SetPointVelocity(0.1);
-
-    // track.SetPointAccDec(0.4, 0.4);
-    // track.SetPointVelocity(0.4);
-
-
-
-//#endif
-    track.SetTrackAccDec(0.1, 0.1);
+
+    track.SetPointAcc(0.03, 0.01);
+    track.SetPointVelocity(0.3);
+    track.SetTrackAcc(0.01, 0.01);
 
     track.TrackPosition(dst);
@@ -462 +355 @@
 void MCosy::TrackPositionGRB(const RaDec &dst) // ra, dec [rad]
 {
+    TrackPosition(dst);
+    return;
+
     MTracking track(this);
     track.SetOut(fOutRep);
-//#ifdef EXPERT
-//    track.SetPointAccDec(0.4, 0.4);
-//    track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
-//#else
-    track.SetPointAccDec(0.4, 0.4);
-    track.SetPointVelocity(0.3);
-//#endif
-    track.SetTrackAccDec(0.1, 0.1);
+    track.SetPointAcc(0.09, 0.03);
+    track.SetPointVelocity(1.0);
+    track.SetTrackAcc(0.01, 0.01);
 
     track.TrackPosition(dst);
@@ -493 +384 @@
     // set deceleration to 50%
     //
-    gLog << inf2 << "Stopping movement (dec=30%)..." << endl;
+    gLog << inf2 << "Stopping movement..." << endl;
     if (fMac1 && fMac2)
     {
-#ifdef EXPERT
-        fMac1->SetDeceleration(TMath::Nint(0.5*fMac1->GetVelRes()));
-        fMac2->SetDeceleration(TMath::Nint(0.5*fMac2->GetVelRes()));
-#else
-        fMac1->SetDeceleration(TMath::Nint(0.3*fMac1->GetVelRes()));
-        fMac2->SetDeceleration(TMath::Nint(0.3*fMac2->GetVelRes()));
-#endif
+        // FIXME: Makes sense?
+        fMac1->SetDeceleration(TMath::Nint(0.03*1000000000));
+        fMac2->SetDeceleration(TMath::Nint(0.09*1000000000));
+
         fMac1->SetRpmMode(FALSE);
         fMac2->SetRpmMode(FALSE);
     }
-
-/*
-    fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
-    fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
-
-    fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
-    fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
-    fMac2->WaitForSdo(0x3000, 0);
-    fMac1->WaitForSdo(0x3000, 0);
-    fMac1->SetRpmMode(FALSE);
-    fMac2->SetRpmMode(FALSE);
-    */
 
     //
@@ -538 +414 @@
 bool MCosy::CheckNetwork()
 {
-    //return kTRUE;
-    //CheckConnections();
+    if (!HasConnection())
+    {
+        gLog << warn << "- No connection to network." << endl;
+        return false;
+    }
 
     CheckForError();
@@ -557 +436 @@
         fMac1->HandleError();
         fMac2->HandleError();
-        //if (fMac3)
-        //    fMac3->HandleError();
         if (HasError() || HasZombie())
             return false;
@@ -564 +441 @@
 
     CheckForError();
-    return true;
+
+    return fMac1->IsOperative() && fMac2->IsOperative();
 }
 
@@ -581 +459 @@
         StopMovement();
         return 0;
-/*
-    case WM_PRESET:
-        cout << "WM_Preset: start." << endl;
-        if (!CheckNetwork())
-            return (void*)0xebb0;
-        fZd1->SetPreset();
-        fZd2->SetPreset();
-        fAz->SetPreset();
-        cout << "WM_Preset: done. (return 0xaffe)" << endl;
-        return (void*)0xaffe;
-*/
-        /*
-    case WM_CALIB:
-        {
-            cout << "WM_Calib: start." << endl;
-            if (!CheckNetwork())
-                return (void*)0xebb0;
-
-            SlaStars sla(fObservatory);
-            sla.Now();
-
-            RaDec rd = *((RaDec*)mp);
-
-            //RaDec rd(37.94, 89.2644);      // POLARIS
-            //RaDec rd(213.915417, 19.1825); // ARCTURUS
-
-            cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
-
-            ZdAz za=sla.CalcZdAz(rd*TMath::DegToRad())*16384.0/k2Pi;
-
-            cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
-
-            ZdAz sepos = GetSePos();
-            cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
-
-            fZd1->SetPreset(za.Zd());
-            fZd2->SetPreset(-za.Zd());
-            fAz->SetPreset(za.Az());
-
-            cout << "WM_Calib: done. (return 0xaffe)" << endl;
-        }
-        return (void*)0xaffe;
-        */
+
     case WM_TPOINT:
         {
@@ -655 +491 @@
             *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " ";
 
-            ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE;
+            ZdAz sepos = GetSePos()*TMath::TwoPi();
             za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
             za *= TMath::RadToDeg();
@@ -681 +517 @@
 
             ZdAz dest = *((ZdAz*)mp) * TMath::DegToRad();
-            if (!SetPosition(dest, kTRUE))
-                return 0x1234;
+            //if (!SetPosition(dest, kTRUE))
+            //    return 0x1234;
 
             SlaStars sla(fObservatory);
@@ -777 +613 @@
         //cout << "WM_Track: done. (return 0x8888)" << endl;
         return 0x7878;
-
+/*
     case WM_TESTSE:
         //cout << "WM_TestSe: start." << endl;
@@ -795 +631 @@
         //cout << "WM_Disply: done. (return 0xd1e1)" << endl;
         return 0xd1e1;
-
+ */
     case WM_TRACK:
     case WM_GRB:
@@ -832 +668 @@
         return 0xbe0e;
 
-    case WM_HOME:
-        //cout << "WM_Home: START" << endl;
-        if (!CheckNetwork())
-            return 0xebb0;
-        else
-        {
-            cout << "HOME NOT ALLOWED... for Magic." << endl;
-            /*
-            cout << "Going Home..." << endl;
-            TEnv env(".cosyrc");
-
-            SetStatus(MDriveCom::kMoving);
-
-            fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
-            fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
-
-            gLog << "SETHOME DONE" << endl;
-
-            SetStatus(HasError() ? MDriveCom::kError : MDriveCom::kStopped);
-
-            fAz->SetPreset();
-            fZd1->SetPreset();
-            fZd2->SetPreset();
-
-            fMac1->ReqPos();
-            fMac2->ReqPos();
-            fMac3->StopMotor();
-            */
-        }
-        //cout << "WM_Home: done. (return 0x403e)" << endl;
-        return 0x403e;
-
     case WM_CALCALTAZ:
         {
@@ -879 +683 @@
             cout << "Zd/Az target:  " << a1.Zd()*TMath::RadToDeg() << "° " << a1.Az()*TMath::RadToDeg() << "°" << endl;
 
-            if (fZd1 && fZd2 && fAz)
+            if (fMac1 && fMac2)
                 a1 = AlignTrackingPos(a1);
 
@@ -886 +690 @@
             a1 *= TMath::RadToDeg();
 
-            const ZdAz a2 = a1*kResSE/360;
+            const ZdAz a2 = a1/360;
 
             cout << "Zd/Az bended:  " << a1.Zd() << "° " << a1.Az() << "°" << endl;
@@ -895 +699 @@
     case WM_ENDSWITCH:
         {
-            ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE;
+            ZdAz pos = GetSePos()*TMath::TwoPi();
             pos = fBending.SubtractOffsets(pos)*TMath::RadToDeg();
 
@@ -919 +723 @@
 }
 
-Int_t MTTalk::Thread()
-{
-    fCosy->TalkThread();
-    return 0;
-}
-
 void MCosy::ReadConfig(MEnv &env)
 {
@@ -947 +745 @@
     gLog << " * Min': " << fMin.Zd()*TMath::RadToDeg() << "deg  " << fMin.Az()*TMath::RadToDeg() << "deg" << endl;
     gLog << " * Max': " << fMax.Zd()*TMath::RadToDeg() << "deg  " << fMax.Az()*TMath::RadToDeg() << "deg" << endl;
-
-    kGear.Set(env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0),
-              env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0));
-
-    Bool_t mac1 = fMac1 && !fMac1->IsZombieNode();
-    Bool_t mac2 = fMac2 && !fMac2->IsZombieNode();
-    //Bool_t mac3 = fMac3 && !fMac3->IsZombieNode();
-
-    Bool_t zd1  = fZd1  && !fZd1->IsZombieNode();
-    Bool_t zd2  = fZd2  && !fZd2->IsZombieNode();
-    Bool_t az   = fAz   && !fAz->IsZombieNode();
-
-    Double_t x = 0;
-    Double_t y = 0;
-//    y = mac1 ? fMac1->GetRes() : (mac3 ? fMac3->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500));
-    y = mac1 ? fMac1->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500);
-    x = mac2 ? fMac2->GetRes() : env.GetValue("Zd_ResRE[re/U_mot]", 1500);
-    kResRE.Set(x,y);
-
-    gLog << " * Az RE resolution: " << x << " re/U_mot" << endl;
-    gLog << " * Zd RE resolution: " << y << " re/U_mot" << endl;
-
-    x = zd1 ? fZd1->GetPhysRes() : (zd2 ? fZd2->GetPhysRes() : env.GetValue("Zd_ResSE[se/U_tel]", 16384));
-    y = az  ? fAz->GetPhysRes() : env.GetValue("Az_ResSE[se/U_tel]", 16384);
-    kResSE.Set(x,y);
-
-    gLog << " * Az SE resolution: " << x << " se/U_tel" << endl;
-    gLog << " * Zd SE resolution: " << y << " se/U_tel" << endl;
-
-    /*
-    kResRE.Y(0);
-    if (fMac1 && !fMac1->IsZombieNode())
-        kResRE.Y(fMac1->GetRes());
-    else
-        y = fMac3 && !fMac3->IsZombieNode() ? fMac3->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500);
-        if (fMac3 && !fMac3->IsZombieNode())
-            kResRE.Y(fMac3->GetRes());
-        else
-            kResRE.Y(env.GetValue("Az_ResRE[re/U_mot]", 1500));
-
-    kResRE.X(0);
-    if (fMac2 && !fMac2->IsZombieNode())
-        kResRE.X(fMac2->GetRes());
-    else
-        kResRE.X(env.GetValue("Zd_ResRE[re/U_mot]", 1500));
-
-    kResSE.X(0);
-    if (fZd1 && !fZd1->IsZombieNode())
-        kResSE.X(fZd1->GetPhysRes());
-    else
-        if (fZd2 && !fZd2->IsZombieNode())
-            kResSE.X(fZd2->GetPhysRes());
-        else
-            kResSE.X(env.GetValue("Zd_ResSE[se/U_mot]", 16384));
-
-    kResSE.Y(0);
-    if (fAz && !fAz->IsZombieNode())
-        kResSE.Y(fAz->GetPhysRes());
-    else
-        kResSE.Y(env.GetValue("Az_ResSE[se/U_mot]", 16384));
-        */
-
-    // believing the Macs manual '*4' shouldn't be necessary, but it is.
-    // Because the a RE is 4 quad counts.
-    // Calculating speeds we have to convert back to qc
-    kResRE  *= 4;
-    kGearTot = Mul(kResRE, kGear);
-//    kGearTot = kResRE*kGear;
-
-//    gLog << all;
-//    gLog << " * Setting Gear Ratios:" << endl;
- //   gLog << "   --------------------" << endl;
-    gLog << " *  X: " << kGear.X() << "*" << kResRE.X()/4 << "/" << kResSE.X() << "=4*" << kGearTot.X() << "/" << kResSE.X() << endl;
-    gLog << " *  Y: " << kGear.Y() << "*" << kResRE.Y()/4 << "/" << kResSE.Y() << "=4*" << kGearTot.Y() << "/" << kResSE.Y() << endl;
-}
-/*
-void MCosy::InitSync()
-{
-    if (!fMac3)
-    {
-        gLog << "Unable to Init Sync! Mac3 not available." << endl;
-        return;
-    }
-
-    const int res = fMac3->GetVelRes();
-
-    fMac3->SetVelocity(0.3*res);
-    fMac3->SetAcceleration(0.2*res);
-    fMac3->SetDeceleration(0.2*res);
-    fMac3->StartPosSync();
-}
-*/
-void MCosy::TalkThreadSeTest()
-{
-//    if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
-    //        return;
-
-    if (fHist)
-    {
-        gLog << err << "You are much too fast... try again." << endl;
-        return;
-    }
-
-    fHist = new TH2F("Diff", "Difference of SE values",
-                           201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
-    fHist->SetXTitle("ZA [\\circ]");
-    fHist->SetYTitle("\\Delta SE");
-
-    Double_t offset = 0;
-
-    int cnt = 0;
-
-    gLog << inf2 << "Starting Shaftencoder Test..." << endl;
-
-    gLog << err << "Build in values!!!" << endl;
-
-    while (fBackground==kBgdSeTest)
-    {
-        fZd1->ResetPosHasChanged();
-        fZd2->ResetPosHasChanged();
-
-        while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
-               fBackground==kBgdSeTest)
-        {
-            usleep(1);
-            TThread::CancelPoint();
-        }
-
-        const Double_t pos[3] = {
-            (fZd1->GetPos()+8192)%16384,
-            (fZd2->GetPos()+8192)%16384,
-            fAz->GetPos() };
-
-        //
-        //  Estimate Offset from the first ten positions
-        //
-        if (cnt++<10)
-        {
-            offset += pos[0]+pos[1];
-            continue;
-        }
-        if (cnt==11)
-        {
-            offset /= 10;
-            cnt++;
-        }
-
-        Double_t apos = (pos[0]-pos[1])/2 * TMath::TwoPi() / kResSE.X();
-
-        ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*TMath::RadToDeg();
-        fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
-    }
-
-    gLog << inf2 << "Shaftencoder Test Stopped... displaying Histogram." << endl;
-
-    fBackground=kBgdSeTestDispl;
-}
-
-void MCosy::TalkThreadGear()
-{
-//    if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
-    //        return;
-
-    if (fHist)
-    {
-        gLog << err << "You are much too fast... try again." << endl;
-        return;
-    }
-
-    fHist = new TH3F("Gear", "Gear Ratio Re/Se",
-                     (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(),
-                     (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(),
-                     61, 349.5, 500.5);
-
-    fHist->SetXTitle("Zd [\\circ]");
-    fHist->SetYTitle("Az [\\circ]");
-    fHist->SetZTitle("Re/Se");
-
-    gLog << inf2 << "Starting Gear determination..." << endl;
-
-    ZdAz se0 = GetSePos();
-    ZdAz re0 = GetRePosPdo();
-
-    while (fBackground==kBgdGear)
-    {
-        fZd1->ResetPosHasChanged();
-        fZd2->ResetPosHasChanged();
-        fAz->ResetPosHasChanged();
-
-        while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
-               !fAz->PosHasChanged() && fBackground==kBgdGear)
-        {
-            usleep(1);
-            TThread::CancelPoint();
-        }
-
-        ZdAz se = GetSePos();
-        ZdAz re = GetRePosPdo();
-
-        ZdAz dse = se-se0;
-        ZdAz dre = re-re0;
-
-        if (fabs(dse.Zd())*144>kResSE.X()) // Each 2.5deg (144)
-        {
-            se0.Zd(se.Zd());
-            re0.Zd(re.Zd());
-
-            se -= dse/2;
-
-            ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*TMath::RadToDeg();
-            ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
-        }
-
-        if (fabs(dse.Az())*144>kResSE.Y()) // Each 2.5deg (144)
-        {
-            se0.Az(se.Az());
-            re0.Az(re.Az());
-
-            se -= dse/2;
-
-            ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*TMath::RadToDeg();
-            ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az());
-        }
-    }
-    gLog << inf2 << "Gear Test Stopped... displaying Histogram." << endl;
-
-    fBackground=kBgdGearDispl;
-}
-
-void MCosy::TalkThread()
-{
-    /* ========== FIXME? =============
-     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return;
-        */
-
-    if (fMac1 && fMac2)
-    {
-        fMac1->ReqPos();
-        fMac2->ReqPos();
-    }
-
-    //InitSync();
-
-    /*** FOR DEMO MODE ***/
-    if (!fZd1 || !fZd2 || !fAz)
-        return;
-    /*** FOR DEMO MODE ***/
-
-    //
-    // Start the Network
-    //
-    while (1)
-    {
-        //
-        // wait until a tracking session is started
-        //
-        while (fBackground==kBgdNone)
-        {
-            usleep(1);
-            TThread::CancelPoint();
-        }
-
-        switch (fBackground)
-        {
-        case kBgdNone:
-            continue;
-/*#ifndef NEWALGO
-        case kBgdTracking:
-            TalkThreadTracking();
-            continue;
-#endif*/
-        case kBgdSeTest:
-            TalkThreadSeTest();
-            continue;
-
-        case kBgdGear:
-            TalkThreadGear();
-            continue;
-
-        default:
-            continue;
-        }
-    }
 }
 
 ZdAz MCosy::GetPointingPos() const
 {
-    if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
+    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
         return ZdAz(0, 0);
 
     // GetPointingPos [deg]
-    const ZdAz seist = GetSePos()*TMath::TwoPi()/kResSE; // [rad]
+    const ZdAz seist = GetSePos()*TMath::TwoPi(); // [rad]
     return fBending.CorrectBack(seist)*TMath::RadToDeg();
 }
@@ -1258 +772 @@
     // Update Gui, foremer MTGui.
     //
-    if (fZd1)
-        fZd1->DisplayVal();
-    if (fZd2)
-        fZd2->DisplayVal();
-    if (fAz)
-        fAz->DisplayVal();
-
+    if (fMac1)
+        fMac1->DisplayVal();
+    if (fMac2)
+        fMac2->DisplayVal();
+
+    /*
     Byte_t avail = 0;
 
     avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
     avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
-    //avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
-    avail |= (fZd1  && !fZd1->IsZombieNode())  ? 0x08 : 0;
-    avail |= (fZd2  && !fZd2->IsZombieNode())  ? 0x10 : 0;
-    avail |= (fAz   && !fAz->IsZombieNode())   ? 0x20 : 0;
 //    avail |= (!(fStatus&MDriveCom::kError) && 1 ? 0x40 : 0;
-
+  */
     Bool_t armed = kTRUE;
 
@@ -1283 +792 @@
         SetStatus(MDriveCom::kError);
 
-
-    ZdAz bendist = fStatus&MDriveCom::kTracking ? fTrackingPos : GetPointingPos();
+    const TString stataz = fMac1 ? fMac1->GetStatusDKC() : "";
+    const TString statzd = fMac2 ? fMac2->GetStatusDKC() : "";
+
+    const UInt_t stat1 = fMac1 ? fMac1->GetStatusPdo3() : 0;
+    const UInt_t stat2 = fMac2 ? fMac2->GetStatusPdo3() : 0;
+
+    ZdAz bendist = GetPointingPos();
 
     //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;
 
-    fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed);
+    static MTimeout tout(0);
+    if (tout.HasTimedOut())
+    {
+        tout.Start(999);
+        fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed);
+    }
 
     fWin->UpdateWeather(*fCom);
-    fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/
-                 fRaDec, fZdAzSoll, fStatus, avail, armed);
+    fWin->Update(bendist, fTrackingError, /*fVelocity, fOffset,*/
+                 fRaDec, fZdAzSoll, fStatus, (stat1<<8)|stat2, HasConnection(), armed, statzd, stataz);
 
     gLog.UpdateGui();
-
-    const Bool_t trigger = fTriggerDisplay;
-    fTriggerDisplay = kFALSE;
-
-    if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
-        DisplayHistTestSe(!trigger);
-
-    if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
-        DisplayHistGear(!trigger);
 
     if (fMutexGui.UnLock()==13)
@@ -1309 +819 @@
 
     return kTRUE;
-}
-
-void MCosy::DisplayHistTestSe(Bool_t del)
-{
-    gLog << inf2 << "Displaying histogram..." << endl;
-
-    TH2F &hist = *(TH2F*)fHist;
-
-    if (del)
-    {
-        fHist = NULL;
-        fBackground = kBgdNone;
-    }
-
-    TCanvas *c=new TCanvas("c1", "", 1000, 1000);
-    c->Divide(1,2);
-
-    c->cd(1);
-    TH2 *h=(TH2*)hist.DrawCopy();
-
-    TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
-    p->SetLineColor(kBlue);
-    p->Draw("same");
-    p->SetBit(kCanDelete);
-
-    c->cd(2);
-
-    TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
-            hist.GetBinLowEdge(hist.GetNbinsX()+1));
-    p2.SetXTitle("Zd [\\circ]");
-    for (int i=0; i<hist.GetNbinsX(); i++)
-        p2.SetBinError(i, p->GetBinError(i));
-    p2.SetLineColor(kRed);
-    p2.SetStats(0);
-    p2.DrawCopy();
-
-    if (del)
-        delete &hist;
-}
-
-void MCosy::DisplayHistGear(Bool_t del)
-{
-    gLog << inf2 << "Displaying histogram..." << endl;
-
-    TH3F &hist = *(TH3F*)fHist;
-
-    if (del)
-    {
-        fHist = NULL;
-        fBackground = kBgdNone;
-    }
-
-    TCanvas *c=new TCanvas("c1", "", 1000, 1000);
-    c->Divide(2,2);
-
-    // ----------
-
-    c->cd(1);
-    TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd
-    h1.SetTitle(" Gear Ratio Zenith Distance [re/se]  ");
-    h1.SetXTitle("Zd [\\circ]");
-    h1.Draw();
-    h1.SetBit(kCanDelete);
-
-    TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s");
-    p1->SetLineColor(kBlue);
-    p1->Draw("same");
-    p1->SetBit(kCanDelete);
-
-    // ----------
-
-    c->cd(2);
-    TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az
-    h2.SetTitle(" Gear Ratio Azimuth [re/se]  ");
-    h2.SetXTitle("Zd [\\circ]");
-    h2.Draw();
-    h2.SetBit(kCanDelete);
-
-    TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s");
-    p2->SetLineColor(kBlue);
-    p2->Draw("same");
-    p2->SetBit(kCanDelete);
-
-    // ----------
-
-    c->cd(3);
-
-    TAxis &axe1 = *h1.GetXaxis();
-
-    TH1F f1("spreadzd", " Spread Zenith Distance ",
-            axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax());
-    f1.SetXTitle("Zd [\\circ]");
-    for (int i=0; i<axe1.GetNbins(); i++)
-        f1.SetBinError(i, p1->GetBinError(i));
-    f1.SetLineColor(kRed);
-    f1.SetStats(0);
-    f1.DrawCopy();
-
-    c->cd(4);
-
-    // ----------
-
-    TAxis &axe2 = *h2.GetXaxis();
-
-    TH1F f2("spreadaz", " Spread Azimuth ",
-            axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax());
-    f2.SetXTitle("Az [\\circ]");
-    for (int i=0; i<axe2.GetNbins(); i++)
-        f2.SetBinError(i, p2->GetBinError(i));
-    f2.SetLineColor(kRed);
-    f2.SetStats(0);
-    f2.DrawCopy();
-
-    // ----------
-
-    if (del)
-        delete &hist;
 }
 
@@ -1445 +838 @@
     ReadConfig(env);
 
-    gLog << inf << "- Starting TX Thread." << endl;
-    fTTalk = new MTTalk(this);
-
     gLog << inf << "- Starting GUI update." << endl;
     fUpdateGui->TurnOn();
@@ -1466 +856 @@
     gLog << inf << "- GUI Update stopped." << endl;
 
-    gLog << inf << "- Stopping TX Thread." << endl;
-    delete fTTalk;
-    gLog << inf << "- TX Thread stopped." << endl;
-
     gLog << inf << "- Stopping CAN network." << endl;
     Network::Stop();
@@ -1483 +869 @@
 // Disable the synchronization by using a negative CAN Id for id2.
 //
-void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
-                        Int_t id4, Int_t id5, Int_t id6)
+void MCosy::Constructor(Int_t id1, Int_t id2)
 {
     //
@@ -1491 +876 @@
     gLog << inf << "- Setting up network." << endl;
 
-    fMac1=new Macs(id1, "Mac/Az");
-    fMac2=new Macs(id3, "Mac/Zd");
-    //if (id2>=0)
-    //    fMac3=new Macs(id2, "Mac/Az-Sync");
-
-    fZd1=new ShaftEncoder(id4, "SE/Zd1");
-    fZd2=new ShaftEncoder(id5, "SE/Zd2");
-    fAz =new ShaftEncoder(id6, "SE/Az");
-
-    fZd1->SetReport(fOutRep);
-    fZd2->SetReport(fOutRep);
-    fAz->SetReport(fOutRep);
-
-    fAz->SetMotor(fMac1);
-    fZd1->SetMotor(fMac2);
-    fZd2->SetMotor(fMac2);
- 
+    fMac1=new Dkc(id1, "DKC/Az");
+    fMac2=new Dkc(id2, "DKC/Zd");
+
+    fMac1->SetReport(fOutRep);
+    fMac2->SetReport(fOutRep);
+
     gLog << inf << "- Connecting devices to network." << endl;
 
@@ -1515 +889 @@
     SetNode(fMac1);
     SetNode(fMac2);
-    //if (id2>=0)
-    //    SetNode(fMac3);
-    SetNode(fZd1);
-    SetNode(fZd2);
-    SetNode(fAz);
 
     //
@@ -1528 +897 @@
 
     gLog << all << "- Starting GUI." << endl;
-    fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
-}
+    fWin=new MGCosy(fObservatory, this);
+}
+
 /*
-void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
+void MCosy::ConstructorDemo()
 {
     //
@@ -1537 +907 @@
     //
     gLog << "- Setting up network." << endl;
-
-    fZd1=new ShaftEncoder(id4, "SE/Zd1", gLog);
-    fZd2=new ShaftEncoder(id5, "SE/Zd2", gLog);
-    fAz =new ShaftEncoder(id6, "SE/Az",  gLog);
-
-    gLog << "- Connecting devices to network." << endl;
-
-    //
-    // Connect the devices to the network
-    //
-    SetNode(fZd1);
-    SetNode(fZd2);
-    SetNode(fAz);
 
     //
@@ -1560 +917 @@
     fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
 }
-
-void MCosy::ConstructorDemo()
-{
-    //
-    // Create Nodes
-    //
-    gLog << "- Setting up network." << endl;
-
-    //
-    // Create Gui Event timer and Gui
-    //
-    gLog << "- Initializing GUI Timer." << endl;
-    fUpdateGui = new TTimer(this, 100); // 100ms
-
-    gLog << "- Starting GUI." << endl;
-    fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
-}
 */
 
@@ -1614 +954 @@
 MCosy::MCosy(MEnv &env, MDriveCom *com, const char *pointing)
 : Network(), fObservatory(MObservatory::kMagic1), fStarguider(NULL),
-fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), /*fMac3(0),*/
-fBackground(kBgdNone), fTriggerDisplay(kFALSE), fStatus(MDriveCom::kStopped),
-fOutTp(0), fOutRep(0)
-{
-    const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
-    const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
-    const Int_t id3 = env.GetValue("Zd_Id-MAC",  3); //3
-    const Int_t id4 = env.GetValue("Zd_Id-SE1",  4); //4
-    const Int_t id5 = env.GetValue("Zd_Id-SE2",  5); //5
-    const Int_t id6 = env.GetValue("Az_Id-SE",   6); //6
+fMac1(0), fMac2(0), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0)
+{
+    const Int_t id1 = env.GetValue("Az_Id", 1);
+    const Int_t id2 = env.GetValue("Zd_Id", 3);
 
     TString name = GetFileName("rep", "cosy", "rep");
@@ -1644 +978 @@
         else
             gLog << err << "ERROR - Reading pointing model from " << pointing << endl;
-        Constructor(id1, id2, id3, id4, id5, id6);/*
+        Constructor(id1, id2);/*
         break;
     case 1:
@@ -1659 +993 @@
     gLog.SetOutputGui(fWin->GetLog(), kTRUE);
 
-    fZd1->SetDisplay(fWin->GetLabel2());
-    fZd2->SetDisplay(fWin->GetLabel3());
-    fAz->SetDisplay(fWin->GetLabel1());
+    fMac2->SetDisplay(fWin->GetLabel2());
+    fMac1->SetDisplay(fWin->GetLabel1());
 
     fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep);
@@ -1716 +1049 @@
 
     gLog << inf2 << "Deleting Nodes." << endl;
-    fZd1->SetReport(0);
-    fZd2->SetReport(0);
-    fAz->SetReport(0);
-
-    delete fAz;
-    delete fZd1;
-    delete fZd2;
+    fMac1->SetReport(0);
+    fMac2->SetReport(0);
+
     delete fMac1;
     delete fMac2;
-    //if (fMac3)
-    //    delete fMac3;
 
     gLog << inf2 << "Deleting MGCosy." << endl;

trunk/MagicSoft/Cosy/main/MCosy.h

@@ -45 +45 @@
 #define WM_ARM          0x1013
 
-class ShaftEncoder;
-class Macs;
+class Dkc;
+
 class MGCosy;
 class MCosy;
-class TH1;
 class MStarguider;
 class MDriveCom;
 class MEnv;
 
-class MTTalk : public MThread
-{
-private:
-    MCosy *fCosy;
+class TTimer;
 
-    Int_t Thread();
-
-public:
-    MTTalk(MCosy *cosy) : MThread("MTTalk"), fCosy(cosy)
-    {
-        RunThread();
-    }
-    ~MTTalk();
-};
-
-class TTimer;
 class MCosy : public Network, public MsgQueue, public TObject
 {
-    friend class MTTalk;
     friend class MSlewing;
     friend class MTracking;
@@ -81 +65 @@
     MStarguider *fStarguider;
 
-    ShaftEncoder *fZd1;
-    ShaftEncoder *fZd2;
-    ShaftEncoder *fAz;
-
-    Macs *fMac1;
-    Macs *fMac2;
-    //Macs *fMac3;
+    Dkc *fMac1;
+    Dkc *fMac2;
 
     MGCosy    *fWin;
@@ -93 +72 @@
 
     TTimer    *fUpdateGui;
-    MTTalk    *fTTalk;    // should be outsourced, like the starguider.
-                          // with a generic interface to both...
-    ZdAz fTrackingPos;    // [deg] Current tracking position
-    ZdAz fTrackingPosRaw; // [deg] Raw tracking position
 
     TMutex fMutexGui;
-
-    enum BackgroundAction_t
-    {
-        kBgdNone,
-        //kBgdTracking,
-        kBgdSeTest,
-        kBgdSeTestDispl,
-        kBgdGear,
-        kBgdGearDispl
-    };
-
-    BackgroundAction_t fBackground;    // Flag for present backgroundthread
 
     ZdAz  fTrackingError; // [rad] Tracking Offset between SE and calc-pos
@@ -116 +79 @@
     RaDec fRaDec;         // Position to track
     ZdAz  fAccuracy;      // Actual accuracy of Tracking
-    ZdAz  fVelocity;      // Actual velocity of Tracking
     ZdAz  fMin;
     ZdAz  fMax;
-
-    TH1  *fHist;
-    Bool_t fTriggerDisplay;
-
-    XY kResSE;   // describing the resolution of the system [se/U_tel]
-    XY kResRE;   // describing the resolution of the system [re/U_mot]
-    XY kGear;    // describing the resolution of the system [U_mot/U_tel]
-    XY kGearTot; // describing the resolution of the system [re/U_tel]
 
     MPointing fBending;
@@ -142 +96 @@
     UInt_t GetStatus() const { return fStatus; }
 
-    ZdAz GetRePos();
-    ZdAz GetRePosPdo();
-    ZdAz GetSePos() const; // [se]
-    // FIXME: Must depend on the Shaftencoder mounted
-    ZdAz GetSePosRad() const { return GetSePos()*TMath::TwoPi()/16384; } // [rad]
-
-    void InitSync();
-
-    void TalkThread();
-    void TalkThreadSeTest();
-    void TalkThreadGear();
-
-    void DisplayHistTestSe(Bool_t del=kTRUE);
-    void DisplayHistGear(Bool_t del=kTRUE);
-
     int  SetPosition(const ZdAz &dst, Bool_t track=kFALSE);
-    void TrackPosition(const RaDec &dst); // ra, dec [rad]
+    void TrackPosition(const RaDec &dst);    // ra, dec [rad]
     void TrackPositionGRB(const RaDec &dst); // ra, dec [rad]
 
@@ -170 +109 @@
     void WaitForEndMovement();
 
-    void Constructor(Int_t id1, Int_t id2, Int_t id3, Int_t id4, Int_t id5, Int_t id6);
+    void Constructor(Int_t id1, Int_t id2);
     //void ConstructorSE(Int_t id1, Int_t id2, Int_t id3);
     //void ConstructorDemo();
@@ -198 +137 @@
     MGCosy *GetWin() { return fWin; }
 
-    ZdAz GetTrackingPosDeg() const { return fTrackingPos; };
-    ZdAz GetTrackingPosRaw() const { return fTrackingPosRaw; };
-    void SetTrackingPosRE(ZdAz za);
-
-    AltAz GetAltAzDeg() const
-    {
-        ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE;
-        AltAz za1(TMath::Pi()/2-sepos.Zd(), sepos.Az());
-        za1 *= kRad2Deg;
-        return za1;
-    }
+    ZdAz GetSePos() const; // [revolutions]
 
     MLog *GetOutRep() { return fOutRep; }
@@ -214 +143 @@
     MDriveCom *GetDriveCom() { return fCom; }
 
-    // static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst);
-    //    static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0));
-
     ClassDef(MCosy, 0)
 };

trunk/MagicSoft/Cosy/main/MSlewing.cc

@@ -5 +5 @@
 
 #include "MCosy.h"
-#include "macs.h"
+#include "dkc.h"
 #include "MDriveCom.h"
 
@@ -17 +17 @@
 //#define EXPERT
 #undef EXPERT
+
+bool MSlewing::SetAcc(Dkc *mac, Float_t acc)
+{
+    // FIXME: Get acceleration scale from DKC!
+    mac->SetAcceleration(TMath::Nint(acc*1000000000));
+    return !mac->IsZombieNode();
+}
 
 // --------------------------------------------------------------------------
@@ -28 +35 @@
 //  target position at the same time.
 //
-void MSlewing::SetPosVelocity(const Float_t ratio, Float_t vel)
-{
-    //
-    // Set velocities
-    //
-    const int vr = fCosy->fMac1->GetVelRes();
-    vel *= vr;
-
-    if (ratio<1)
-    {
-        fCosy->fMac1->SetVelocity(TMath::Nint(vel));
-        fCosy->fMac2->SetVelocity(TMath::Nint(vel*ratio));
+void MSlewing::SetPosVelocity(const ZdAz &res, Float_t vel)
+{
+    const Double_t taz = TMath::Abs(res.Az())/fCosy->fMac1->GetVelMax();
+    const Double_t tzd = TMath::Abs(res.Zd())/fCosy->fMac2->GetVelMax();
+
+    if (tzd > taz)
+    {
+        fCosy->fMac1->SetVelocityRel(vel*TMath::Abs(res.Az()/res.Zd()));
+        fCosy->fMac2->SetVelocityRel(vel);
     }
     else
     {
-        fCosy->fMac1->SetVelocity(TMath::Nint(vel/ratio));
-        fCosy->fMac2->SetVelocity(TMath::Nint(vel));
-    }
-}
-
-// --------------------------------------------------------------------------
-//
-// Does a relative positioning.
+        fCosy->fMac1->SetVelocityRel(vel);
+        fCosy->fMac2->SetVelocityRel(vel*TMath::Abs(res.Zd()/res.Az()));
+    }
+}
+
+// --------------------------------------------------------------------------
+//
+// Does an absolute positioning.
 //
 // The steps to move are given in a ZdAz object relative to the current
@@ -57 +61 @@
 // if Axis 2==kTRUE. The function waits for the movement to be finished.
 //
-void MSlewing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
+void MSlewing::DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
 {
     if (fCosy->HasZombie())
@@ -64 +68 @@
     fCosy->SetStatus(MDriveCom::kMoving);
 
-    if (axe1) fCosy->fMac2->StartRelPos(TMath::Nint(rd.Zd()));
-    if (axe2) fCosy->fMac1->StartRelPos(TMath::Nint(rd.Az()));
+    if (axe1) fCosy->fMac2->StartAbsPosRev(rd.Zd());
+    if (axe2) fCosy->fMac1->StartAbsPosRev(rd.Az());
+
+    if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 0);
+    if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 0);
+
+    // FIXME: We need a delay here to account for the delay of the
+    // toggle bit in the SPS. We need a more precise return value from
+    // the SPS:
+    usleep(150000);
+
 #ifdef EXPERT
     cout << "Waiting for positioning..." << flush;
 #endif
-    if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1);
-    if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1);
-
     fCosy->WaitForEndMovement();
 #ifdef EXPERT
@@ -78 +88 @@
 }
 
-bool MSlewing::SetAccDec(Macs *mac, Float_t acc, Float_t dec)
-{
-    const int vr = mac->GetVelRes();
-    mac->SetAcceleration(TMath::Nint(acc*vr));
-    mac->SetDeceleration(TMath::Nint(dec*vr));
-    return !mac->IsZombieNode();
-}
-
 bool MSlewing::Break()
 {
     return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie();
+}
+
+
+// --------------------------------------------------------------------------
+//
+// Caluclate the difference between feedback 1 and feedback 2 at
+// the given zenith angle (feedback 2)
+//
+Double_t MSlewing::GetDiff(const ZdAz &za) const
+{
+    const Double_t zd = za.Zd(); //[revolutions]
+
+    const Double_t sh = -1.21  *(TMath::SinH(0.916*zd*TMath::TwoPi())-1);
+    const Double_t cs =  0.667 *TMath::Cos(1.735*(zd-0.236)*TMath::TwoPi());
+    const Double_t of =  0.6497;
+
+    return (sh+cs+of)/360;      //[revolutions]
 }
 
@@ -104 +123 @@
 int MSlewing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
 {
-    const ZdAz d = dst*kRad2Deg;
-
-    MTime t(-1);
-    gLog << all << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
-
-    //
-    // Calculate new target position (shortest distance to go)
-    //
-    //const ZdAz src = fCosy->GetSePos(); // [se]
-
-    //
-    // Make sure that the motors are in sync mode (necessary if the
-    // MACS has been rebooted from a Zombie state.
-    //
-    //InitSync();
-    //if (fMac3->IsZombieNode())
-    //    return false;
+    gLog << all << MTime(-1) << " - Target Position: " << dst.Zd()*TMath::RadToDeg() << "deg, " << dst.Az()*TMath::RadToDeg() << "deg (Zd/Az)" << endl;
 
     //
@@ -128 +131 @@
     // const ZdAz dest = CorrectTarget(src, dst);
     //
-    ZdAz bend = fCosy->fBending(dst); // [rad]
-
-    const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se]
+    const ZdAz bend = fCosy->fBending(dst);  // [rad]
+    const ZdAz dest = bend/TMath::TwoPi();   // [revolutions]
 
     if (!fCosy->CheckRange(bend))
         return kFALSE;
 
-    bend *= kRad2Deg;
     fCosy->fZdAzSoll = dst;
 
-    //cout << "Source        Zd: " << src.Zd()  << "se  Az:" << src.Az()  << "se" << endl;
-    //cout << "Destination   Zd: " << Rad2SE(dst.Zd()) << "se  Az:" << Rad2SE(dst.Az())  << "se" << endl;
-    //cout << "Bend'd Dest   Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
-    //cout << "Bend'd Dest   Zd: " << bend.Zd() << "deg  Az:" << bend.Az() << "deg" << endl;
-
-    //
-    // Set velocities
-    //
-    //const int vr = fCosy->fMac1->GetVelRes();
-
-    const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi();
-
     int i;
-    for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++)
-    {
-
+    for (i=0; i<(track?1:10) && !Break(); i++)
+    {
         gLog << inf2 << "- Step #" << i << endl;
 
-        // Get Shaft Encoder Positions
-        const ZdAz p=fCosy->GetSePos();
-
-        // calculate control deviation
-        ZdAz rd = dest-p; // [se]
-        ZdAz cd = rd;     // [se]
-        // Correct for having two SE available
-// FIMXE....
-//       cd.Zd(cd.Zd()*2);
-        // Round to check whether we are as near as possible
-        // to the value we expect
-
-        cout << cd.Zd() << " " << cd.Az() << endl;
-
-        cd /= fCosy->kResSE; // Scale to Revolutions
+        // Get feedback 2
+        const ZdAz sepos = fCosy->GetSePos();
+
+        // Calculate residual to move deviation
+        const ZdAz res = dest-sepos; // [revolutions]
 
         gLog << inf2 << "- Shaftencoders show a residual deviation of dZd=";
-        gLog << MString::Format("%.2f", cd.Zd()*360*60) << "' and dAz=";
-        gLog << MString::Format("%.2f", cd.Az()*360*60) << "'" << endl;
-
-        cout << cd.Zd() << " " << cd.Az() << endl;
- 
-        cd *= 1./fMaxResidual; // Scale to units of the maximum residual
-
-        cout << fCosy->kResSE.X() << " " << 1./fMaxResidual << endl;
-        cout << cd.Zd() << " " << cd.Az() << endl;
-
+        gLog << MString::Format("%.2f", res.Zd()*360*60) << "' and dAz=";
+        gLog << MString::Format("%.2f", res.Az()*360*60) << "'" << endl;
+
+        // Check which axis should still be moved
+        ZdAz cd  = res;             // [revolutions]
+        cd *= 1./fMaxResidual;      // Scale to units of the maximum residual
         cd.Abs();
 
-        // FIXME:: Do NOT check Shaftencoder values!!!
         // Check if there is a control deviation on the axis
         const Bool_t cdzd = cd.Zd()>0.5 ? kTRUE : kFALSE;
         const Bool_t cdaz = cd.Az()>0.5 ? kTRUE : kFALSE;
 
-        cout << (int)cdzd << " " << (int)cdaz << endl;
-
         // check if we reached the correct position already
         if (!cdzd && !cdaz)
         {
-            t.Now();
-            gLog << all << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
+            gLog << all << MTime(-1) << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
             fCosy->SetStatus(MDriveCom::kStopped);
             fCosy->fCom->SendStatus("Target position reached.");
@@ -204 +174 @@
         // ==============================================
         //   Estimate the noncircularity of the zd axis
-//        const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE;
-
-//        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())*rad2se;
- //       const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se;
-        // ===========================================
-
-        gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl;
-
-
-        // change units from se to re
-//        rd *= fCosy->kGearTot/fCosy->kResSE; // [re]
-        rd *= Div(fCosy->kGearTot,fCosy->kResSE); // [re]
-//        rd.Zd(f2-f1);
-
-        // Initialize Velocities so that we reach both positions
-        // at the same time
-        if (i)
-        {
-            //cout << "--- LO-SPEED ---" << endl;
-            SetAccDec(fCosy->fMac1, 0.05, 0.05);
-            SetAccDec(fCosy->fMac2, 0.05, 0.05);
-
-            SetPosVelocity(1.0, 0.05);
-        }
-        else
-        {
-            // 15 motor revolutions
-            const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y();
-
-//          lout << "MSlewing::SetPosition y: " << y << " rd.Az(): " << rd.Az() << endl;
-//          lout << "MSlewing::SetPosition fCosy->kGearTot.Y(): " << fCosy->kGearTot.Y() << " fCosy->kResSE.Y(): " << fCosy->kResSE.Y() << endl;
-
-            if (rd.Az()>-y && rd.Az()<y)
-                SetAccDec(fCosy->fMac1, 0.05, 0.05);
-            else
-                SetAccDec(fCosy->fMac1, fAcc, fDec);
-
-            SetAccDec(fCosy->fMac2, fAcc, fDec);
-
-            SetPosVelocity(fabs(rd.Ratio()), fVel);
-        }
-
-        rd.Round();
-
-        // FIXME? Check for Error or Zombie?
-
-        // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
-        // cout << " + APOS:  Zd=" << setw(6) << p.Zd()  << "se   Az=" << setw(6) << p.Az()  << "se" << endl;
-        // cout << " +       dZd=" << setw(6) << cd.Zd() << "se  dAz=" << setw(6) << cd.Az() << "se" << endl;
-        // cout << " +       dZd=" << setw(6) << rd.Zd() << "re  dAz=" << setw(6) << rd.Az() << "re" << endl;
-        // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X()  << "se   Az=" << setw(6) << kGearRatio.Y()  << "se" << endl;
-
-        // repositioning (relative)
-        gLog << inf2 << "- Do Relative Positioning..." << endl;
-        DoRelPos(rd, cdzd, cdaz);
-        gLog << inf2 << "- Relative Positioning Done" << endl;
-    }
-    if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/)
-    {
-        t.Now();
-        gLog << all << t << " - Positioning done." << endl;
+        const Double_t add = GetDiff(sepos)-GetDiff(dest);
+
+        const ZdAz dest2(dest.Zd()+add, dest.Az());
+        const ZdAz res2 = dest-sepos;
+        // =================================================
+
+        //gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl;
+
+        SetAcc(fCosy->fMac1, fAcc.Az());
+        SetAcc(fCosy->fMac2, fAcc.Zd());
+
+        SetPosVelocity(res2, fVel);
+
+        gLog << inf2 << "- Do absolute positioning..." << endl;
+        DoAbsPos(dest2, cdzd, cdaz);
+        gLog << inf2 << "- Absolute Positioning Done" << endl;
+    }
+    if (i==1 && track && !Break())
+    {
+        gLog << all << MTime(-1) << " - Positioning done." << endl;
         fCosy->SetStatus(MDriveCom::kStopped);
         fCosy->fCom->SendStatus("Tracking preposition reached.");
@@ -278 +204 @@
         fCosy->SetStatus(MDriveCom::kStopped);
 
-    t.Now();
-    gLog << warn << t << " - Warning: Requested position not reached (i=" << i << ")" << endl;
+    gLog << warn << MTime(-1) << " - Warning: Requested position not reached (i=" << i << ")" << endl;
 
     fCosy->fCom->SendStatus("Target position missed!");

trunk/MagicSoft/Cosy/main/MSlewing.h

@@ -6 +6 @@
 #endif
 
-class Macs;
+class Dkc;
 class MCosy;
-class ZdAz;
+
+#ifndef MARS_MPointing
+#include "MPointing.h"
+#endif
 
 class MSlewing
@@ -15 +18 @@
     MCosy *fCosy;
 
-    bool SetAccDec(Macs *mac, Float_t acc, Float_t dec);
+    bool SetAcc(Dkc *mac, Float_t acc);
     bool Break();
 
 private:
     Float_t fVel;
-    Float_t fAcc;
-    Float_t fDec;
+    ZdAz    fAcc;
 
     Float_t fMaxResidual; // [rev] Maximum residual deviation to stop pointin
 
     void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
-    void SetPosVelocity(const Float_t ratio, Float_t vel);
+    void DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
+    void SetPosVelocity(const ZdAz &res, Float_t vel);
+
+    Double_t GetDiff(const ZdAz &za) const;
 
 public:
-    MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1), fMaxResidual(1./16384) { }
+    MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0,0), fMaxResidual(1./16384) { }
+    virtual ~MSlewing() { }
 
-    void SetPointAccDec(Float_t acc, Float_t dec) { fAcc = acc; fDec = dec; }
+    void SetPointAcc(Float_t zd, Float_t az) { fAcc.Zd(zd); fAcc.Az(az); }
     void SetPointVelocity(Float_t vel) { fVel = vel; }
     int  SetPosition(const ZdAz &dst, Bool_t track=kFALSE);

trunk/MagicSoft/Cosy/main/MStarguider.cc

@@ -523 +523 @@
     fCaos = new MCaos;
     fCaos->ReadResources();
+    fCaos->SetRadii(237.9, 239.9);
 
     fStargCaos = new MCaos;
@@ -1666 +1667 @@
     // From the Shaftencoders we get the current 'pointing' position
     // as it is seen by the drive system (system pointing position)
-    const ZdAz za1 = fCosy->GetTrackingPosRaw();
+    // FIXME????
+    const ZdAz za1 = fCosy->GetSePos()*TMath::TwoPi();
 
     // Write real pointing position
@@ -1740 +1742 @@
     if (d>1)
     {
-        MString txt;
-        txt.Print("%dfps", (int)((n-n0)/d+.5));
+        const TString txt = MString::Format("%dfps", (int)((n-n0)/d+.5));
         fFps->SetText(txt);
         fTime = t;
@@ -1787 +1788 @@
             pos = fCosy->GetPointingPos();
         center = fCaos->Run(img, printl, printr, pos, t, 19, 3.0);
-        cout << "Caos Filter Camera center position: " << center.GetX() << " " << center.GetY() << endl;
+        cout << "Caos Filter Camera center position: " << center.GetX() << " " << center.GetY() << " (R=" << center.GetR() << ")" << endl;
 
     }
@@ -1995 +1996 @@
             fSkyBright->SetBackgroundColor(color);
 
-            MString txt;
-            txt.Print("Sky Brightness: %.1f", bright);
+            const MString txt = MString::Format("Sky Brightness: %.1f", bright);
             fSkyBright->SetText(txt);
 
@@ -2085 +2085 @@
         if (aa.Az()<0)
             aa.Az(aa.Az()+360);
-        txt.Print("(%d, %d) %.1fd/%.1fd", fDx, fDy, -aa.Alt(), aa.Az()-180);
+        txt.Form("(%d, %d) %.1fd/%.1fd", fDx, fDy, -aa.Alt(), aa.Az()-180);
     }
     else
-        txt.Print("(%d, %d)", fDx, fDy);
+        txt.Form("(%d, %d)", fDx, fDy);
     fPosZoom->SetText(txt);
 }
@@ -2124 +2124 @@
     MAstro::Deg2Dms(deg, sgn, d, m, s);
 
-    MString txt;
-    str += txt.Print("%c %03d %02d %03d ", sgn, d, m, s);
-}
+    str += MString::Format("%c %03d %02d %03d ", sgn, d, m, s);
+}

trunk/MagicSoft/Cosy/main/MTracking.cc

@@ -3 +3 @@
 #include "MLogManip.h"
 
-#include "macs.h"
+#include "dkc.h"
 #include "shaftencoder.h"
 
@@ -20 +20 @@
 // --------------------------------------------------------------------------
 //
-// request the current positions from the rotary encoders.
-// use GetRePos to get the psotions. If the request fails the function
-// returns kFALSE, otherwise kTRUE
-//
-bool MTracking::RequestRePos()
-{
-    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();
-    }
-
-    //
-    // If the waiting was interrupted due to a network error,
-    // print some logging message.
-    //
-    if (fCosy->HasError())
-        gLog << err << "ERROR - while requesting re pos from Macs (SDO #6004)" << endl;
-
-    return false;
-}
-
-// --------------------------------------------------------------------------
-//
 // Sets the tracking velocity
 //
@@ -68 +27 @@
 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();
-    }
+    const Double_t vrzd = fCosy->fMac2->GetVelRes();
+    const Double_t vraz = fCosy->fMac1->GetVelRes();
+
+    //
+    // Send the new velocities for both axes.
+    //
+    fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)(v.Zd()*vrzd));  // SetRpmVelocity [re/min]
+    fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)(v.Az()*vraz));  // SetRpmVelocity [re/min]
+
+    //
+    // Wait for the objects to be acknoledged.
+    //
+    fCosy->fMac2->WaitForSdo(0x3006, 1, 100);
+    fCosy->fMac1->WaitForSdo(0x3006, 1, 100);
+
+    //
+    // If the waiting for the objects wasn't interrupted return kTRUE
+    //
+    if (!Break())
+        return kTRUE;
+
+    fCosy->PrintError();
 
     //
@@ -117 +76 @@
     // Start revolution mode
     //
-    if (!SetAccDec(fCosy->fMac2, fTrackAcc, fTrackDec))
+    if (!SetAcc(fCosy->fMac2, fTrackAcc.Zd()))
         return false;
 
-    if (!SetAccDec(fCosy->fMac1, fTrackAcc, fTrackDec))
+    if (!SetAcc(fCosy->fMac1, fTrackAcc.Az()))
         return false;
 
@@ -135 +94 @@
     return true;
 }
+
 /*
 void MTracking::StopTracking()
@@ -177 +137 @@
 }
 */
+
 // --------------------------------------------------------------------------
 //
@@ -185 +146 @@
 // velocities are limited to the maximum velocity.
 //
-Bool_t MTracking::LimitSpeed(ZdAz *vt, const SlaStars &sla) const
-{
+Bool_t MTracking::LimitSpeed(const ZdAz &vt, const SlaStars &sla) const
+{
+    // vt [deg/min]
+
+    // We can set a maximum speed here
+    // And we can limit the change of the speed (which is done
+    //  by acceleration in the drive anyway)
+
+    return kTRUE;
+/*
     // vt[re/min]
 
@@ -237 +206 @@
     }
     return rc;
-}
-
-// --------------------------------------------------------------------
-//
-// Return pointing position of the telescope based on the
-// Shaftencoders with interpolation with motor encoders.
-//
-// GetPointingPos [re]
-//
-ZdAz MTracking::GetPointingPosRE(Bool_t pdo) const
-{
-     // Conversion factor from se to re
-     //const XY re = fCosy->kGearTot/fCosy->kResSE; //[re/se]
-     const XY re = Div(fCosy->kGearTot, fCosy->kResSE); //[re/se]
-
-     // Check wether moving direction has changed
-     const bool bool1 = fCosy->fZd1->DirHasChanged();
-     const bool bool2 = fCosy->fZd2->DirHasChanged();
-
-     // If both directions have changed reset the flags
-     if (bool1 && bool2)
-     {
-         fCosy->fZd1->ResetDirHasChanged();
-         fCosy->fZd2->ResetDirHasChanged();
-     }
-
-     // Get shaftencoder positions
-     // Ignore the shaftencoder which has not yet changed its value
-     const Int_t pzd1 = fCosy->fZd1->GetPosDirCorrected();
-     const Int_t pzd2 = fCosy->fZd2->GetPosDirCorrected();
-     const Int_t paz  = fCosy->fAz->GetPos();
-
-     const Int_t res1 = fCosy->fZd1->GetPhysRes();
-     const Int_t res2 = fCosy->fZd2->GetPhysRes();
-
-     // Get current shaftencoder position of the telescope
-     Double_t seposzd1 = ((pzd1+res1/2)%res1)*re.X();
-     Double_t seposzd2 = ((pzd2+res2/2)%res2)*re.X();
-     //Double_t seposzd1 = pzd1*re.X();
-     //Double_t seposzd2 = pzd2*re.X();
-     Double_t seposaz  = paz *re.Y();
-
-     // distance between (To+dt) and To [re]
-     // position time difference < 5usec
-     // fRePos does the synchronization between the
-     // Shaft- and the rotary encoders
-     const ZdAz repos = pdo ? fCosy->GetRePosPdo() : fCosy->GetRePos();
-
-     // Get rotary encoder positions
-     // Get stored offset if one SE has not changed its direction yet
-     const Int_t offset1 = fCosy->fZd1->GetOffsetDirCorrected();
-     const Int_t offset2 = fCosy->fZd2->GetOffsetDirCorrected();
-
-     // Calculate the part of one SE which the motors moved
-     // since the last SE has changed its value
-     const Double_t offzd1 = repos.Zd() - offset1;
-     const Double_t offzd2 = repos.Zd() - offset2;
-     const Double_t offaz  = repos.Az() - fCosy->fAz->GetOffset();
-
-     // Correct for the direction in which the motor is moving
-     // (in which the shaftencoders should change its values)
-     if (offaz<0)
-         seposaz += re.Y();
-     if (offzd1<0)
-         seposzd1 += re.X();
-     if (offzd2<0)
-         seposzd2 += re.X();
-
-     // If the correction exceeds one shaftencoder step stop interpolation
-     // of shaftencoder positions using rotary encoder values.
-     //ofstream fout("offsets.log", ios::app);
-     //fout << MTime(-1) << " " << offaz << " " << offzd1 << " " << offzd2 << endl;
-     /*
-      if (TMath::Abs(offaz)>re.Y())
-         offaz = TMath::Sign(re.Y(), offaz);
-      if (TMath::Abs(offzd1)>re.X())
-         offzd1 = TMath::Sign(re.X(), offzd1);
-      if (TMath::Abs(offzd2)>re.X())
-         offzd2 = TMath::Sign(re.X(), offzd2);
-         */
-
-     // and interpolate the shaftencoder steps using the motor
-     // encoder positon (Be carefull the minus-sign is important)
-     seposzd1 += offzd1;
-     seposzd2 -= offzd2;
-     seposaz  += offaz;
-
-     return ZdAz((seposzd1-seposzd2)/2, seposaz);
+    */
 }
 
@@ -359 +241 @@
     }
 
-    //
-    // calculate offset from present se position
-    //
-    //const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearTot/fCosy->kResSE; //[re]
-    if (!RequestRePos())
-        return;
-
-    // Estimate Offset before starting to track
-    ZdAz repos = fCosy->GetRePos();
-    fCosy->fZd1->SetOffset(TMath::Nint(repos.Zd()));
-    fCosy->fZd2->SetOffset(TMath::Nint(repos.Zd()));
-    fCosy->fAz->SetOffset(TMath::Nint(repos.Az()));
-
-    fCosy->SetTrackingPosRE(GetPointingPosRE());
-
     // Initialize Tracker (slalib or starguider)
     fCosy->fRaDec = dst;
@@ -402 +269 @@
     // *OLD*const float dt = 1;  // 1 second
     const float dt = 5;//3;  // 2 second
-    while (!Break())
-    {
-        //
-        // Request Target position for Now+dt
-        //
-        sla.Now(dt);
-
-        //
-        // Request nominal position for this time in the future (To+dt)
-        //
+    while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/)
+    {
+        /*
+        sla.Now(1);
         const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
         ZdAz dest = fCosy->AlignTrackingPos(pointing);     // fix ambiguity
-
-        //ZdAz vcalc = sla.GetApproxVel(fCosy->fRaDec);
-        //vcalc *= fCosy->kGearRatio2*4./60.; // [re/min]
-
-        float dtime = -1;
-        //if (kFALSE /*fUseStarguider*/)
-        //    dtime = Starguider(sla.GetMjd(), dest);
-
-        ZdAz repos;
-        if (dtime<0)
-        {
-            dest = fCosy->fBending(dest);            // [rad]
-            if (!fCosy->CheckRange(dest))
-                break;
-
-            // Destination position at t+dt in re-units
-            dest *= fCosy->kGearTot/TMath::TwoPi();  // [re]
-
-            // Request absolute position of rotary encoder from Macs
-            // Such that the RE position used in GetPointingPos is
-            // as up-to-date as possible.
-// DO I NEED THIS OR IS THE PDOPOS ENOUGH?
-            if (!RequestRePos())
-                break;
-
-            // *NEW* offset handling
-            // Get current position of the telescope and
-            // forward this position to MCosy
-            ZdAz sepos = GetPointingPosRE(); //[re]
-            fCosy->SetTrackingPosRE(sepos);
-
-            // distance between (To+dt) and To [re]
-            // position time difference < 5usec
-            // fRePos does the synchronization between the
-            // Shaft- and the rotary encoders
-            repos = fCosy->GetRePos();
-
-            // Now calculate the distance to move from now
-            // to a time in t+dt.
-            dest -= sepos;
-
-            dtime = dt;
-        }
+        dest = fCosy->fBending(dest);            // [rad]
+        if (!fCosy->CheckRange(dest))
+            break;
+        dest *= 1./TMath::TwoPi(); //[rev]
+        fCosy->fMac2->SendSDO(0x6004, 0, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false);
+        fCosy->fMac1->SendSDO(0x6004, 0, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false);
+        usleep(100000); // 1s
+        continue;
+        */
+
+        //
+        // Request Target position for Now+dt
+        //
+        sla.Now(dt);
+
+        //
+        // Request nominal position for this time in the future (To+dt)
+        //
+        const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
+        ZdAz dest = fCosy->AlignTrackingPos(pointing);     // fix ambiguity
+
+        //ZdAz repos;
+        dest = fCosy->fBending(dest);            // [rad]
+        if (!fCosy->CheckRange(dest))
+            break;
+
+        // Destination position at t+dt in re-units
+        dest *= TMath::RadToDeg();  // [re]
+
+        const ZdAz sepos = fCosy->GetSePos()*360;  // [deg]
+
+        // Now calculate the distance to move from now
+        // to a time in t+dt.
+        dest -= sepos;
 
         //
@@ -462 +314 @@
         // correct for the duration of RaDec2AltAz
         //
-        /* --- OLD --- */
-        ZdAz v = dest*60.0/dtime; //[re/min]
-        /* --- NEW --- seems to work worse! */
-        //const Double_t dtaz = sla.GetTime() - fCosy->fMac1->GetPosTime();
-        //const Double_t dtzd = sla.GetTime() - fCosy->fMac2->GetPosTime();
-        //
-        //ZdAz v = dest*60.0;
-        //v.Zd(v.Zd()/dtzd);
-        //v.Az(v.Az()/dtaz);
-        /* --- END --- */
-
-        //*fCosy->fOutRep << "> Dt:  " << dtaz << "  " << dtzd << endl;
-
-        if (LimitSpeed(&v, sla))
+        const ZdAz v  = dest * 60/dt;  // [deg/min]
+        const ZdAz vt = v/360;         // [rpm]
+
+        //Double_t kpZd = TMath::Abs(fCosy->fTrackingError.Zd()*TMath::RadToDeg()*60*4);
+        //Double_t kpAz = TMath::Abs(fCosy->fTrackingError.Az()*TMath::RadToDeg()*60*12);
+        //v.Zd(v.Zd()*(1+TMath::Min(0.3, kpZd)));
+        //v.Az(v.Az()*(1+TMath::Min(0.3, kpAz)));
+
+        if (LimitSpeed(v, sla))
         {
             gLog << dbg << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl;
@@ -482 +329 @@
 
         //
-        // calculate real velocity of future [re/min]
-        // believing the Macs manual '/4' shouldn't be necessary, but it is.
-        //
-        ZdAz vt = v; //[re'/min]
-        //lout << " " << vt.Zd() << " " << vt.Az() << " ";
-        vt.Round();
-        //lout << " " << vt.Zd() << " " << vt.Az() << endl;
-
-        //
         // check if the drive is fast enough to follow the star
         //
-        if (vt.Zd()>.9*fCosy->fMac1->GetVelRes() || vt.Az()>.9*fCosy->fMac2->GetVelRes())
+        if (TMath::Abs(vt.Zd())>0.5*fCosy->fMac2->GetVelMaxRev() ||
+            TMath::Abs(vt.Az())>0.5*fCosy->fMac1->GetVelMaxRev())
         {
-            gLog << err << "ERROR - Tracking speed faster than 90% of possible maximum velocity." << endl;
+            gLog << err << "ERROR - Tracking speed faster than 50% of possible maximum velocity." << endl;
+            gLog << "Zd: " << vt.Zd() << "   Az: " << vt.Az() << endl;
             break;
         }
@@ -503 +343 @@
         // Maybe we should attenuate the changes
         //
-        //*fCosy->fOutRep << "> SetVelocity1:  " << vt.Zd() << "  " << vt.Az() << endl;
         if (!SetVelocity(vt))
             break;
-        //*fCosy->fOutRep << "> SetVelocity2 " << endl;
-
-        //
-        // Now do 'unnecessary' things (timing)
-        //
-        fCosy->fVelocity = vt/fCosy->kGear; // [U_mot/min]
-        // *OLD* fVelocity = vt/kGearRatio2*4;
-
-        if (fOut)
-        {
-            fOut->Lock("MTracking::TrackPosition");
-            *fOut << "RE-REPORT 00 " << MTime(-1) << " " << repos.Zd() << " " << repos.Az() <<" " << vt.Zd() << " " << vt.Az() << endl;
-            fOut->UnLock("MTracking::TrackPosition");
-        }
 
         //
@@ -547 +372 @@
 Int_t MTracking::Thread()
 {
-    if (fCosy->fZd1->IsZombieNode() && fCosy->fZd2->IsZombieNode())
-        return 1;
-
-    if (fCosy->fAz->IsZombieNode())
-        return 2;
-
     if (!fCosy->fMac1 || !fCosy->fMac2)
         return 3;
@@ -558 +377 @@
     gLog << inf2 << "- Tracking Thread started (" << MTime(-1) << ")" << endl;
 
-    //const XY re2se = fCosy->kGearTot/fCosy->kResSE; //[re/se]
-
     SlaStars sla(fCosy->fObservatory);
     sla.Now();
 
-    //ZdAz time;
-
     ZdAz soll = sla.CalcZdAz(fCosy->fRaDec); // [rad]
 
@@ -571 +386 @@
     //
     bool phca1=false;
-    bool phca2=false;
     bool phcaz=false;
-
-    //ZdAz wasse = fCosy->GetSePos();
-    //ZdAz oldse = fCosy->GetSePos();
 
     while (1)
@@ -585 +396 @@
 
         // Check for changes of the shaftencoder values
-        //*fCosy->fOutRep << "> ResetPosHasChanged" << endl;
-        fCosy->fZd1->ResetPosHasChanged();
-        fCosy->fZd2->ResetPosHasChanged();
-        fCosy->fAz->ResetPosHasChanged();
-        //*fCosy->fOutRep << "> Check for PosHasChanged" << endl;
+        fCosy->fMac1->ResetHasChangedPos2();
+        fCosy->fMac2->ResetHasChangedPos2();
         do
         {
-            phca1 = fCosy->fZd1->PosHasChanged();
-            phca2 = fCosy->fZd2->PosHasChanged();
-            phcaz = fCosy->fAz->PosHasChanged();
+            phcaz = fCosy->fMac1->HasChangedPos2();
+            phca1 = fCosy->fMac2->HasChangedPos2();
 
             usleep(1);
+
             TThread::CancelPoint();
 
-        } while (!phca1 && !phca2 && !phcaz);
-
-        // Get time from last shaftencoder position change (position: ist)
-        // FIXME: Is this correct?
-        //        time.Az(fCosy->fMac1->GetMjd());
-        //        time.Zd(fCosy->fMac2->GetMjd());
-
-        //Double_t mjd1 = fCosy->fZd1->GetMjd();
-        //Double_t mjd2 = fCosy->fZd2->GetMjd();
-        //Double_t mjd0 = fCosy->fAz->GetMjd();
-
-        Double_t mjdaz = fCosy->fMac1->GetPdoMjd();//mjd0;
-        Double_t mjdzd = fCosy->fMac2->GetPdoMjd();//TMath::Max(mjd1, mjd2);
-
-        // get current position of shaftencoders (interpolated
-        // using motor encoders)
-        const ZdAz istse = GetPointingPosRE(kTRUE)/fCosy->kGearTot*TMath::TwoPi();
-        //const ZdAz istse = fCosy->GetSePosPdo();
+        } while (!phca1 && !phcaz);
+
+        // get current position and corresponding time of shaftencoders
+        const ZdAz istse = fCosy->GetSePos()*TMath::TwoPi();  // [deg]
+
+        const Double_t mjdaz = fCosy->fMac1->GetMjdPos2();
+        const Double_t mjdzd = fCosy->fMac2->GetMjdPos2();
+
 
         // calculate offset for both axis (only one is needed)
-        // *NEW* offset handling
-        //.const ZdAz offset = istre; //(istse*re2se - istre)*weight + fRePos*(weight-1);
         // if Shaftencoder changed position, calculate nominal position
-        if (phca1 || phca2)
+        if (phca1)
         {
             ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, mjdzd);
@@ -644 +440 @@
             TThread::CancelPoint();
         }
-
-
-        //fCosy->fZdAzSoll = soll;
-/*
-        // Calculate the aligned tracking position from 'soll'-position
-        if (phca1 || phca2)
-            fCosy->fTrackingError.Zd(soll.Zd()-istse.Zd());
-        if (phcaz)
-            fCosy->fTrackingError.Az(soll.Az()-istse.Az());
-            */
     }
 

trunk/MagicSoft/Cosy/main/MTracking.h

@@ -17 +17 @@
 {
 private:
-    Float_t fTrackAcc;
-    Float_t fTrackDec;
-
-    //Int_t fSePosZd1;
-    //Int_t fSePosZd2;
-    //Int_t fSePosAz;
-
-    //Int_t fRePosZd1;
-    //Int_t fRePosZd2;
-    //Int_t fRePosAz;
-
-    //ZdAz    fRePos;  // Offset between se and re coordinate system [re]
-    //ZdAz    fSePos;  // Shaftencoder Position
+    ZdAz fTrackAcc;
 
     MLog   *fOut;
 
-    bool RequestRePos();
     bool SetVelocity(const ZdAz &v);
-    bool LimitSpeed(ZdAz *vt, const SlaStars &sla) const;
+    bool LimitSpeed(const ZdAz &vt, const SlaStars &sla) const;
     bool InitTracking();
-    ZdAz GetPointingPosRE(Bool_t pdo=kFALSE) const;
 
     Int_t Thread();
 
 public:
-    MTracking(MCosy *cosy) : MSlewing(cosy), MThread("MTracking"), fTrackAcc(0.1), fTrackDec(0.1), fOut(0) { }
+    MTracking(MCosy *cosy) : MSlewing(cosy), MThread("MTracking"), fTrackAcc(0, 0), fOut(0) { }
 
     void TrackPosition(const RaDec &dst); // ra, dec [rad]
-    void SetTrackAccDec(Float_t acc, Float_t dec) { fTrackAcc=0.1; fTrackDec=0.1; }
+    void SetTrackAcc(Float_t zd, Float_t az) { fTrackAcc.Zd(zd); fTrackAcc.Az(az); }
 
     void SetOut(MLog *fout) { fOut = fout; }