wip #64

Author: previ

coderbot.py

@@ -162,8 +162,6 @@ def _dc_motor(self, speed_left=100, speed_right=100, elapse=-1, steps_left=-1, s
         self._encoder_motor_stopping_right = False
         self._encoder_motor_stopped_left = False
         self._encoder_motor_stopped_right = False
-        self._motor_current_power_left = speed_left
-        self._motor_current_power_right = speed_right
         if steps_left >= 0 or steps_right >= 0:
             self._encoder_sem.acquire()
 
@@ -260,6 +258,7 @@ def __init__(self, parent, _pigpio, pin_enable, pin_forward, pin_backward, pin_e
             self._pin_duty = 0
             self._pin_reverse = 0
             self._power = 0.0
+            self._power_actual = 0.0
             self._encoder_dist = 0
             self._encoder_speed = 0.0
             self._encoder_last_tick = 0
@@ -269,61 +268,72 @@ def __init__(self, parent, _pigpio, pin_enable, pin_forward, pin_backward, pin_e
             self._encoder_k_v_1 = 80
             self._motor_stopping = False
             self._motor_running = False
+            self._motor_stop_fast = True 
             self._pigpio.set_mode(self._pin_encoder, pigpio.INPUT)
             self._cb = self._pigpio.callback(self._pin_encoder, pigpio.RISING_EDGE, self._cb_encoder)
+            self._motor_lock = threading.RLock()
 
         def exit(self):
             self._cb.cancel()
 
         def _cb_encoder(self, gpio, level, tick):
+            self._motor_lock.acquire()
             self._encoder_dist += 1
             delta_ticks = tick - self._encoder_last_tick if tick > self._encoder_last_tick else tick - self._encoder_last_tick + 4294967295
             self._encoder_last_tick = tick
             self._encoder_speed = 1000000.0 / delta_ticks #convert speed in steps per second
-            #print "speed: " + str(self._encoder_speed)
-            if self._encoder_dist_target >= 0:
-                delta_s = max(min(self._encoder_speed / self._encoder_k_s_1, 70),0) #delta_s is the delta (in steps)before the target  to reverse the motor in order to arrive at target
+            #print "pin: " + str(self._pin_forward) + " dist: " + str(self._encoder_dist) + " target: " + str(self._encoder_dist_target)
+            if self._encoder_dist_target >= 0 and self._motor_stop_fast:
+                delta_s = max(min(self._encoder_speed / self._encoder_k_s_1, 100),0) #delta_s is the delta (in steps)before the target  to reverse the motor in order to arrive at target
                 #print "pin: " + str(self._pin_forward) + " dist: " + str(self._encoder_dist) + " target: " + str(self._encoder_dist_target) + " delta_s: " + str(delta_s)
                 if (self._encoder_dist >= self._encoder_dist_target - delta_s and
                     not self._motor_stopping and self._motor_running):
                     self._motor_stopping = True
                     self._pigpio.write(self._pin_duty, 0)
                     self._pigpio.set_PWM_dutycycle(self._pin_reverse, self._power)
-                    #print "stopping"
                 elif (self._motor_running and
                     ((self._motor_stopping and
                      self._encoder_speed < self._encoder_k_v_1) or
                      (self._motor_stopping and
                      self._encoder_dist >= self._encoder_dist_target))):
                     self.stop()
-                    self._parent._check_complete()
-                    #print "stopped - pin: " + str(self._pin_forward) + " dist: " + str(self._encoder_dist) + " target: " + str(self._encoder_dist_target)
                     logging.info( "dist: " + str(self._encoder_dist) + " speed: " + str(self._encoder_speed))
                 else:
                     self._parent._cb_encoder(self, gpio, level, tick)
+            if self._encoder_dist_target >= 0 and not self._motor_stop_fast:
+                if self._encoder_dist >= self._encoder_dist_target:
+                    self.stop()
+            self._motor_lock.release()
+            if not self._motor_running:
+                self._parent._check_complete()
 
         def control(self, power=100.0, elapse=-1, speed=100.0, steps=-1):
+            self._motor_lock.acquire()
             self._direction = speed > 0
             self._encoder_dist_target = steps
             self._motor_stopping = False
             self._motor_running = True
             self._encoder_dist = 0
             self._encoder_speed_target = abs(speed)
             self._power = abs(power) #TODO: initial power must be a function of desired speed
+            self._power_actual = abs(power) #TODO: initial power must be a function of desired speed
             self._pin_duty = self._pin_forward if self._direction else self._pin_backward
             self._pin_reverse = self._pin_backward if self._direction else self._pin_forward
             self._pigpio.write(self._pin_reverse, 0)
             self._pigpio.set_PWM_dutycycle(self._pin_duty, self._power)
             self._pigpio.write(self._pin_enable, True)
+            self._motor_lock.release()
             if elapse > 0:
                 time.sleep(elapse)
                 self.stop()
 
         def stop(self):
+            self._motor_lock.acquire()
             self._motor_stopping = False
             self._motor_running = False 
             self._pigpio.write(self._pin_forward, 0)     
             self._pigpio.write(self._pin_backward, 0)
+            self._motor_lock.release()
 
         def distance(self):
             return self._encoder_dist
@@ -338,16 +348,16 @@ def running(self):
             return self._motor_running
 
         def adjust_power(self, power_delta):
-            power = min(max(self._power + power_delta, 0), 100)
-            self._pigpio.set_PWM_dutycycle(self._pin_duty, power)
+            self._power_actual = min(max(self._power + power_delta, 0), 100)
+            self._pigpio.set_PWM_dutycycle(self._pin_duty, self._power_actual)
 
     class TwinMotorsEncoder(object):
         def __init__(self, pigpio, pin_enable, pin_forward_left, pin_backward_left, pin_encoder_left, pin_forward_right, pin_backward_right, pin_encoder_right):
-            self._motor_left = CoderBot.MotorEncoder(self, pigpio, pin_enable, pin_forward_left, pin_backward_left, pin_encoder_left)
-            self._motor_right = CoderBot.MotorEncoder(self, pigpio, pin_enable, pin_forward_right, pin_backward_right, pin_encoder_right) 
             self._straight = False
             self._running = False
             self._encoder_sem = threading.Condition()
+            self._motor_left = CoderBot.MotorEncoder(self, pigpio, pin_enable, pin_forward_left, pin_backward_left, pin_encoder_left)
+            self._motor_right = CoderBot.MotorEncoder(self, pigpio, pin_enable, pin_forward_right, pin_backward_right, pin_encoder_right) 
 
         def exit(self):
             self._motor_left.exit()
@@ -377,6 +387,12 @@ def stop(self):
             self._motor_right.stop()
             self._running = False
 
+        def distance(self):
+            return (self._motor_left.distance() + self._motor_right.distance()) / 2
+
+        def speed(self):
+            return (self._motor_left.speed() + self._motor_right.speed()) / 2
+
         def _cb_encoder(self, motor, gpio, level, tick):
             if (self._straight and self._running and not self._motor_left.stopping() and not self._motor_right.stopping() and
                 abs(self._motor_left.distance() - self._motor_right.distance()) > 2):