PID on each wheel with tuned parameters

Author: sorre97

rotary_encoder/motorencoder.py

@@ -35,7 +35,7 @@ def __init__(self, pi, enable_pin, forward_pin, backward_pin, feedback_pin_A, fe
         # quadrature encoder variables
         self._start_timer = 0
         self._current_timer = 0
-        self._ticks_threshold = 3
+        self._ticks_threshold = 100
         self._ticks_counter = 0
 
         # other
@@ -149,6 +149,20 @@ def adjust_power(self, power):
         self._motor_lock.release()
 
     # CALLBACK
+    """ The callback function rotary_callback is called on FALLING_EDGE by the
+            rotary_decoder with a parameter value of 1 (1 new tick)
+
+            - Gearbox ratio: 120:1 (1 wheel revolution = 120 motor revolution)
+            - Encoder ratio: 16:1 encoder ticks for 1 motor revolution
+            - 1 wheel revolution = 120 * 16 = 1920 ticks
+            - R = 30mm        - 1 wheel revolution = 2πR = 2 * π * 30mm = 188.5mm
+            - 1920 ticks = 188.5mm
+            - 1 tick = 0.0981mm
+            - 1 tick : 0.0981mm = x : 1000mm -> x = 10193 ticks approximately 
+            So 0.0981 is the ticks->distance(mm) conversion coefficient
+
+            The callback function calculates current velocity by taking groups of 
+            ticks_threshold ticks"""
     # callback function
     def rotary_callback(self, tick):
         self._motor_lock.acquire()
@@ -161,6 +175,8 @@ def rotary_callback(self, tick):
             elapse = self._current_timer - self._start_timer # calculating time elapse
             # calculating current speed
             self._encoder_speed = self._ticks_threshold * 0.0981 / elapse  # (mm/s)
+            #print("Elapse: %f" % (elapse))
+            #print("Speed: %f" % (self._encoder_speed))
 
         self._ticks += tick  # updating ticks
         self._distance = self._ticks * 0.0981  # (mm) travelled so far

rotary_encoder/rotarydecoder.py

@@ -75,11 +75,11 @@ def _pulse(self, gpio, level, tick):
          # backward (A leading B)
          if (gpio == self._feedback_pin_A and level == 1):
             if (self._levelB == 0):
-               self._callback(-1)   # A leading B, moving forward
+               self._callback(-1)   # A leading B, moving backward
                self._direction = -1 # backward
          elif (gpio == self._feedback_pin_A and level == 0):
             if (self._levelB == 1):
-               self._callback(-1)   # A leading B, moving forward
+               self._callback(-1)   # A leading B, moving backward
                self._direction = -1 # backward
 
          # forward (B leading A)

rotary_encoder/wheelsaxel.py

@@ -104,48 +104,75 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0):
         #PID parameters
         # assuming that power_right is equal to power_left and that coderbot
         # moves at 11.5mm/s at full PWM duty cycle
-        TARGET = 0.95 * power_right #velocity [mm/s]
-        KP = 0.02   #proportional coefficient
-        KI = 0.005
-        SAMPLETIME = 0.05
-        left_speed = TARGET
-        right_speed = left_speed
-        integral_error = 0
+        MAX_SPEED = 260
+        TARGET_LEFT = (MAX_SPEED/100) * power_left #velocity [mm/s]
+        TARGET_RIGHT = (MAX_SPEED / 100) * power_right  # velocity [mm/s]
+
+        # SOFT RESPONSE
+        #KP = 0.04  #proportional coefficient
+        #KD = 0.02  # derivative coefficient
+        #KI = 0.005 # integral coefficient
+
+        # MEDIUM RESPONSE
+        #KP = 0.8  #proportional coefficient
+        #KD = 0.04 # derivative coefficient
+        #KI = 0.02 # integral coefficient
+
+        # STRONG RESPONSE
+        KP = 0.9   # proportional coefficient
+        KD = 0.05  # derivative coefficient
+        KI = 0.03  # integral coefficient
+
+        SAMPLETIME = 0.1
+
+        left_speed = TARGET_LEFT
+        right_speed = TARGET_RIGHT
+
+        left_derivative_error = 0
+        right_derivative_error = 0
+        left_integral_error = 0
+        right_integral_error = 0
 
         # moving for certaing amount of distance
         while(abs(self.distance()) < target_distance):
             # PI controller
 
             # relative error
-            left_error = TARGET - self._left_motor.speed()
-            right_error = TARGET - self._right_motor.speed()
+            left_error = TARGET_LEFT - self._right_motor.speed()
+            right_error = TARGET_RIGHT - self._left_motor.speed()
 
-            left_speed += (left_error * KP) - (integral_error * KI)
-            right_speed += (right_error * KP) + (integral_error * KI)
+            left_speed += (left_error * KP) + (left_derivative_error * KD) + (left_integral_error * KI)
+            right_speed += (right_error * KP) + (right_derivative_error * KD) + (right_integral_error * KI)
+            print("LEFT correction: %f" % (left_error * KP + left_derivative_error * KD + left_integral_error * KI))
+            print("RIGHT correction: %f" % (right_error * KP + right_derivative_error * KD + right_integral_error * KI))
 
             # conrispondent new power
-            left_power = max(min(100 * left_speed / 95, 100), 0)
-            right_power =  max(min(100 * right_speed / 95, 100), 0)
+            left_power = max(min(100 * left_speed / MAX_SPEED, 100), 0)
+            right_power =  max(min(100 * right_speed / MAX_SPEED, 100), 0)
 
