Index: trunk/MagicSoft/Cosy/devdrv/dkc.cc =================================================================== --- trunk/MagicSoft/Cosy/devdrv/dkc.cc (revision 9430) +++ trunk/MagicSoft/Cosy/devdrv/dkc.cc (revision 9431) @@ -20,6 +20,6 @@ : NodeDrv(nodeid, name), fMacId(2*nodeid+1), fPdoPos1(0), fPdoPos2(0), fPosActive(0), fRpmActive(0), - fStatusDKC(0), fStatusPdo3(0xff), fArmed(false), - fReport(NULL),fLabel(NULL) + fStatus(0), fStatusDKC(0), fStatusPdo3(0xff), fArmed(false), + fReport(NULL),fLabel(NULL),fUpdPos(0) { } @@ -79,4 +79,5 @@ case 0xe252: return "Bleeder overtemp warning"; case 0xe257: return "Continous current limit active"; + case 0xe259: return "Command velocity limit active"; case 0xe264: return "Target position out of numerical range"; case 0xe829: return "Positive position limit exceeded"; Index: trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc =================================================================== --- trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (revision 9430) +++ trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (revision 9431) @@ -15,24 +15,11 @@ ShaftEncoder::ShaftEncoder(const BYTE_t nodeid, const char *name) : NodeDrv(nodeid, name), - fPos(0), fVel(0), fAcc(0), fTurn(0), fTicks(0), fTurns(0), - fDirection(kUndefined), fHysteresisPos(0), fHysteresisNeg(0), + fPos(0), fVel(0), fAcc(0), fPosRes(0), fTurns(0), fLabel(NULL), fUpdPos(0), fPosHasChanged(false), fDirHasChanged(false), - fReport(NULL), fMotor(NULL), - fOffset(0), fDirChangedPos(0), fDirChangedOffset(0) -{ -} -/* -void ShaftEncoder::CheckTwin(Int_t diff) const -{ - if (!fTwin) - return; - - if (fTwin->fIsUpdated) - fTwin->fIsUpdated = kFALSE; - else - fTwin->fOffset += diff; -} -*/ + fReport(NULL) +{ +} + void ShaftEncoder::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv) { @@ -95,18 +82,10 @@ gLog << inf2 << "- Reported position: " << dec << (Int_t)val << endl; fPos = val; - fTurn = 0; - fDirChangedPos = val; - fOffset = fMotor ? fMotor->GetPdoPos() : 0; - fPosHasChanged = true; - fDirHasChanged = true; - fDirection = kUndefined; - //fIsUpdated=kTRUE; - //fOffset = 0; return; case 0x6501: - gLog << inf2 << "- Phys. Ticks/Revolution: " << dec << val << endl; - fTicks = val; + gLog << inf2 << "- Position resolution Ticks/Revolution: " << dec << val << endl; + fPosRes = val; return; @@ -179,23 +158,4 @@ } -void ShaftEncoder::DisplayVal() -{ - const LWORDS_t pos = GetPos(); - if (IsZombieNode()) - { - fLabel->SetText(new TGString("")); - fUpdPos = ~pos; - return; - } - - char text[21]=""; - if (pos!=fUpdPos && fLabel) - { - sprintf(text, "%ld", pos); - fLabel->SetText(new TGString(text)); - fUpdPos = pos; - } -} - void ShaftEncoder::HandlePDOType0(const BYTE_t *data, const timeval_t &tv) { @@ -207,14 +167,4 @@ //if (pos==fPos) // return; - - // Warning: A multiturn shaftencoder is assumed! - if ((pos>fDirChangedPos && posfPos)) - { - fDirChangedPos = pos; - fDirHasChanged = true; - } - else - fOffset = fMotor ? fMotor->GetPdoPos() : 0; fPos = pos; @@ -243,14 +193,4 @@ //if (fPos==pos) // return; - - // Warning: A multiturn shaftencoder is assumed! - if ((pos>fDirChangedPos && posfPos)) - { - fDirChangedPos = pos; - fDirHasChanged = true; - } - else - fOffset = fMotor ? fMotor->GetPdoPos() : 0; //CheckTwin(fPos-pos); @@ -284,16 +224,5 @@ fAcc = data[6] | (data[7]<<8); - const int dnlim = fTicks/10; - const int uplim = fTurns*fTicks-dnlim; - - int turn = fTurn; - - if (fPos > uplim && pos < dnlim) - turn++; - - if (fPos < dnlim && pos > uplim) - turn--; - - LWORDS_t multipos = pos+turn*fTicks; // SE position of multiturn shaftencoder +// LWORDS_t multipos = pos+turn*fTicks; // SE position of multiturn shaftencoder //if (fPos==pos && fTurn==fTurn) @@ -302,30 +231,6 @@ //CheckTwin(fPos-pos); - fOffset = fMotor ? fMotor->GetPdoPos() : 0; - - // Warning: A multiturn shaftencoder is assumed! - if ((multipos>fDirChangedPos && multiposfPos)) - { - fDirChangedPos = GetPos(); - fDirHasChanged = true; - fDirChangedOffset = fOffset; - } - else - { - fDirHasChanged = false; - } - - if (multipos>GetPos()) - fDirection = kForward; - else - if (multiposLock("ShaftEncoder::HandlePDOType2"); *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO2 " << pos << " " << fVel << " " << fAcc << " " << GetNodeName() << endl; - *fReport << "DIR-REPORT " << (int)GetId() << " " << (Int_t)fDirHasChanged << " " << (Int_t)fDirChangedPos << endl; + //*fReport << "DIR-REPORT " << (int)GetId() << " " << (Int_t)fDirHasChanged << " " << (Int_t)fDirChangedPos << endl; fReport->UnLock("ShaftEncoder::HandlePDOType2"); } @@ -390,25 +295,4 @@ // - // Set logic ticks/revolution = physical ticks/revolution => scale factor = 1 - // - gLog << inf2 << "- " << GetNodeName() << ": Configuring log. tick/rev (0x6001)." << endl; - SendSDO(0x6001, fTicks); - WaitForSdo(0x6001); - - // - // Set maximum number of ticks (ticks * turns) - // - gLog << inf2 << "- " << GetNodeName() << ": Configuring max number of ticks (0x6002)." << endl; - SendSDO(0x6002, (LWORD_t)(fTicks*fTurns)); - WaitForSdo(0x6002); - - // - // Delete preset Value - // - gLog << inf2 << "- " << GetNodeName() << ": Delete preset value (0x6003)." << endl; - SendSDO(0x6003, (LWORD_t)0xffffffff); - WaitForSdo(0x6003); - - // // Configure PDOs // @@ -428,15 +312,4 @@ gLog << inf2 << "- " << GetNodeName() << ": Start Node (NMT)." << endl; SendNMT(kNMT_START); - - /* - cout << "---1---" << endl; - MTimeout t(1000); - while (!t.HasTimedOut()) - usleep(1); - cout << "---2---" << endl; - */ - - // StartGuarding(200, 1, kTRUE); // 175 - // StartGuarding(10*GetId(), 2); // 175 } @@ -458,23 +331,4 @@ } -void ShaftEncoder::SetPreset(LWORD_t pre) -{ - gLog << inf2 << "- " << GetNodeName() << ": Setting Preset." << endl; - - SendSDO(0x6003, (LWORD_t)pre); - if (!WaitForSdo(0x6003)) - return; - - fPos = pre%fTicks; - fTurn = pre/fTicks; - fDirChangedPos = fPos; - fOffset = fMotor ? fMotor->GetPdoPos() : 0; - - fPosHasChanged = true; - fDirHasChanged = true; - - fDirection = kUndefined; -} - void ShaftEncoder::StopDevice() { @@ -482,3 +336,2 @@ SendNMT(kNMT_STOP); } - Index: trunk/MagicSoft/Cosy/devdrv/shaftencoder.h =================================================================== --- trunk/MagicSoft/Cosy/devdrv/shaftencoder.h (revision 9430) +++ trunk/MagicSoft/Cosy/devdrv/shaftencoder.h (revision 9431) @@ -18,15 +18,9 @@ { private: - LWORDS_t fPos; // ticks - WORDS_t fVel; // ticks per 5ms - WORDS_t fAcc; // ticks per 25ms^2 - WORDS_t fTurn; // Number of turn - LWORD_t fTicks; // Number of ticks per revolution (resolution) - WORD_t fTurns; // Number of possible turns - - Direction_t fDirection; - - Float_t fHysteresisPos; - Float_t fHysteresisNeg; + LWORDS_t fPos; // ticks + WORDS_t fVel; // ticks per 5ms + WORDS_t fAcc; // ticks per 25ms^2 + LWORD_t fPosRes; // Number of ticks per revolution (resolution) + WORD_t fTurns; // Number of possible turns TGLabel *fLabel; // @@ -38,10 +32,4 @@ MTime fTime; MLog *fReport; - - Macs *fMotor; // Corresponding Motor/MACS - Int_t fOffset; // offset between SE and Motor - Int_t fDirChangedPos; // Last position at which the SE changed its moving direction - Int_t fDirChangedOffset; // Offset between SE and Motor when SE changed its moving direction last - void HandlePDOType0(const BYTE_t *data, const timeval_t &tv); @@ -60,64 +48,11 @@ void StopDevice(); - void SetDisplay(TGLabel *label) { fLabel = label; } - void SetMotor(Macs *m) { fMotor = m; } - //void SetTwin(ShaftEncoder *se) { fTwin = se; } - void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv); void HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv); - /* - void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv) { NodeDrv::HandleSDOOK(idx, subidx, tv); } - void HandleSDOError(LWORD_t data) { NodeDrv::HandleSDOError(data); } - */ + void HandlePDO1(const BYTE_t *data, const timeval_t &tv) { HandlePDOType2(data, tv); } void HandlePDO2(const BYTE_t *data, const timeval_t &tv) { HandlePDOType2(data, tv); } - LWORDS_t GetPos() const { return IsZombieNode() ? 0 : fPos+fTurn*fTicks; } // FIXME? 0? - Int_t GetDirection() const { return IsZombieNode() ? 0 : fPos-fDirChangedPos; } // FIXME? 0? - Int_t GetDirChangedPos() const { return IsZombieNode() ? 0 : fDirChangedPos; } // FIXME? 0? - LWORD_t GetPhysRes() const { return fTicks; } - Int_t GetOffset() const { return fOffset; } - Int_t GetDirChangedOffset() const { return fDirChangedOffset; } - - Int_t GetPosDirCorrected() const - { - return DirHasChanged() ? GetDirChangedPos() : GetPos(); - } - Int_t GetOffsetDirCorrected() const - { - return DirHasChanged() ? GetDirChangedOffset() : GetOffset(); - } - - void SetOffset(Int_t off) { fOffset = off; } - double GetMjd(); - - void SetPreset(LWORD_t pre=0); - - void DisplayVal(); - - bool PosHasChanged() const { return fPosHasChanged; } - bool DirHasChanged() const { return fDirHasChanged; } - void ResetPosHasChanged() { fPosHasChanged = false; } - void ResetDirHasChanged() { fDirHasChanged = false; } - - void SetReport(MLog *log) { fReport = log; } - - void SetHysteresisNeg(Float_t f) { fHysteresisNeg = f; } - void SetHysteresisPos(Float_t f) { fHysteresisPos = f; } - - Float_t GetPosCorrected() const - { - switch (fDirection) - { - case kUndefined: - return GetPos(); - case kForward: - return GetPos()-fHysteresisPos; - case kBackward: - return GetPos()+fHysteresisNeg; - } - return 0; - } ClassDef(ShaftEncoder, 0)