Fixed bug on distance control movement, extended routes with new distance control movement, new test cases

Author: sorre97

api.py

@@ -106,11 +106,11 @@ def stop():
     return 200
 
 def move(data):
-    bot.move(speed=data["speed"], elapse=data["elapse"])
+    bot.move(speed=data["speed"], time_elapse=data["elapse"], target_distance=data["distance"])
     return 200
 
 def turn(data):
-    bot.turn(speed=data["speed"], elapse=data["elapse"])
+    bot.turn(speed=data["speed"], time_elapse=data["elapse"])
     return 200
 
 def exec(data):

coderbot.py

@@ -114,35 +114,36 @@ def get_instance(cls, servo=False, motor_trim_factor=1.0):
             cls.the_bot = CoderBot(servo, motor_trim_factor)
         return cls.the_bot
 
-    def move(self, speed=100, time_elapse=0, target_distance=0):
+    def move(self, speed=100, elapse=0, distance=0):
+        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))
-        self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=time_elapse, target_distance=target_distance)
+        self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance)
 
-    def turn(self, speed=100, time_elapse=0):
+    def turn(self, speed=100, elapse=0):
         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=time_elapse)
+        self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse)
 
     def turn_angle(self, speed=100, angle=0):
         z = self._ag.get_gyro_data()['z']
-        self.turn(speed, time_elapse=0)
+        self.turn(speed, elapse=0)
         while abs(z - self._ag.get_gyro_data()['z']) < angle:
             time.sleep(0.05)
             logging.info(self._ag.get_gyro_data()['z'])
         self.stop()
 
-    def forward(self, speed=100, time_elapse=0):
-        self.move(speed=speed, time_elapse=time_elapse)
+    def forward(self, speed=100, elapse=0, distance=0):
+        self.move(speed=speed, elapse=elapse, distance=distance)
 
-    def backward(self, speed=100, time_elapse=0):
-        self.move(speed=-speed, time_elapse=elapse)
+    def backward(self, speed=100, elapse=0, distance=0):
+        self.move(speed=-speed, elapse=elapse, distance=distance)
 
-    def left(self, speed=100, time_elapse=0):
-        self.turn(speed=-speed, time_elapse=time_elapse)
+    def left(self, speed=100, elapse=0):
+        self.turn(speed=-speed, elapse=elapse)
 
-    def right(self, speed=100, time_elapse=0):
-        self.turn(speed=speed, time_elapse=time_elapse)
+    def right(self, speed=100, elapse=0):
+        self.turn(speed=speed, elapse=elapse)
 
     def get_sonar_distance(self, sonar_id=0):
         return self.sonar[sonar_id].get_distance()

rotary_encoder/rotarydecoder.py

@@ -38,6 +38,18 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
                      1 raising edge
                      2 from watchdog
        
+                      +---------+         +---------+      0   
+                      |         |         |         |
+            B         |         |         |         |
+                      |         |         |         |
+            +---------+         +---------+         +----- 1   # B leading A
+                +---------+         +---------+            0   # forward
+                |         |         |         |
+            A   |         |         |         |
+                |         |         |         |
+            ----+         +---------+         +----------+ 1   
+            
+            
                 +---------+         +---------+            0
                 |         |         |         |
             A   |         |         |         |
@@ -48,17 +60,6 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
             B         |         |         |         |
                       |         |         |         |
             +---------+         +---------+         +----- 1
-            
-                      +---------+         +---------+      0   
-                      |         |         |         |
-            A         |         |         |         |
-                      |         |         |         |
-            +---------+         +---------+         +----- 1   # B leading A
-                +---------+         +---------+            0   # forward
-                |         |         |         |
-            B   |         |         |         |
-                |         |         |         |
-            ----+         +---------+         +----------+ 1   
    """
    def _pulse(self, gpio, level, tick):
       # interrupt comes from pin A

rotary_encoder/wheelsaxel.py

@@ -45,13 +45,13 @@ def __init__(self, pi, enable_pin,
     def distance(self):
         l_dist = self._left_motor.distance()
         r_dist = self._right_motor.distance()
-        return (l_dist + r_dist) / 2
+        return (l_dist + r_dist) * 0.5
 
     #speed
     def speed(self):
         l_speed = self._left_motor.speed()
         r_speed = self._right_motor.speed()
-        return (l_speed + r_speed) / 2
+        return (l_speed + r_speed) * 0.5
 
     # MOVEMENT
     """ Movement wrapper method 
