Changeset 921 for trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc

Timestamp:

08/30/01 16:02:20 (23 years ago)

Author:

tbretz

Message:

*** empty log message ***

File:

• trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (modified) (4 diffs)

trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc

@@ -13 +13 @@
 #include <sys/resource.h>  // PRIO_PROCESS
 
-ShaftEncoder::ShaftEncoder(BYTE_t nodeid, MLog &out) : NodeDrv(nodeid, out), fLabel(NULL)
+ShaftEncoder::ShaftEncoder(BYTE_t nodeid, MLog &out) : NodeDrv(nodeid, out), fLabel(NULL), fPosHasChanged(false)
 {
 }
@@ -112 +112 @@
 }
 
-void ShaftEncoder::HandlePDOType0(BYTE_t *data)
+void ShaftEncoder::HandlePDOType0(BYTE_t *data, struct timeval *tv)
 {
     //
     // Decode information, we have a 14bit only
     //
-    fPos  = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24);
-}
-
-void ShaftEncoder::HandlePDOType1(BYTE_t *data)
+    LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24);
+    if (pos==fPos)
+        return;
+
+    fPos = pos;
+    fTime.SetTimer(tv);
+    fPosHasChanged = true;
+}
+
+void ShaftEncoder::HandlePDOType1(BYTE_t *data, struct timeval *tv)
 {
     //
@@ -127 +133 @@
     LWORDS_t pos  = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24);
     BYTE_t   flag = data[4];
-    pos=pos;
+
+    if (fPos==pos)
+        return;
+
+    fPos=pos;
+    fTime.SetTimer(tv);
+    fPosHasChanged=true;
+
     flag=flag;
 }
@@ -141 +154 @@
     fAcc = data[6] | (data[7]<<8);
 
-    fTime.SetTimer(tv);
-
     const int uplim = 9*fTicks/10;
     const int dnlim = 1*fTicks/10;
 
+    int turn = fTurn;
+
     if (fPos > uplim && pos < dnlim)
-        fTurn++;
+        turn++;
 
     if (fPos < dnlim && pos > uplim)
-        fTurn--;
-
-    fPos = pos;
+        turn--;
+
+    if (fPos==pos && fTurn==fTurn)
+        return;
+
+    fPos  = pos;
+    fTurn = turn;
+    fTime.SetTimer(tv);
+    fPosHasChanged=true;
 }