Changeset 7230 for trunk/MagicSoft/Cosy/devdrv

Timestamp:

07/28/05 22:24:32 (19 years ago)

Author:

fgoebel

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy/devdrv

Files:

• shaftencoder.cc (modified) (8 diffs)
• shaftencoder.h (modified) (4 diffs)

trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc

@@ -16 +16 @@
 ShaftEncoder::ShaftEncoder(const BYTE_t nodeid, const char *name, MLog &out)
     : NodeDrv(nodeid, name, out), fPos(0), fVel(0), fAcc(0),
-    fTurn(0), fLabel(NULL), fPosHasChanged(false), fReport(NULL),/*fTwin(0),
-    fIsUpdated(kFALSE),*/ fMotor(0), fOffset(0)
+    fTurn(0), fLabel(NULL), fPosHasChanged(false), fDirHasChanged(false),
+    fReport(NULL),
+    /*fTwin(0), fIsUpdated(kFALSE),*/
+      fMotor(0), fOffset(0), fDirChangedPos(0),
+      fDirection(kUndefined), fHysteresisPos(0), fHysteresisNeg(0)
 {
 }
@@ -93 +96 @@
         fPos  = val;
         fTurn = 0;
+        fDirChangedPos = val;
         fOffset = fMotor ? fMotor->GetPdoPos() : 0;
+        fPosHasChanged = true;
+        fDirHasChanged = true;
+        fDirection = kUndefined;
         //fIsUpdated=kTRUE;
         //fOffset = 0;
@@ -198 +205 @@
     //    return;
 
+    // Warning: A multiturn shaftencoder is assumed!
+    if ((pos>fDirChangedPos && pos<fPos) ||
+        (pos<fDirChangedPos && pos>fPos))
+    {
+        fDirChangedPos = pos;
+        fDirHasChanged = true;
+    }
+    else
+        fOffset = fMotor ? fMotor->GetPdoPos() : 0;
+
     fPos = pos;
-    fOffset = fMotor ? fMotor->GetPdoPos() : 0;
     fTime.Set(*tv);
     fPosHasChanged = true;
@@ -225 +241 @@
     //    return;
 
+    // Warning: A multiturn shaftencoder is assumed!
+    if ((pos>fDirChangedPos && pos<fPos) ||
+        (pos<fDirChangedPos && pos>fPos))
+    {
+        fDirChangedPos = pos;
+        fDirHasChanged = true;
+    }
+    else
+        fOffset = fMotor ? fMotor->GetPdoPos() : 0;
+
     //CheckTwin(fPos-pos);
     fPos=pos;
-    fOffset = fMotor ? fMotor->GetPdoPos() : 0;
     fTime.Set(*tv);
     fPosHasChanged=true;
@@ -267 +292 @@
         turn--;
 
+    LWORDS_t multipos = pos+turn*fTicks; // SE position of multiturn shaftencoder
+
     //if (fPos==pos && fTurn==fTurn)
     //    return;
@@ -272 +299 @@
     //CheckTwin(fPos-pos);
 
+    fOffset = fMotor ? fMotor->GetPdoPos() : 0;
+
+    // Warning: A multiturn shaftencoder is assumed!
+    if ((multipos>fDirChangedPos && multipos<fPos) ||
+        (multipos<fDirChangedPos && multipos>fPos))
+    {
+        fDirChangedPos = GetPos();
+        fDirHasChanged = true;
+        fDirChangedOffset = fOffset;
+    }
+    else 
+    {
+        fDirHasChanged = false;
+    }
+
+    if (multipos>GetPos()) fDirection = kForward;
+    else if (multipos<GetPos()) fDirection = kBackward;
+    else fDirection = kUndefined;
+
+
     fPos  = pos;
     fTurn = turn;
 
-    fOffset = fMotor ? fMotor->GetPdoPos() : 0;
     fTime.Set(*tv);
     fPosHasChanged=true;
@@ -281 +327 @@
     //fOffset = 0;
 
-
     if (fReport)
     {
         fReport->Lock("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->UnLock("ShaftEncoder::HandlePDOType2");
     }
@@ -415 +461 @@
     fPos  = pre%16384;
     fTurn = pre/16384;
-
+    fDirChangedPos = fPos;
     fOffset = fMotor ? fMotor->GetPdoPos() : 0;
+
+    fPosHasChanged = true;
+    fDirHasChanged = true;
+
+    fDirection = kUndefined;
 }
 

trunk/MagicSoft/Cosy/devdrv/shaftencoder.h

@@ -13 +13 @@
 class TGLabel;
 
+enum Direction_t { kUndefined, kForward, kBackward };                                                    
+
 class ShaftEncoder : public NodeDrv
 {
@@ -23 +25 @@
     WORD_t   fTurns; // Number of possible turns
 
+    Direction_t fDirection;
+  
+    Float_t fHysteresisPos;
+    Float_t fHysteresisNeg;
+
     TGLabel  *fLabel;     //
     LWORDS_t  fUpdPos;    // ticks
 
     bool fPosHasChanged;  //!
+    bool fDirHasChanged;  //!
 
     MTime fTime;
     MLog *fReport;
 
-    Macs *fMotor;
-    Int_t fOffset;
+    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(BYTE_t *data, timeval_t *tv);
@@ -62 +73 @@
     void HandlePDO2(BYTE_t *data, timeval_t *tv) { HandlePDOType2(data, tv); }
 
-    LWORDS_t GetPos() const { return IsZombieNode() ? 0 : fPos+fTurn*fTicks; } // FIXME? 0?
+    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; }
+
     void     SetOffset(Int_t off) { fOffset = off; }
 
@@ -74 +89 @@
 
     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; 
+      }
+    }
 
     ClassDef(ShaftEncoder, 0)