@@ -91,10 +91,8 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0):
         self._right_motor.control(power_right)
 
         # moving for certaing amount of distance
-        while(self.distance() < target_distance):
-            sleep(0.05) # check if arrived every 50ms,
-            print(str(self.distance()))
-            target_distance = target_distance - self.distance() # updating target distance
+        while(abs(self.distance()) < target_distance):
+            pass # busy waiting
 
         # robot arrived
         self.stop()
@@ -110,7 +108,6 @@ def control_velocity(self, time_elapse=0, target_distance=0):
     def stop(self):
         self._left_motor.stop()
         self._right_motor.stop()
-
         self._is_moving = False
         self._wheelsAxle_lock.release()
 

rotary_encoder_tests/rotary_encoder_coderbot_test.py

@@ -0,0 +1,8 @@
+from coderbot import CoderBot
+
+c = CoderBot.get_instance();
+
+c._twin_motors_enc.control_distance(100, 100, 200)
+
+while(True):
+    print(c._twin_motors_enc.distance())

rotary_encoder_tests/rotary_encoder_wheelsAxel_test.py

@@ -0,0 +1,43 @@
+from wheelsaxel import WheelsAxel
+import pigpio
+
+# GPIO
+# motors
+PIN_MOTOR_ENABLE = 22
+PIN_LEFT_FORWARD = 25
+PIN_LEFT_BACKWARD = 24
+PIN_RIGHT_FORWARD = 4
+PIN_RIGHT_BACKWARD = 17
+#?
+PIN_PUSHBUTTON = 11
+# servo
+PIN_SERVO_3 = 9
+PIN_SERVO_4 = 10
+# sonar
+PIN_SONAR_1_TRIGGER = 18
+PIN_SONAR_1_ECHO = 7
+PIN_SONAR_2_TRIGGER = 18
+PIN_SONAR_2_ECHO = 8
+PIN_SONAR_3_TRIGGER = 18
+PIN_SONAR_3_ECHO = 23
+# encoder
+PIN_ENCODER_LEFT_A = 14
+PIN_ENCODER_LEFT_B = 6
+PIN_ENCODER_RIGHT_A = 15
+PIN_ENCODER_RIGHT_B = 12
+
+pi = pigpio.pi()
+
+twin_motors_enc = WheelsAxel(
+            pi,
+            enable_pin=PIN_MOTOR_ENABLE,
+            left_forward_pin=PIN_LEFT_FORWARD,
+            left_backward_pin=PIN_LEFT_BACKWARD,
+            left_encoder_feedback_pin_A=PIN_ENCODER_LEFT_A,
+            left_encoder_feedback_pin_B=PIN_ENCODER_LEFT_B,
+            right_forward_pin=PIN_RIGHT_FORWARD,
+            right_backward_pin=PIN_RIGHT_BACKWARD,
+            right_encoder_feedback_pin_A=PIN_ENCODER_RIGHT_A,
+            right_encoder_feedback_pin_B=PIN_ENCODER_RIGHT_B)
+
+twin_motors_enc.control_distance(100, 100, 200)

v2.yml

@@ -209,17 +209,21 @@ paths:
           description: Movement speed and duration
           schema:
             type: object
-            default: {'speed': 100, 'elapse':-1}
+            default: {'speed': 100, 'elapse':0, 'distance':0}
             required:
               - speed
               - elapse
+              - distance
             properties:
               speed:
                 type: number
                 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)
               elapse:
                 type: number
-                description: Duration of the movement. -1 moves the bot until a /stop command is received.
+                description: Duration of the movement. 0 doesn't move the coderbot.
+              distance:
+                type: number
+                description: Target distance for the bot to travel
       responses:
         200:
           description: Sent command to the bot GPIO.