Index: /trunk/MagicSoft/Cosy/Changelog =================================================================== --- /trunk/MagicSoft/Cosy/Changelog (revision 913) +++ /trunk/MagicSoft/Cosy/Changelog (revision 914) @@ -39,4 +39,7 @@ - reorganized parts of the existing code + * devdrv/macs.cc: + - changed some output from cout to lout + @@ -100,6 +103,5 @@ * slalib/Makefile: - - added missing files - + - added missing files Index: /trunk/MagicSoft/Cosy/MCosy.cc =================================================================== --- /trunk/MagicSoft/Cosy/MCosy.cc (revision 913) +++ /trunk/MagicSoft/Cosy/MCosy.cc (revision 914) @@ -9,4 +9,5 @@ #include #include +#include #include "MGCosy.h" @@ -151,7 +152,12 @@ } +ZdAz MCosy::GetRePosPdo() +{ + return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos()); +} + int MCosy::SetPosition(const ZdAz &dst) // [rad] { - // FIXME: CORRECT BY fOffset !!!!!!!!!! + // FIXME: CORRECT BY fTrackingError !!!!!!!!!! // @@ -336,4 +342,7 @@ dest = CorrectTarget(GetSePos(), sla.CalcZdAz(dst)); + // + // Request absolute position of rotary encoder from MACS + // fMac2->RequestSDO(0x6004); fMac1->RequestSDO(0x6004); @@ -346,5 +355,5 @@ // - // Copy fOffset to a local variable + // Copy fTrackingError to a local variable // ZdAz offset = fOffset; @@ -402,12 +411,13 @@ dest0 *= kGearRatio; dest0 -= GetRePos()+offset; - dest0.Round(); + //dest0.Round(); fAccuracy = dest0/kGearRatio2; // [deg] + fVelocity = vt/kGearRatio2; //cout << "Control deviation: "; fout << setprecision(15) << setw(15) << mjd*60.*60.*24. << " "; - fout << setw(4) << (int)fOffset.Zd() << " "; - fout << setw(4) << (int)fOffset.Az() << " "; + fout << setw(4) << (int)fTrackingError.Zd() << " "; + fout << setw(4) << (int)fTrackingError.Az() << " "; fout << setw(4) << dest0.Zd() << " "; fout << setw(4) << dest0.Az() << " "; @@ -515,5 +525,5 @@ return (void*)0xffffffff; } - +/* void *MTGui::Thread() { @@ -521,5 +531,5 @@ return NULL; } - +*/ void *MTTalk::Thread() { @@ -609,26 +619,35 @@ usleep(1); - ZdAz sollalt; // [se] - ZdAz sollaz; // [se] ZdAz old; - - ZdAz ist=fOffset/kGearRatio; // [se] - // - // only update fOffset while tracking - // + ZdAz ist = fTrackingError/kGearRatio; // [se] + ZdAz istre = fOffset; // [re] + // + // only update fTrackingError while tracking + // + ZdAz sollzd; + ZdAz sollaz; while (fTracking) { - usleep(100000/*00*/); // 0.1s + // + // This is the time constant which defines how fast + // you correct for external influences (like wind) + // + usleep(100000); // 0.1s // // Make changes (eg wind) smoother - attenuation of control function // - - ZdAz offset(fOffset.Zd()*9.0/10.0+((ist.Zd()-sollalt.Zd())*kGearRatio.X())/10.0, - fOffset.Az()*9.0/10.0+((ist.Az()-sollaz.Az()) *kGearRatio.Y())/10.0); + fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(), + (ist.Az()-sollaz.Az())*kGearRatio.Y()); + + + const float weight = 0.3; + + ZdAz offset(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight, + fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight); fOffset = offset; - // fOffset.Zd(((offset.Zd()>1000)||(offset.Zd()<-1000))?0:offset.Zd()); - // fOffset.Az(((offset.Az()>1000)||(offset.Az()<-1000))?0:offset.Az()); + // fTrackingError.Zd(((offset.Zd()>1000)||(offset.Zd()<-1000))?0:offset.Zd()); + // fTrackingError.Az(((offset.Az()>1000)||(offset.Az()<-1000))?0:offset.Az()); // @@ -644,47 +663,41 @@ continue; - // - // if Alt Shaftencoder changed position + istre = GetRePosPdo(); + + // + // Get time from last shaftencoder position change (position: ist) + // + const double tzd = (fAlt1->GetMjd()+fAlt2->GetMjd())/2.0; + const double taz = fAz->GetMjd(); + + // + // if Shaftencoder changed position + // calculate were we should be // if ((int)ist.Zd() != (int)old.Zd()) { - // - // Get time from last shaftencoder position change - // - const double t = (fAlt1->GetMjd()+fAlt2->GetMjd())/2.0; - - // - // calculate were we should be - // - sla.SetMjd(t); - sollalt = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); - - old.Zd(ist.Zd()); + sla.SetMjd(tzd); + sollzd = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se] } - // - // if Alt Shaftencoder changed position - // if ((int)ist.Az() != (int)old.Az()) { - // - // Get time from last shaftencoder position change - // - const double t = fAz->GetMjd(); - - // - // calculate were we should be - // - sla.SetMjd(t); - sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); - - old.Az(ist.Az()); + sla.SetMjd(taz); + sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se] } + + old = ist; } } } - +/* void MCosy::GuiThread(MTGui *t) { + fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); + + fAz->SetDisplay(fWin->GetLabel1()); + fAlt1->SetDisplay(fWin->GetLabel2()); + fAlt2->SetDisplay(fWin->GetLabel3()); + while (!t->HasStopFlag()) { @@ -698,9 +711,38 @@ ZdAz ist = GetSePos()*(360.0/16384.0); // [se] - fWin->Update(ist, fAccuracy); + fWin->Update(ist, fAccuracy, fVelocity, fTrackingError/kGearRatio2); } + + delete fWin; cout << "Not running anymore." << endl; } +*/ +Bool_t MCosy::HandleTimer(TTimer *t) +{ + // + // Update Gui, foremer MTGui. + // + fAlt1->DisplayVal(); + fAlt2->DisplayVal(); + fAz->DisplayVal(); + + // gSystem->ProcessEvents(); + + // ZdAz err1 = fTrackingError/kGearRatio2; + // ZdAz err2(30./3600., 20./3600.); // = fTrackingError/kGearRatio2; + + ZdAz dummy = fOffset/kGearRatio2/4.; // /4. because using velocity canvas + + ZdAz ist = GetSePos()*(360.0/16384.0); // [se] + + fWin->Update(ist, fTrackingError/kGearRatio2, fVelocity, dummy); + + // cout << fTrackingError.Zd()/kGearRatio2.X()*3600. << " "; + // cout << fTrackingError.Az()/kGearRatio2.Y()*3600. << endl; + + return kTRUE; +} + int MCosy::StopWaitingForSDO() const @@ -718,10 +760,13 @@ lout << "- Starting GUI Thread." << endl; - fTGui = new MTGui(this); + fUpdateGui->TurnOn(); + // fTGui = new MTGui(this); } void MCosy::Stop() { - delete fTGui; + cout << "Stopping Gui Timer Events." << endl; + fUpdateGui->TurnOff(); + // delete fTGui; lout << "- GUI Thread stopped." << endl; @@ -755,4 +800,9 @@ SetNode(fAz); + // + // Create Gui Event timer and Gui + // + fUpdateGui = new TTimer(this, 100); // 100ms + fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); @@ -760,4 +810,5 @@ fAlt1->SetDisplay(fWin->GetLabel2()); fAlt2->SetDisplay(fWin->GetLabel3()); + } @@ -780,4 +831,6 @@ MCosy::~MCosy() { + delete fUpdateGui; + cout << "Deleting Nodes." << endl; Index: /trunk/MagicSoft/Cosy/MCosy.h =================================================================== --- /trunk/MagicSoft/Cosy/MCosy.h (revision 913) +++ /trunk/MagicSoft/Cosy/MCosy.h (revision 914) @@ -21,4 +21,5 @@ class MCosy; +/* class MTGui : public MThread { @@ -35,4 +36,5 @@ } }; +*/ class MTTalk : public MThread @@ -51,6 +53,6 @@ } }; - -class MCosy : public Network, public MsgQueue +class TTimer; +class MCosy : public Network, public MsgQueue, public TObject { friend class MTGui; @@ -68,13 +70,18 @@ MGCosy *fWin; - MTGui *fTGui; + TTimer *fUpdateGui; + + //MTGui *fTGui; MTTalk *fTTalk; - ZdAz fOffset; // Tracking Offset between SE and calc-pos [re] - RaDec fRaDec; // Position to track - int fTracking; // Flag for present tracking action - ZdAz fAccuracy; // Actual accuracy of Tracking + ZdAz fTrackingError; // Tracking Offset between SE and calc-pos [re] + ZdAz fOffset; // Offset between se and re coordinate system [re] + RaDec fRaDec; // Position to track + int fTracking; // Flag for present tracking action + ZdAz fAccuracy; // Actual accuracy of Tracking + ZdAz fVelocity; // Actual velocity of Tracking ZdAz GetRePos(); + ZdAz GetRePosPdo(); ZdAz GetSePos(); // [se] @@ -99,4 +106,6 @@ void *Proc(int msg, void *mp); + Bool_t HandleTimer(TTimer *t); + static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst); // static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0)); Index: /trunk/MagicSoft/Cosy/Makefile =================================================================== --- /trunk/MagicSoft/Cosy/Makefile (revision 913) +++ /trunk/MagicSoft/Cosy/Makefile (revision 914) @@ -24,5 +24,5 @@ CINT = M INCLUDES = -I. -Iincl -Ibase -Igui -Idevdrv -Icandrv -Ivideodev -Icatalog -LIBS = -lpng -lz -L/usr/X11R6/lib +LIBS = -lpng -lz -L/usr/X11R6/lib -lpthread # @@ -38,10 +38,10 @@ # SUBDIRS = \ - gui \ - candrv \ - devdrv \ - catalog \ - videodev \ - base \ + gui \ + candrv \ + catalog \ + devdrv \ + videodev \ + base \ slalib Index: /trunk/MagicSoft/Cosy/base/coord.h =================================================================== --- /trunk/MagicSoft/Cosy/base/coord.h (revision 913) +++ /trunk/MagicSoft/Cosy/base/coord.h (revision 914) @@ -78,4 +78,5 @@ public: ZdAz(double zd=0, double az=0) : XY(zd, az) {} + ZdAz(const ZdAz &c) : XY(c) {} double Zd() const { return fX; }