add motor min max power

previ · previ · commit b8023a0e1a2d · 2022-11-25T01:01:33.000+01:00
modify PID
diff --git a/coderbot/api.py b/coderbot/api.py
@@ -3,36 +3,40 @@
 This file contains every method called by the API defined in v2.yml
 """
 
+import logging
 import os
 import subprocess
-import logging
-import connexion
-from werkzeug.datastructures import Headers
-from flask import (request,
-                   send_file,
-                   Response)
-import picamera
 import urllib
+
 import connexion
+import picamera
+from flask import Response, request, send_file
+from werkzeug.datastructures import Headers
 
-from program import ProgramEngine, Program
 from config import Config
 from activity import Activities
+from audio import Audio
 from camera import Camera
 from cnn.cnn_manager import CNNManager
-from musicPackages import MusicPackageManager
-from audio import Audio
 from coderbotTestUnit import run_test as runCoderbotTestUnit
-from coderbot import CoderBot
+from musicPackages import MusicPackageManager
+from program import Program, ProgramEngine
+
 from balena import Balena
+from coderbot import CoderBot
 
 BUTTON_PIN = 16
 
 config = Config.read()
-bot = CoderBot.get_instance(
-    motor_trim_factor=float(config.get("move_motor_trim", 1.0)),
-    hw_version=config.get("hw_version")
-)
+bot = CoderBot.get_instance(motor_trim_factor=float(config.get('move_motor_trim', 1.0)),
+                            motor_max_power=int(config.get('motor_max_power', 100)),
+                            motor_min_power=int(config.get('motor_min_power', 0)),
+                            hw_version=config.get('hardware_version'),
+                            pid_params=(float(config.get('pid_kp', 1.0)),
+                                        float(config.get('pid_kd', 0.1)),
+                                        float(config.get('pid_ki', 0.01)),
+                                        float(config.get('pid_max_speed', 200)),
+                                        float(config.get('pid_sample_time', 0.01))))
 audio_device = Audio.get_instance()
 cam = Camera.get_instance()
 
@@ -125,9 +129,10 @@ def move(body):
 def turn(body):
     speed=body.get("speed")
     elapse=body.get("elapse")
-    if speed is None or speed == 0:
+    distance=body.get("distance")
+    if (speed is None or speed == 0) or (elapse is not None and distance is not None):
         return 400
-    bot.turn(speed=speed, elapse=elapse)
+    bot.turn(speed=speed, elapse=elapse, distance=distance)
     return 200
 
 def takePhoto():
diff --git a/coderbot/coderbot.py b/coderbot/coderbot.py
@@ -20,6 +20,7 @@
 import os
 import sys
 import time
+from math import copysign
 import logging
 import pigpio
 import sonar
@@ -100,7 +101,7 @@ class CoderBot(object):
 
     # pylint: disable=too-many-instance-attributes
 
-    def __init__(self, motor_trim_factor=1.0, hw_version="5"):
+    def __init__(self, motor_trim_factor=1.0, motor_min_power=0, motor_max_power=100, hw_version="5", pid_params=(0.8, 0.1, 0.01, 200, 0.01)):
         try:
             self._mpu = mpu.AccelGyroMag()
             logging.info("MPU available")
@@ -116,6 +117,8 @@ def __init__(self, motor_trim_factor=1.0, hw_version="5"):
         self._cb_elapse = dict()
         self._encoder = self.GPIOS.HAS_ENCODER
         self._motor_trim_factor = motor_trim_factor
+        self._motor_min_power = motor_min_power
+        self._motor_max_power = motor_max_power
         self._twin_motors_enc = WheelsAxel(
             self.pi,
             enable_pin=self.GPIOS.PIN_MOTOR_ENABLE,
@@ -126,7 +129,8 @@ def __init__(self, motor_trim_factor=1.0, hw_version="5"):
             right_forward_pin=self.GPIOS.PIN_RIGHT_FORWARD,
             right_backward_pin=self.GPIOS.PIN_RIGHT_BACKWARD,
             right_encoder_feedback_pin_A=self.GPIOS.PIN_ENCODER_RIGHT_A,
-            right_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_RIGHT_B)
+            right_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_RIGHT_B,
+            pid_params=pid_params)
         self.motor_control = self._dc_enc_motor
 
         self._cb1 = self.pi.callback(self.GPIOS.PIN_PUSHBUTTON, pigpio.EITHER_EDGE, self._cb_button)
@@ -153,21 +157,23 @@ def exit(self):
             s.cancel()
 
     @classmethod
-    def get_instance(cls, motor_trim_factor=1.0, hw_version="5", servo=False):
+    def get_instance(cls, motor_trim_factor=1.0, motor_max_power=100, motor_min_power=0, hw_version="5", pid_params=(0.8, 0.1, 0.01, 200, 0.01)):
         if not cls.the_bot:
-            cls.the_bot = CoderBot(motor_trim_factor=motor_trim_factor, hw_version=hw_version)
+            cls.the_bot = CoderBot(motor_trim_factor=motor_trim_factor,  motor_max_power= motor_max_power, motor_min_power=motor_min_power, hw_version=hw_version, pid_params=pid_params)
         return cls.the_bot
 
+    def get_motor_power(self, speed):
+        return int(copysign(min(max(((self._motor_max_power - self._motor_min_power) * abs(speed) / 100) + self._motor_min_power, self._motor_min_power), self._motor_max_power), speed))
+
     def move(self, speed=100, elapse=None, distance=None):
-        self._motor_trim_factor = 1.0
-        speed_left = min(100, max(-100, speed * self._motor_trim_factor))
-        speed_right = min(100, max(-100, speed / self._motor_trim_factor))
+        speed_left = speed * self._motor_trim_factor
+        speed_right = speed / self._motor_trim_factor
         self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance)
 
-    def turn(self, speed=100, elapse=-1):
-        speed_left = min(100, max(-100, speed * self._motor_trim_factor))
-        speed_right = -min(100, max(-100, speed / self._motor_trim_factor))
-        self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse)
+    def turn(self, speed=100, elapse=None, distance=None):
+        speed_left = speed * self._motor_trim_factor
+        speed_right = -speed / self._motor_trim_factor
+        self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance)
 
     def turn_angle(self, speed=100, angle=0):
         z = self._mpu.get_gyro()[2]
@@ -225,8 +231,8 @@ def _servo_control(self, pin, angle):
         self.pi.set_PWM_dutycycle(pin, duty)
 
     def _dc_enc_motor(self, speed_left=100, speed_right=100, time_elapse=None, target_distance=None):
-        self._twin_motors_enc.control(power_left=speed_left,
-                                      power_right=speed_right,
+        self._twin_motors_enc.control(power_left=self.get_motor_power(speed_left),
+                                      power_right=self.get_motor_power(speed_right),
                                       time_elapse=time_elapse,
                                       target_distance=target_distance)
 
diff --git a/coderbot/coderbotTestUnit.py b/coderbot/coderbotTestUnit.py
@@ -12,13 +12,13 @@
     If no test was executed on that component, 0 is preserved.
 """
 from coderbot import CoderBot
