Functions to control both wheels on distance and time elapse

Author: sorre97

coderbot.py

@@ -26,8 +26,8 @@
 import mpu
 
 PIN_MOTOR_ENABLE = 22
-PIN_LEFT_FORWARD = 25
-PIN_LEFT_BACKWARD = 24
+PIN_LEFT_FORWARD = 25   # BACKWARD
+PIN_LEFT_BACKWARD = 24  # FORWARD
 PIN_RIGHT_FORWARD = 4
 PIN_RIGHT_BACKWARD = 17
 PIN_PUSHBUTTON = 11

rotary_encoder/motorencoder.py

@@ -67,19 +67,19 @@ def is_moving(self):
     def control(self, power=100.0, time_elapse=0):
         self._motor_lock.acquire()  # acquiring lock
 
-        self._direction = 1 if power > 0 else -1  # setting direction according to speed
-        self._power = power
         self.stop()  # stopping motor to initialize new movement
+
+        self._direction = 1 if power > 0 else -1  # setting direction according to speed
+        self._power = power # setting current power
+
         self._pi.write(self._enable_pin, True)  # enabling motors
 
         # going forward
-        if (self._direction):
+        if (self._direction == 1):
             self._pi.set_PWM_dutycycle(self._forward_pin, abs(power))
-            self._direction = 1
         # going bacakward
         else:
             self._pi.set_PWM_dutycycle(self._backward_pin, abs(power))
-            self._direction = -1
 
         self._is_moving = True
 

rotary_encoder/wheelsaxel.py

@@ -52,22 +52,67 @@ def speed(self):
         return (l_speed + r_speed) / 2
 
     # MOVEMENT
-    def control(self, power_left = 100, power_right = 100, time_elapse = 0):
-
-        self._left_motor.control(power_left, time_elapse)
-        self._right_motor.control(power_right, time_elapse)
+    def control_time(self, power_left=100, power_right=100, time_elapse=0):
+         self._wheelsAxle_lock.acquire()
+         self._left_motor.control(power_left)
+         self._right_motor.control(power_right)
+         self._is_moving = True
+
+         if(time_elapse > 0):
+             sleep(time_elapse)
+             self.stop()
+
+    def control_distance(self, power_left=100, power_right=100, target_distance=0):
+        self._wheelsAxle_lock.acquire()
         self._is_moving = True
 
-        if(time_elapse > 0):
-            sleep(time_elapse)
-            self.stop()
+        self._left_motor.control(power_left)
+        self._right_motor.control(power_right)
+
+        while(target_distance > 0):
+            target_distance = target_distance - self.distance()
+            print(str(target_distance))
+
+        self.stop()
+
+    # def control_distance(self, power_left = 100, power_right = 100, target_distance = 0):
+    #     self._is_moving = True
+    #     delta = 1
+    #
+    #     while(target_distance > 0):
+    #
+    #         self._left_motor.control(min(max(power_left * delta, power_left), 100))
+    #         self._right_motor.control(min(max(power_right * delta, power_right), 100))
+    #
+    #         print("Target distance: " + str(target_distance))
+    #         print("Power left: " + str(power_left * delta))
+    #         print("Power right: " + str(power_right * delta))
+    #
+    #         sleep(.2)
+    #
+    #         try:
+    #             delta = self._left_motor.ticks() / self._right_motor.ticks()
+    #             target_distance = target_distance - self.distance()
+    #         except:
+    #             delta = 1
+    #
+    #         print("delta_ticks: " + str(delta))
+    #         print("new target distance: " + str(target_distance))
+    #         print("")
+    #
+    #     print("Stopping...")
+    #     self.stop()
 
     """ The stop function calls the two stop functions of the two
-        correspondent motors """
+        correspondent motors 
+        locks are automatically obtained """
     def stop(self):
         self._left_motor.stop()
         self._right_motor.stop()
+        print(self._left_motor.distance())
+        print(self._right_motor._distance)
         self._is_moving = False
+        self._wheelsAxle_lock.release()
 
     # CALLBACK