add encoder to control_time (with t > 0)

Author: previ

coderbot/main.py

@@ -68,15 +68,7 @@ def run_server():
         try:
             app.bot_config = Config.read()
 
-            bot = CoderBot.get_instance(motor_trim_factor=float(app.bot_config.get('move_motor_trim', 1.0)),
-                                        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))))
+            bot = CoderBot.get_instance()
 
             try:
                 audio_device = Audio.get_instance()

coderbot/rotary_encoder/wheelsaxel.py

@@ -1,6 +1,6 @@
 import pigpio
 import threading
-from time import sleep
+from time import sleep, time
 import logging
 
 from rotary_encoder.motorencoder import MotorEncoder
@@ -90,7 +90,8 @@ def control(self, power_left=100, power_right=100, time_elapse=None, target_dist
     """ Motor time control allows the motors
         to run for a certain amount of time """
     def control_time(self, power_left=100, power_right=100, time_elapse=-1):
-        #self._wheelsAxle_lock.acquire() # wheelsAxle lock acquire
+        if time_elapse > 0:
+            return self.control_time_encoder(power_left, power_right, time_elapse)
 
         # applying tension to motors
         self._left_motor.control(power_left, -1)
@@ -103,6 +104,92 @@ def control_time(self, power_left=100, power_right=100, time_elapse=-1):
             sleep(time_elapse)
             self.stop()
 
+    """ Motor time control allows the motors
+        to run for a certain amount of time """
+    def control_time_encoder(self, power_left=100, power_right=100, time_elapse=-1):
+        #self._wheelsAxle_lock.acquire() # wheelsAxle lock acquire
+        self._is_moving = True
+
+        # get desired direction from power, then normalize on power > 0 
+        left_direction = power_left/abs(power_left)
+        right_direction = power_right/abs(power_right)
+        power_left = abs(power_left)
+        power_right = abs(power_right)
+
+        self._left_motor.reset_state()
+        self._right_motor.reset_state()
+
+        # applying tension to motors
+        self._left_motor.control(power_left * left_direction)
+        self._right_motor.control(power_right * right_direction)
+
+        #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_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]
+
+        left_derivative_error = 0
+        right_derivative_error = 0
+        left_integral_error = 0
+        right_integral_error = 0
+        left_prev_error = 0
+        right_prev_error = 0
+        time_init = time()
+
+        # moving for certaing amount of distance
+        logging.info("moving? " + str(self._is_moving) + " distance: " + str(self.distance()) + " target: " + str(time_elapse))
+        while(time() - time_init < time_elapse 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 = 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 * 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 * 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, power_left), 0)
+                power_right_norm =  max(min(corrected_power_right, power_right), 0)
+
+                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 - 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(self.pid_sample_time)
+
+        logging.info("control_distance.stop, target elapse: " + str(time_elapse) + 
+            " actual distance: " + str(self.distance()) + 
+            " l ticks: " + str(self._left_motor.ticks()) + 
+            " r ticks: " + str(self._right_motor.ticks()))
+        # robot arrived
+        self.stop()
+
     """ Motor distance control allows the motors
             to run for a certain amount of distance (mm) """
     def control_distance(self, power_left=100, power_right=100, target_distance=0):

coderbot/v1.yml

@@ -446,7 +446,7 @@ paths:
       tags:
         - Direct control
       requestBody:
-        description: Movement speed and duration
+        description: Movement speed and duration or distance
         required: true
         content:
           application/json:
@@ -464,12 +464,14 @@ paths:
       tags:
         - Direct control
       requestBody:
-        description: Movement Speed and duration
+        description: Movement Speed and duration or distance
         required: true
         content:
           application/json:
             schema:
-              $ref: '#/components/schemas/TurnParams'
+              oneOf:
+                - $ref: '#/components/schemas/TurnParamsElapse'
+                - $ref: '#/components/schemas/TurnParamsDistance'
       responses:
         200:
           description: Sent command to the bot GPIO.
@@ -629,7 +631,7 @@ components:
       required:
         - speed
         - distance
-    TurnParams:
+    TurnParamsElapse:
       type: object
       properties:
         speed:
@@ -642,14 +644,25 @@ components:
           minimum: -1.0
           maximum: 1.0
           description: Duration of the movement. 0 doesn't move the coderbot.
+      required:
+        - speed
+        - elapse
+    TurnParamsDistance:
+      type: object
+      properties:
+        speed:
+          type: number
+          minimum: -100.0
+          maximum: 100.0
+          description: 0 to 100 represent a forward movement (100 being the maximum speed), while 0 to -100 is a backward movement (-100 being the maximu speed)
         distance:
           type: number
           minimum: 0.0
-          maximum: 1000.0
+          maximum: 100000.0
           description: Target distqnce in mm.
       required:
         - speed
-        - elapse
+        - distance
     Settings:
       type: object
     Photo:

defaults/config.json

@@ -36,17 +36,20 @@
   "ctrl_hud_image":"",
   "load_at_start":"",
   "sound_start":"$startup",
-  "hw_version":"5",
+  "hardware_version":"5",
   "audio_volume_level":"100",
   "wifi_mode":"ap",
   "wifi_ssid":"coderbot",
   "wifi_psk":"coderbot",
   "packages_installed":"",
   "admin_password":"",
   "hardware_version":"5",
-  "pid_kp":"0.9",
-  "pid_kd":"0.1",
-  "pid_ki":"0.01",
+  "pid_kp":"8.0",
+  "pid_kd":"0.0",
+  "pid_ki":"0.0",
   "pid_max_speed":"200",
-  "pid_sample_time":"0.01"
+  "pid_sample_time":"0.05",
+  "movement_use_mpu": "false",
+  "movement_use_motion": "false",
+  "movement_use_encoder": "true"
 }