-c = CoderBot.get_instance()
 
 # Single components tests
 
 # encoder motors
 def __test_encoder():
     try:
+        c = CoderBot.get_instance()
         # moving both wheels at speed 100 clockwise
         print("moving both wheels at speed 100 clockwise")
         assert(c.speed() == 0)
diff --git a/coderbot/main.py b/coderbot/main.py
@@ -22,10 +22,10 @@
 # Logging configuration
 logger = logging.getLogger()
 logger.setLevel(os.environ.get("LOGLEVEL", "INFO"))
-sh = logging.StreamHandler()
-formatter = logging.Formatter('%(message)s')
-sh.setFormatter(formatter)
-logger.addHandler(sh)
+# sh = logging.StreamHandler()
+# formatter = logging.Formatter('%(message)s')
+# sh.setFormatter(formatter)
+# logger.addHandler(sh)
 
 ## (Connexion) Flask app configuration
 
@@ -69,7 +69,14 @@ def run_server():
             app.bot_config = Config.read()
 
             bot = CoderBot.get_instance(motor_trim_factor=float(app.bot_config.get('move_motor_trim', 1.0)),
-                                        hw_version=app.bot_config.get('hardware_version'))
+                                        motor_max_power=int(app.bot_config.get('motor_max_power', 100)),
+                                        motor_min_power=int(app.bot_config.get('motor_min_power', 0)),
+                                        hw_version=app.bot_config.get('hardware_version'),
+                                        pid_params=(float(app.bot_config.get('pid_kp', 1.0)),
+                                                    float(app.bot_config.get('pid_kd', 0.1)),
+                                                    float(app.bot_config.get('pid_ki', 0.01)),
+                                                    float(app.bot_config.get('pid_max_speed', 200)),
+                                                    float(app.bot_config.get('pid_sample_time', 0.01))))
 
             try:
                 audio_device = Audio.get_instance()
