Index: trunk/MagicSoft/Cosy/Changelog =================================================================== --- trunk/MagicSoft/Cosy/Changelog (revision 8874) +++ trunk/MagicSoft/Cosy/Changelog (revision 8875) @@ -30,4 +30,8 @@ * videodev/VideodevIncl.h: - added TVector2 + + * aposs/Magic.m: + - added GRB mode + - added a division by four to the setting of the rpm mode velocity Index: trunk/MagicSoft/Cosy/aposs/Magic.m =================================================================== --- trunk/MagicSoft/Cosy/aposs/Magic.m (revision 8874) +++ trunk/MagicSoft/Cosy/aposs/Magic.m (revision 8875) @@ -216,7 +216,7 @@ /* OLD ORIGINAL VALUES: Prop=100, Div=300, Int=1000 */ if (get cannr==1) then -SET KPROP 950 -SET KDER 2000 -SET KINT 0 +SET KPROP 100 /*950*/ +SET KDER 300 /*2000*/ +SET KINT 1000 /*0*/ elseif (get cannr==2) then SET KPROP 100 @@ -329,7 +329,4 @@ kIoModule = 4*256 -ON INT -(kIoModule+6) GOSUB DOOR_STATUS_CHANGED -ON INT (kIoModule+6) GOSUB DOOR_STATUS_CHANGED - /*-------------------------------------------------------------------------*/ /* Init */ @@ -350,4 +347,6 @@ init = 0 + + oldstate = kTRUE gosub reset @@ -361,18 +360,15 @@ else fuse = in (kIoModule+1) - emcy = in (kIoModule+2) + emcy = in (kIoModule+2) vltg = in (kIoModule+3) mode = in (kIoModule+4) door = in (kIoModule+6) ready = in 1 - + if (door==0) then preop = kTRUE - endif - - if (preop==kTRUE) then - print "Door open!" - gosub reset - elseif (ready==0) and (RF==1) then + endif + + if (ready==0) and (RF==1) then print "DKC not ready, but RF set... setting RF=AH=0!" gosub reset @@ -438,5 +434,11 @@ endif endif - + + if (preop==kTRUE and oldstate!=preop) then + print "Door switch changed state!" + gosub reset + endif + oldstate = preop + rc = CANIN sdorx -1 0 canhi canlo goto mainloop @@ -455,5 +457,5 @@ SUBPROG reset init = 0 - + out 1 0 out 2 0 @@ -461,5 +463,5 @@ motor off waitt 1000 - + if (brake==1 and get cannr==3) then waitt 3000 /* wait 3s for DKC to stop the motor */ @@ -467,5 +469,5 @@ waitt 1000 endif - + canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4)) (rf | (brake<<1)) return @@ -482,13 +484,10 @@ PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval */ - if (idx==0x1000 and subidx==1) then - if (sdoval==1) then - if (door==0) then - canlo = 1 - else - preop = kFALSE - endif - elseif - preop = kTRUE + if (idx==0x1000 and subidx==1) then + preop = kTRUE + if (sdoval==1 and door==1) then + preop = kFALSE + else + canlo = 1 endif elseif (idx==0x1003 and subidx==0 and sdoval==0) then @@ -537,7 +536,16 @@ elseif (idx == 0x2003) then if (subidx) then - DEC sdoval + /* If velocity mode active */ + /*if (AXEND&4) + DEC sdoval%4 + else*/ + DEC sdoval + /*endif*/ else - ACC sdoval + /*if (AXEND&4) + ACC sdoval%4 + else*/ + ACC sdoval + /*endif*/ endif elseif (idx == 0x3000) then @@ -584,5 +592,5 @@ elseif (subidx == 1) then /* It is unclear where this arbitraty factor comes from */ - CVEL sdoval/4 + CVEL sdoval%4 endif elseif (idx == 0x3007) then @@ -810,5 +818,5 @@ /*-------------------------------------------------------------------------*/ SUBPROG PROC_PDO1 - CANOUT pdo1 (AXEND|(preop<<9)) APOS + CANOUT pdo1 (AXEND|(((~preop)&1)<<8)) APOS RETURN Index: trunk/MagicSoft/Cosy/cosy.cc =================================================================== --- trunk/MagicSoft/Cosy/cosy.cc (revision 8874) +++ trunk/MagicSoft/Cosy/cosy.cc (revision 8875) @@ -196,9 +196,9 @@ */ - MDriveCom com(ceco, ceco_tx, ceco_rx); - - MCosy *cosy = new MCosy(env, &com, pointing); - - com.SetMsgQueue(cosy); + MDriveCom *com = new MDriveCom(ceco, ceco_tx, ceco_rx); + + MCosy *cosy = new MCosy(env, com, pointing); + + com->SetMsgQueue(cosy); Interface *interface = new Ethernet(sps, 5357, 5358, cosy); @@ -252,4 +252,7 @@ delete cosy; + gLog << all << "Deleting DriveCom at " << MTime(-1) << endl; + delete com; + if (kDebugThreads) TThread::Ps(); Index: trunk/MagicSoft/Cosy/devdrv/macs.cc =================================================================== --- trunk/MagicSoft/Cosy/devdrv/macs.cc (revision 8874) +++ trunk/MagicSoft/Cosy/devdrv/macs.cc (revision 8875) @@ -12,4 +12,6 @@ using namespace std; + +//#define EXPERT /* @@ -514,6 +516,7 @@ //StartHostGuarding(); - // REMOVE THIS AND LET CC START THE NODE - // StartNode(); +#ifdef EXPERT + StartNode(); +#endif gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl; @@ -643,5 +646,5 @@ } - gLog << dbg << GetNodeName() << ": Starting abolsute positioning to " << (LWORDS_t)pos << " ticks." << endl; + gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl; SendSDO(0x6004, 1, (LWORD_t)pos); fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO @@ -657,5 +660,5 @@ } - gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl; + gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (LWORDS_t)pos << " ticks." << endl; SendSDO(0x6004, 0, (LWORD_t)pos); fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO Index: trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.cc (revision 8874) +++ trunk/MagicSoft/Cosy/main/MCosy.cc (revision 8875) @@ -406,6 +406,6 @@ //#ifdef EXPERT - //point.SetPointAccDec(0.3, 0.3); - //point.SetPointVelocity(0.3); +// point.SetPointAccDec(0.25, 0.25); +// point.SetPointVelocity(0.4); //#else @@ -449,5 +449,5 @@ track.SetPointVelocity(0.1); - // track.SetPointAccDec(0.4, 0.4); + // track.SetPointAccDec(0.4, 0.4); // track.SetPointVelocity(0.4); @@ -699,17 +699,15 @@ return 0xebb0; - //const bool arm = *((bool*)mp); - //if (arm) - //{ + const bool arm = mp ? *((bool*)mp) : true; + if (arm) + { fMac1->Arm(); fMac2->Arm(); - //cout << "ARMED" << endl; - //} - //else - //{ - // fMac1->Disarm(); - // fMac2->Disarm(); - //cout << "DISARMED" << endl; - //} + } + else + { + fMac1->Disarm(); + fMac2->Disarm(); + } } //cout << "WM_Position: done. (return 0x7777)" << endl; @@ -1290,5 +1288,5 @@ //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl; - fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError); + fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed); fWin->UpdateWeather(*fCom); @@ -1614,5 +1612,5 @@ } -MCosy::MCosy(MEnv &env, const char *addr, const char *pointing) +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),*/ @@ -1665,6 +1663,7 @@ fAz->SetDisplay(fWin->GetLabel1()); - fCom = new MDriveCom(this, addr, fOutRep); -// fCom->Start(); + fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep); + fCom->SetOutRep(fOutRep); + // fCom->Start(); } @@ -1691,4 +1690,10 @@ MCosy::~MCosy() { + if(fCom) + { + fCom->SetMsgQueue(NULL); + fCom->SetOutRep(NULL); + } + gLog << inf2 << "Deleting GUI timer." << endl; // FIXME: Wait until last Update was finished!!! @@ -1704,8 +1709,9 @@ delete fOutTp; } + delete fOutRep; - gLog << inf2 << "Deleting CC communication." << endl; - delete fCom; + //gLog << inf2 << "Deleting CC communication." << endl; + //delete fCom; gLog << inf2 << "Deleting Nodes." << endl; Index: trunk/MagicSoft/Cosy/main/MCosy.h =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.h (revision 8874) +++ trunk/MagicSoft/Cosy/main/MCosy.h (revision 8875) @@ -181,5 +181,5 @@ public: - MCosy(MEnv &env, const char *addr, const char *pointing); + MCosy(MEnv &env, MDriveCom *com, const char *pointing); ~MCosy(); @@ -212,5 +212,4 @@ MLog *GetOutRep() { return fOutRep; } - //rwagner MDriveCom *GetDriveCom() { return fCom; } Index: trunk/MagicSoft/Cosy/main/MSlewing.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MSlewing.cc (revision 8874) +++ trunk/MagicSoft/Cosy/main/MSlewing.cc (revision 8875) @@ -1,2 +1,3 @@ +#include "MSlewing.h" #include "MSlewing.h" @@ -7,4 +8,5 @@ #include "MDriveCom.h" +#include "MString.h" #include "MPointing.h" @@ -165,9 +167,28 @@ // Round to check whether we are as near as possible // to the value we expect - cd.Round(); - + + cout << cd.Zd() << " " << cd.Az() << endl; + + cd /= fCosy->kResSE; // Scale to 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; + + cd.Abs(); + + // FIXME:: Do NOT check Shaftencoder values!!! // Check if there is a control deviation on the axis - const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE; - const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE; + 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 @@ -183,17 +204,20 @@ // ============================================== // 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; - // ==++========================================= +// 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); +// rd.Zd(f2-f1); // Initialize Velocities so that we reach both positions @@ -201,7 +225,7 @@ if (i) { - //lout << "--- LO-SPEED ---" << endl; - SetAccDec(fCosy->fMac1, 0.1, 0.1); - SetAccDec(fCosy->fMac2, 0.1, 0.1); + //cout << "--- LO-SPEED ---" << endl; + SetAccDec(fCosy->fMac1, 0.05, 0.05); + SetAccDec(fCosy->fMac2, 0.05, 0.05); SetPosVelocity(1.0, 0.05); @@ -209,4 +233,5 @@ else { + // 15 motor revolutions const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y(); @@ -215,20 +240,7 @@ if (rd.Az()>-y && rd.Az()fMac1, 0.05, 0.05); - } else - { - - //lout << "MSlewing::SetPosition SetAccDec Mac1: " << fAcc << fDec << endl; - - SetAccDec(fCosy->fMac1, fAcc, fDec); - - } - - - // new 16.05.05 F.G. - // SetAccDec(fCosy->fMac1, fAcc, fDec); + SetAccDec(fCosy->fMac1, fAcc, fDec); SetAccDec(fCosy->fMac2, fAcc, fDec); @@ -267,5 +279,5 @@ t.Now(); - gLog << warn << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl; + gLog << warn << t << " - Warning: Requested position not reached (i=" << i << ")" << endl; fCosy->fCom->SendStatus("Target position missed!"); Index: trunk/MagicSoft/Cosy/main/MSlewing.h =================================================================== --- trunk/MagicSoft/Cosy/main/MSlewing.h (revision 8874) +++ trunk/MagicSoft/Cosy/main/MSlewing.h (revision 8875) @@ -23,13 +23,16 @@ Float_t fDec; + 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); public: - MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1) { } + MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1), fMaxResidual(1./16384) { } void SetPointAccDec(Float_t acc, Float_t dec) { fAcc = acc; fDec = dec; } void SetPointVelocity(Float_t vel) { fVel = vel; } int SetPosition(const ZdAz &dst, Bool_t track=kFALSE); + void SetMaxResidual(Float_t ac) { fMaxResidual = ac; } ClassDef(MSlewing, 0) Index: trunk/MagicSoft/Cosy/videodev/VideodevIncl.h =================================================================== --- trunk/MagicSoft/Cosy/videodev/VideodevIncl.h (revision 8874) +++ trunk/MagicSoft/Cosy/videodev/VideodevIncl.h (revision 8875) @@ -1,3 +1,5 @@ #ifndef __CINT__ +#include + #endif // __CINT__