-            print("Left SPEED: %f" % (self._left_motor.speed()))
-            print("Right SPEED: %f" % (self._right_motor.speed()))
-            print("Left POWER: %f" % (left_power))
-            print("Right POWER: %f" % (right_power))
+            print("Left SPEED: %f" % (self._right_motor.speed()))
+            print("Right SPEED: %f" % (self._left_motor.speed()))
+            print("Left POWER: %f" % (right_power))
+            print("Right POWER: %f" % (left_power))
+            print("")
 
             # adjusting power on each motors
             self._left_motor.adjust_power(left_power)
             self._right_motor.adjust_power(right_power)
 
+            print("Left error: %f" % (left_error))
+            print("Right error: %f"  % (right_error))
+            print("")
 
-            integral_error += (left_speed - right_speed)
-
-            # restoring factor
-            left_speed = TARGET
-            right_speed = TARGET
 
             # checking each SAMPLETIME seconds
             sleep(SAMPLETIME)
 
+            left_derivative_error = left_error
+            right_derivative_error = right_error
+            left_integral_error += left_error
+            right_integral_error += right_error
+
         # robot arrived
         self.stop()
 

rotary_encoder_tests/rotarydecoder.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+
+import pigpio
+
+class RotaryDecoder:
+
+   """ Class to decode mechanical rotary encoder pulses """
+
+   def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
+
+      self._pi = pi
+      self._feedback_pin_A = feedback_pin_A   # encoder feedback pin A
+      self._feedback_pin_B = feedback_pin_B   # encoder feedback pin B
+      self._callback = callback   # callback function on event
+      self._direction = 0         # direction, forward = 1, backward = -1, steady = 0
+
+      self._levelA = 0  # value of encoder feedback pin A
+      self._levelB = 0  # value of encoder feedback pin B
+
+      # setting up GPIO
+      self._pi.set_mode(feedback_pin_A, pigpio.INPUT)
+      self._pi.set_mode(feedback_pin_B, pigpio.INPUT)
+      self._pi.set_pull_up_down(feedback_pin_A, pigpio.PUD_UP)
+      self._pi.set_pull_up_down(feedback_pin_B, pigpio.PUD_UP)
+
+      # callback function on EITHER_EDGE for each pin
+      self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulse)
+      self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulse)
+
+      self._lastGpio = None
+
+   """ pulse is the callback function on EITHER_EDGE
+       We have two feedback input from pin A and B (two train waves)
+       it returns a 1 if the square waves have A leading B because we're moving forward
+       It returns a -1 if the square waves have B leading A because we're moving backwards
+       In either case, A is staggered from B by (+-)pi/2 radiants
+       
+                +---------+         +---------+            0
+                |         |         |         |
+            A   |         |         |         |
+                |         |         |         |
+            ----+         +---------+         +----------+ 1   # A leading B
+                      +---------+         +---------+      0   # forward
+                      |         |         |         |
+            B         |         |         |         |
+                      |         |         |         |
+            +---------+         +---------+         +----- 1
+   """
+   def _pulse(self, gpio, level, tick):
+      # interrupt comes from pin A
+      if (gpio == self._feedback_pin_A):
+         self._levelA = level   # set level of squared wave (0, 1) on A
+      # interrupt comes from pin B
+      else:
+         self._levelB = level   # set level of squared wave (0, 1) on B
+
+      if (gpio != self._lastGpio): # debounce
+         self._lastGpio = gpio
+
+         if (gpio == self._feedback_pin_A and level == 1):
+            if (self._levelB == 0):
+               self._callback(1) # A leading B, moving forward
+               self._direction = 1 # forward
+         elif (gpio == self._feedback_pin_A and level == 0):
+            if (self._levelB == 1):
+               self._callback(1) # A leading B, moving forward
+               self._direction = 1 # forward
+         elif (gpio == self._feedback_pin_B and level == 1):
+            if (self._levelA == 0):
+               self._callback(-1)   # B leading A, moving forward
+               self._direction = -1 # backwards
+         elif (gpio == self._feedback_pin_B and level == 0):
+            if (self._levelA == 1):
+               self._callback(-1) # A leading B, moving forward
+               self._direction = -1 # forward
+
+   def cancel(self):
+
+      """
+      Cancel the rotary encoder decoder callbacks.
+      """
+      self._callback_triggerA.cancel()
+      self._callback_triggerB.cancel()
+
+

rotary_encoder_tests/testPID.py

@@ -0,0 +1,7 @@
+from coderbot import CoderBot
+import sys
+
+DISTANCE = float(sys.argv[1])
+
+c = CoderBot()
+c.move(speed=70, distance=DISTANCE)

rotary_encoder_tests/velocity_test2.py

@@ -0,0 +1,10 @@
+from time import sleep
+from motorencoder import MotorEncoder
+import pigpio
+
+pi = pigpio.pi()
+
+m = MotorEncoder(pi, 22, 25, 24, 14, 16)
+
+m.control(power=100.0, time_elapse=5)
+

rotary_encoder_tests/velocity_test3.py

@@ -0,0 +1,18 @@
+from time import sleep
+from motorencoder import MotorEncoder
+import pigpio
+
+pi = pigpio.pi()
+
+m = MotorEncoder(pi, 22, 25, 24, 14, 16)
+
+while(True):
+	print("Speed: %d" % (m._encoder_speed))
+	print("Distance: %d" % (m._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.05)
+
+