diff --git a/coderbot/rotary_encoder/motorencoder.py b/coderbot/rotary_encoder/motorencoder.py
@@ -156,18 +156,19 @@ def rotary_callback(self, tick):
 
         # taking groups of n ticks each
         if (self._ticks_counter == 0):
-            self._start_timer = time()  # clock started
+            self._start_timer = tick  # clock started
         elif (abs(self._ticks_counter) == self._ticks_threshold):
-            self._current_timer = time()
-            elapse = self._current_timer - self._start_timer # calculating time elapse
+            self._current_timer = tick
+            elapse = (self._current_timer - self._start_timer) / 1000000.0 # calculating time elapse
             # calculating current speed
             self._encoder_speed = self._ticks_threshold * self._distance_per_tick / elapse  # (mm/s)
 
-        self._ticks += tick  # updating ticks
+        self._ticks += 1  # updating ticks
 
         if(abs(self._ticks_counter) < self._ticks_threshold):
             self._ticks_counter += 1
         else:
+            self._start_timer = tick  # clock started
             self._ticks_counter = 0
 
         # updating ticks counter using module
diff --git a/coderbot/rotary_encoder/rotarydecoder.py b/coderbot/rotary_encoder/rotarydecoder.py
@@ -27,8 +27,8 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
         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._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulseA)
+        self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulseA)
 
         self._lastGpio = None
 
@@ -91,6 +91,9 @@ def _pulse(self, gpio, level, tick):
         self._lock.release()
         self._callback(direction)
 
