PD controller structure

Author: sorre97

rotary_encoder/motorencoder.py

@@ -132,49 +132,23 @@ def reset_state(self):
         self._ticks_counter = 0
         self._is_moving = False  # resetting moving flag
 
-    # 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
-    # it calculates velocity via approssimation
-    # it doeas a mean on current time passed and actual distance travelled
-    # NOT USEFUL FOR VELOCITY REGULATION since we need to know the current
-    # velocity updated each time
-    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
+    # adjust power for velocity control loop
+    def adjust_power(self, power):
+        self._motor_lock.acquire()  # acquiring lock
 
-        # velocity calculation
-        self._current_timer = time()
+        self._power = power  # setting current power
 
-        elapse = self._current_timer - self._start_timer
-        if(elapse != 0):
-            self._encoder_speed = self._distance / elapse #(mm/s)
+        # adjusting power forward
+        if (self._direction == 1):
+            self._pi.set_PWM_dutycycle(self._forward_pin, abs(power))
+        # adjusting power bacakward
+        else:
+            self._pi.set_PWM_dutycycle(self._backward_pin, abs(power))
 
+        # releasing lock on motor
         self._motor_lock.release()
-    """
 
+    # CALLBACK
     # callback function
     def rotary_callback(self, tick):
         self._motor_lock.acquire()

rotary_encoder/wheelsaxel.py

@@ -101,10 +101,50 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0):
         self._left_motor.control(power_left)
         self._right_motor.control(power_right)
 
+        #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
+
         # moving for certaing amount of distance
-        # threshold value avoid to stop it after
         while(abs(self.distance()) < target_distance):
-            pass # busy waiting
+            # PI controller
+
+            # relative error
+            left_error = TARGET - self._left_motor.speed()
+            right_error = TARGET - self._right_motor.speed()
+
+            left_speed += (left_error * KP) - (integral_error * KI)
+            right_speed += (right_error * KP) + (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)
+
+            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))
+
+            # adjusting power on each motors
+            self._left_motor.adjust_power(left_power)
+            self._right_motor.adjust_power(right_power)
+
+
+            integral_error += (left_speed - right_speed)
+
+            # restoring factor
+            left_speed = TARGET
+            right_speed = TARGET
+
+            # checking each SAMPLETIME seconds
+            sleep(SAMPLETIME)
 
         # robot arrived
         self.stop()