calculated velocity on each wheel according to ticks per time

Author: sorre97

rotary_encoder/motorencoder.py

@@ -35,6 +35,9 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, fe
         # other
         self._motor_lock = threading.RLock()
         self._start_timer = 0
+        self._current_timer = 0
+        self._ticks_threshold = 10
+        self._ticks_counter = 0
         self._rotary_decoder = RotaryDecoder(pi, feedback_pin_A, feedback_pin_B, self.rotary_callback)
 
     # GETTERS
@@ -123,6 +126,8 @@ def reset_state(self):
         self._encoder_speed = 0  # resetting encoder speed
         self._direction = 0  # resetting direction
         self._start_timer = 0
+        self._current_timer = 0
+        self._ticks_counter = 0
         self._is_moving = False  # resetting moving flag
 
     # CALLBACK
@@ -138,16 +143,45 @@ def reset_state(self):
         - 1 tick = 0.0981mm
         - 1 tick : 0.0981mm = x : 1000mm -> x = 10193 ticks approximately """
 
+    """
     # callback function
     def rotary_callback(self, tick):
         self._motor_lock.acquire()
+        
+        # on first movement
+        if(self._distance == 0):
+            self._start_timer = time() # clock started
+        
+        
         self._ticks += tick  # updating ticks
         self._distance = self._ticks * 0.0981  # (mm) travelled
 
         # velocity calculation
-        elapse = time() - self._start_timer
+        self._current_timer = time()
+
+        elapse = self._current_timer - self._start_timer
         if(elapse != 0):
-            self._encoder_speed = (self._distance / 1000) / elapse #(mm/s)
+            self._encoder_speed = self._distance / elapse #(mm/s)
+
+        self._motor_lock.release()
+    """
+
+    # callback function
+    def rotary_callback(self, tick):
+        self._motor_lock.acquire()
+
+        # taking groups of n ticks each
+        if (self._ticks_counter == 0):
+            self._start_timer = time()  # clock started
+        elif (self._ticks_counter == self._ticks_threshold):
+            self._current_timer = time()
+            elapse = self._current_timer - self._start_timer
+            self._encoder_speed = self._ticks_threshold * 0.0981 / elapse  # (mm/s)
+
+        self._ticks += tick  # updating ticks
+        self._distance = self._ticks * 0.0981  # (mm) travelled so far
+
+        self._ticks_counter += 1 % (self._ticks_threshold + 1)
 
         self._motor_lock.release()
 

rotary_encoder_tests/velocity_test.py

@@ -0,0 +1,15 @@
+from time import sleep
+from coderbot import CoderBot
+
+c = CoderBot.get_instance()
+
+while(True):
+	print("Speed: %d" % (c._twin_motors_enc._left_motor._encoder_speed))
+	print("Distance: %d" % (c._twin_motors_enc._left_motor._distance))
+	print("Start timer: %d" % (c._twin_motors_enc._left_motor._start_timer))
+	print("Current timer: %d" % (c._twin_motors_enc._left_motor._current_timer))
+	print("Elapse: %d" % (c._twin_motors_enc._left_motor._current_timer - c._twin_motors_enc._left_motor._start_timer))
+	print("")
+	sleep(0.3)
+
+