+    def _pulseA(self, gpio, level, tick):
+        self._callback(tick)
+
     def cancel(self):
 
         """
diff --git a/coderbot/rotary_encoder/wheelsaxel.py b/coderbot/rotary_encoder/wheelsaxel.py
@@ -16,7 +16,8 @@ class WheelsAxel:
 
     def __init__(self, pi, enable_pin,
                  left_forward_pin, left_backward_pin, left_encoder_feedback_pin_A, left_encoder_feedback_pin_B,
-                 right_forward_pin, right_backward_pin, right_encoder_feedback_pin_A, right_encoder_feedback_pin_B):
+                 right_forward_pin, right_backward_pin, right_encoder_feedback_pin_A, right_encoder_feedback_pin_B,
+                 pid_params):
 
         # state variables
         self._is_moving = False
@@ -36,6 +37,12 @@ def __init__(self, pi, enable_pin,
                                          right_encoder_feedback_pin_A,
                                          right_encoder_feedback_pin_B)
 
+        self.pid_kp = pid_params[0]
+        self.pid_kd = pid_params[1]
+        self.pid_ki = pid_params[2]
+        self.pid_max_speed = pid_params[3]
+        self.pid_sample_time = pid_params[4]
+
         # other
         #self._wheelsAxle_lock = threading.RLock() # race condition lock
 
@@ -118,70 +125,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
-        MAX_SPEED = 180
-        target_speed_left = (MAX_SPEED / 100) * power_left #velocity [mm/s]
-        target_speed_right = (MAX_SPEED / 100) * power_right  # velocity [mm/s]
+        
+        target_speed_left = (self.pid_max_speed / 100) * power_left #velocity [mm/s]
+        target_speed_right = (self.pid_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
+        # KP = 0.04  #proportional coefficient
+        # KD = 0.02  # derivative coefficient
+        # KI = 0.005 # integral coefficient
 
         # MEDIUM RESPONSE
-        KP = 0.4  #proportional coefficient
-        KD = 0.1 # derivative coefficient
-        KI = 0.02 # integral coefficient
+        # KP = 0.9  # proportional coefficient
+        # KD = 0.1  # derivative coefficient
+        # KI = 0.05 # integral coefficient
 
         # STRONG RESPONSE
-        #KP = 0.9   # proportional coefficient
-        #KD = 0.05  # derivative coefficient
-        #KI = 0.03  # integral coefficient
-
-        SAMPLETIME = 0.01
+        # KP = 0.9   # proportional coefficient
+        # KD = 0.05  # derivative coefficient
+        # KI = 0.03  # integral coefficient
 
         left_derivative_error = 0
         right_derivative_error = 0
         left_integral_error = 0
         right_integral_error = 0
+        left_prev_error = 0
+        right_prev_error = 0
         # moving for certaing amount of distance
         logging.info("moving? " + str(self._is_moving) + " distance: " + str(self.distance()) + " target: " + str(target_distance))
         while(abs(self.distance()) < abs(target_distance) and self._is_moving == True):
             # PI controller
             logging.debug("speed.left: " + str(self._left_motor.speed()) + " speed.right: " + str(self._right_motor.speed()))
             if(abs(self._left_motor.speed()) > 10 and abs(self._right_motor.speed()) > 10):
                 # relative error
-                left_error = (target_speed_left - self._left_motor.speed()) / target_speed_left * 100.0
-                right_error = (target_speed_right - self._right_motor.speed()) / target_speed_right * 100.0
+                left_error = float(target_speed_left - self._left_motor.speed()) / target_speed_left
+                right_error = float(target_speed_right - self._right_motor.speed()) / target_speed_right
 
-                left_correction = (left_error * KP) + (left_derivative_error * KD) + (left_integral_error * KI)
-                right_correction = (right_error * KP) + (right_derivative_error * KD) + (right_integral_error * KI)
+                left_correction = (left_error * self.pid_kp) + (left_derivative_error * self.pid_kd) + (left_integral_error * self.pid_ki)
+                right_correction = (right_error * self.pid_kp) + (right_derivative_error * self.pid_kd) + (right_integral_error * self.pid_ki)
 
-                corrected_power_left = power_left + left_correction - right_correction
-                corrected_power_right  = power_right + right_correction - left_correction
+                corrected_power_left = power_left + (left_correction * power_left)
+                corrected_power_right  = power_right + (right_correction * power_right)
 
                 #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
-                power_left_norm = max(min(corrected_power_left, 100), 0)
-                power_right_norm =  max(min(corrected_power_right, 100), 0)
+                power_left_norm = max(min(corrected_power_left, power_left), 0)
+                power_right_norm =  max(min(corrected_power_right, power_right), 0)
 
-                logging.info("ls:" + str(int(self._left_motor.speed())) + " rs: " + str(int(self._right_motor.speed())) + 
-                              " le:" + str(int(left_error)) + " re: " + str(int(right_error)) + 
+                logging.debug("ls:" + str(int(self._left_motor.speed())) + " rs: " + str(int(self._right_motor.speed())) + 
+                              " le:" + str(left_error) + " re: " + str(right_error) + 
+                              " ld:" + str(left_derivative_error) + " rd: " + str(right_derivative_error) + 
+                              " li:" + str(left_integral_error) + " ri: " + str(right_integral_error) + 
                               " lc: " + str(int(left_correction)) + " rc: " + str(int(right_correction)) + 
                               " lp: " + str(int(power_left_norm)) + " rp: " + str(int(power_right_norm)))
  
                 # adjusting power on each motors
                 self._left_motor.adjust_power(power_left_norm * left_direction )
                 self._right_motor.adjust_power(power_right_norm * right_direction)
 
-                left_derivative_error = left_error
-                right_derivative_error = right_error
-                left_integral_error += left_error
-                right_integral_error += right_error
+                left_derivative_error = (left_error - left_prev_error) / self.pid_sample_time
+                right_derivative_error = (right_error - right_prev_error) / self.pid_sample_time
+                left_integral_error += (left_error * self.pid_sample_time)
+                right_integral_error += (right_error * self.pid_sample_time)
+
+                left_prev_error = left_error
+                right_prev_error = right_error
 
             # checking each SAMPLETIME seconds
-            sleep(SAMPLETIME)
+            sleep(self.pid_sample_time)
 
         logging.info("control_distance.stop, target dist: " + str(target_distance) + 
             " actual distance: " + str(self.distance()) + 
diff --git a/coderbot/v1.yml b/coderbot/v1.yml
@@ -642,6 +642,11 @@ components:
           minimum: -1.0
           maximum: 1.0
           description: Duration of the movement. 0 doesn't move the coderbot.
+        distance:
+          type: number
+          minimum: 0.0
+          maximum: 1000.0
+          description: Target distqnce in mm.
       required:
         - speed
         - elapse
diff --git a/defaults/config.json b/defaults/config.json
