wip PIDcontroller

Author: previ

cnn_models/models.json

@@ -16,5 +16,11 @@
     "image_width": 224,
     "status": 1,
     "image_height": 224
+  },
+  "object_detect_mobile": {
+    "output_layer": "final_result",
+    "image_width": 224,
+    "status": 1,
+    "image_height": 224
   }
 }

main.py

@@ -106,7 +106,7 @@ def serve_legacy():
                            config=app.bot_config,
                            program_level=app.bot_config.get("prog_level", "std"),
                            cam=Camera.get_instance() != None,
-                           cnn_model_names=json.dumps([[name] for name in CNNManager.get_instance().get_models().keys()]))
+                           cnn_model_names=json.dumps([[name, name] for name in CNNManager.get_instance().get_models().keys()]))
 
 @babel.localeselector
 def get_locale():
@@ -198,7 +198,7 @@ def handle_bot():
     except:
         cam = None
         motion = None
-    
+
     audio = Audio.get_instance()
 
     cmd = request.args.get('cmd')

rotary_encoder/motorencoder.py

@@ -149,54 +149,9 @@ def adjust_power(self, power):
         # releasing lock on motor
         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
-    # 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
-
-        self._power = power  # setting current power
-
-        # 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
     """ 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
@@ -205,7 +160,6 @@ def rotary_callback(self, tick):
             - 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

rotary_encoder/wheelsaxel.py

@@ -105,8 +105,8 @@ 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 = 260
-        TARGET_LEFT = (MAX_SPEED/100) * power_left #velocity [mm/s]
+        MAX_SPEED = 180
+        TARGET_LEFT = (MAX_SPEED / 100) * power_left #velocity [mm/s]
         TARGET_RIGHT = (MAX_SPEED / 100) * power_right  # velocity [mm/s]
 
         # SOFT RESPONSE
@@ -115,14 +115,14 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0):
         #KI = 0.005 # integral coefficient
 
         # MEDIUM RESPONSE
-        #KP = 0.8  #proportional coefficient
-        #KD = 0.04 # derivative coefficient
-        #KI = 0.02 # integral coefficient
+        KP = 0.2  #proportional coefficient
+        KD = 0.1 # derivative coefficient
+        KI = 0.02 # integral coefficient
 
         # STRONG RESPONSE
-        KP = 0.9   # proportional coefficient
-        KD = 0.05  # derivative coefficient
-        KI = 0.03  # integral coefficient
+        #KP = 0.9   # proportional coefficient
+        #KD = 0.05  # derivative coefficient
+        #KI = 0.03  # integral coefficient
 
         SAMPLETIME = 0.1
 
@@ -134,45 +134,51 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0):
         left_integral_error = 0
         right_integral_error = 0
 
+        power_left_norm = power_left
+        power_right_norm = power_right
         # moving for certaing amount of distance
         while(abs(self.distance()) < target_distance):
             # PI controller
-
-            # relative error
-            left_error = TARGET_LEFT - self._right_motor.speed()
-            right_error = TARGET_RIGHT - self._left_motor.speed()
-
-            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 / MAX_SPEED, 100), 0)
-            right_power =  max(min(100 * right_speed / MAX_SPEED, 100), 0)
-
-            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("")
-
+            if(self._left_motor.speed() > 10 and self._right_motor.speed() > 10):
+                # relative error
+                left_error = (TARGET_LEFT - self._left_motor.speed())/TARGET_LEFT*100.0
+                right_error = (TARGET_RIGHT - self._right_motor.speed())/TARGET_RIGHT*100.0
+
+                power_left = power_left_norm + (left_error * KP) + (left_derivative_error * KD) + (left_integral_error * KI)
+                power_right  = power_left_norm + (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
+                power_left_norm = max(min(power_left, 100), 0)
+                power_right_norm =  max(min(power_right, 100), 0)
+
+                #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("")
+                print("ml:", int(self._right_motor.speed()), " mr: ", int(self._left_motor.speed()), 
+                      " le:", int(left_error), " re: ", int(right_error), 
+                      " ls: ", int(left_speed), " rs: ", int(right_speed), 
+                      " lp: ", int(power_left), " rp: ", int(power_right))
+ 
+                # adjusting power on each motors
+                self._left_motor.adjust_power(power_left_norm)
+                self._right_motor.adjust_power(power_right_norm)
+
+                #print("Left error: %f" % (left_error))
+                #print("Right error: %f"  % (right_error))
+                #print("")
+
+                left_derivative_error = left_error
+                right_derivative_error = right_error
+                left_integral_error += left_error
+                right_integral_error += right_error
 
             # 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()