# How to tell when BrickPi3 has completed command

Hi,

I’ve built a small robot with differential steering via left and right NXT motors at the back and free rotating castors at the front.

As a quick test I thought I’d write a python script to go forwards\backwards\turn left\right and then string a number of moves together. At this point I hit my first problem.

My understanding is, when I call “set_motor_position_relative()”, the Pi sends a command to the chip on the BrickPi3. At this point, the BrickPi3 reads the encoder values, runs its PD algorithm and starts sending power to the actual motor.

However the Python script has continued without waiting for the command to complete, so while the motors are still in motion and the target position has not yet been reached, the python script has gone onto the next command to turn left\go backwards\whatever.

Is there a way to get a callback from the BrickPi3 when the desired target position has been reached? Failing that is there a standard way of polling the BrickPi3 to ask if it has finished the last command?

Regards,
Ralph

Its very unfinished, but here’s my code so far:

import brickpi3
import math
import time

# Global
BP = brickpi3.BrickPi3()
WHEEL_DIAMETER = 8.2 # CM
GEAR_RATIO = 12 / 20  # 12t:20t
WHEEL_TRACK_WIDTH = 15.00 # CM (nice round number, what are the chances!)

LEFT_MOTOR = BP.PORT_B
RIGHT_MOTOR = BP.PORT_C

def distance_to_motor_degrees(distance):
wheel_circumference = math.pi * WHEEL_DIAMETER
num_wheel_revolutions = distance / wheel_circumference
num_motor_revolutions = num_wheel_revolutions / GEAR_RATIO
motor_degrees = 360 * num_motor_revolutions
return motor_degrees

def turn_left(angle):
fraction_of_circle = angle / 360
required_distance = fraction_of_circle * (math.pi * WHEEL_TRACK_WIDTH)
motor_degrees = distance_to_motor_degrees(required_distance)
BP.set_motor_power(LEFT_MOTOR, 0)
BP.set_motor_position_relative(RIGHT_MOTOR, motor_degrees)

def move_forward(distance):
motor_degrees = distance_to_motor_degrees(distance)
BP.set_motor_position_relative(LEFT_MOTOR, motor_degrees)
BP.set_motor_position_relative(RIGHT_MOTOR, motor_degrees)

if __name__ == "__main__":
required_distance = float(input("Enter number of centimeters to move: "))
try:
move_forward(required_distance)
wait_until_move_completed() ???
turn_left(30)
BP.reset_all()
except KeyboardInterrupt:
BP.reset_all()



“Dirty” way would be to loop, checking when the fourth value of both get_motor_status() is zero?

(I’m not very experienced with Python so there may be a “more correct” way to implement…)

# wait for either motor to start running
while ((BP.get_motor_status(LEFT_MOTOR)[3] == 0) and
(BP.get_motor_status(RIGHT_MOTOR)[3] == 0) ):
time.sleep(0.05)

# wait for both motors to be stopped
while ((BP.get_motor_status(LEFT_MOTOR)[3] != 0) and
(BP.get_motor_status(RIGHT_MOTOR)[3] != 0) ):
time.sleep(0.5)


note checking fast for getting started, and slower for stopped.

Thank you for the reply, I can see what that code is trying to do, so I copied it in verbatim. Unfortunately it doesn’t seem to work, or at least it works once on the first move command, but none of the other moves work. The robot moves forward then stops.

I added a bit of debug to your code, just so I could see how many times the while loops iterated:

def wait_for_motors_to_stop():
count = 0
print("  Waiting for motor to start...")
# wait for either motor to start running
while ((BP.get_motor_status(LEFT_MOTOR)[3] == 0) and
(BP.get_motor_status(RIGHT_MOTOR)[3] == 0) ):
time.sleep(0.05)
count += 1
print("  Done: {}".format(count))
count = 0

print("  Waiting for motor to stop...")
# wait for both motors to be stopped
while ((BP.get_motor_status(LEFT_MOTOR)[3] != 0) and
(BP.get_motor_status(RIGHT_MOTOR)[3] != 0) ):
time.sleep(0.25)
count += 1
print("  Done: {}".format(count))


And my main program looks like this:

        print("Move forward")
move_forward(required_distance)
wait_for_motors_to_stop()

print("Turn left")
turn_left(45)
wait_for_motors_to_stop()

print("Turn right")
turn_right(45)
wait_for_motors_to_stop()

print("Move backwards")
move_backward(required_distance)
wait_for_motors_to_stop()

print("Reset")
BP.reset_all()


And the output when the program runs is:

Enter number of centimeters to move: 25
Move forward
motor_degrees: 582.2741820435197
Waiting for motor to start...
Done: 1
Waiting for motor to stop...
Done: 8
Turn left
motor_degrees: 137.1951219512195
Waiting for motor to start...
Done: 1
Waiting for motor to stop...
Done: 0
Turn right
motor_degrees: 137.1951219512195
Waiting for motor to start...
Done: 0
Waiting for motor to stop...
Done: 0
Move backwards
motor_degrees: 582.2741820435197
Waiting for motor to start...
Done: 0
Waiting for motor to stop...
Done: 0
Reset



So now I’m super confused why the move_forward works, but the subsequent ones don’t! Any ideas?

1 Like

Yes, my mistake! The wait for stop needs to be an “or” , not an “and”:

wait while left_motor is turning or right_motor is turning.

with “and” it stops waiting if any one wheel is not turning, which on a left or right command is immediately.

or maybe instead of waiting for stop, use a moving() test which is true if either wheel is turning.

def moving():
return  (BP.get_motor_status(LEFT_MOTOR)[3] != 0) or
(BP.get_motor_status(RIGHT_MOTOR)[3] != 0)


and using it:


move_xxx()
while  not moving(): time.sleep(0.05)   <-- or move this into each move_xxx() command
while moving(): time.sleep(0.1)



The neat thing about the “while moving():” is that you could loop checking the distance sensor, and issue an early stop() if an obstacle appears unexpectedly.

and, or, xor, not and, not a and not b, not a or b: I’m just not thinking logically today.

p.s. It will be interesting to see if this change fixes move_backward(). The and/or mixup might not explain why move_backward() stopped right away.

This is great, and works much better, but it doesn’t work 100% of the time. Quite often, the program hangs and has to be stopped with Ctrl+C. I think it’s due to a servomotor not being able to meet its set point, as when I touch the wheel its vibrating and there is a quiet, but audible buzzing noise. I think this causes the 2nd while moving():  loop to never complete.

I’ll have to look into some sort of timeout, or perhaps a tolerance value for the moving() check. This is starting to get complicated, its a great shame there’s nothing in the BrickPi standard libraries to help here!

Absolutely inevitable. Every time I start a new project for my robot, the goal is clear, my approach is clear (KISS), the excitement is high. The first code is simple, the feeling is “alright, that wasn’t too bad”, and it sort of worked. A few days later the code handles two of fifty special cases I discovered, and looks like a TL;DR novel without any pictures to summarize it.

I commend you on choosing an uncharted path. Sometimes searching github will turn up some gold nuggets. If you take the time to appreciate what you are learning and accomplishing, without worrying about speed of progress, there is real fun to be had.

Finally got my little robot working properly today. I added timeouts to the wait_for_motors_to_stop() function, and also did some commenting and refactoring (apologies for the code quality I’m still learning Python)

"""
Wrapper round brickpi3 class to provide higher level functions
"""
class Movement():
# Constants for current robot
WHEEL_DIAMETER = 8.2 # CM
GEAR_RATIO = 12 / 20  # 12t:20t
WHEEL_TRACK_WIDTH = 15.00 # CM (nice round number, what are the chances!)
LEFT_MOTOR = None
RIGHT_MOTOR = None
BP = None

def __init__(self, BP, left_motor, right_motor):
self.LEFT_MOTOR = left_motor
self.RIGHT_MOTOR = right_motor
self.BP = BP

"""
Sets motor speeds to drive in an un-ending circle with specified radius
Note: radius is measured to the inner driving wheel, not the further out one.
"""

outer_wheel_speed = speed
inner_wheel_speed = 0
outer_wheel_speed = speed
inner_wheel_speed = outer_wheel_speed * fraction

if isRightHand:
self.BP.set_motor_dps(self.LEFT_MOTOR, outer_wheel_speed)
self.BP.set_motor_dps(self.RIGHT_MOTOR, inner_wheel_speed)
else:
self.BP.set_motor_dps(self.LEFT_MOTOR, inner_wheel_speed)
self.BP.set_motor_dps(self.RIGHT_MOTOR, outer_wheel_speed)

def _distance_to_motor_degrees(self, distance):
wheel_circumference = math.pi * self.WHEEL_DIAMETER
num_wheel_revolutions = distance / wheel_circumference
num_motor_revolutions = num_wheel_revolutions / self.GEAR_RATIO
motor_degrees = 360 * num_motor_revolutions
# print("    motor_degrees: {}".format(motor_degrees))
return motor_degrees

def rotate_left(self, angle):
"""
Rotates the robot on the spot by the angle in degrees by rotating one
motor forward and the other backward by an equal amount.
"""
fraction_of_circle = angle / 360
required_distance = fraction_of_circle * (math.pi * self.WHEEL_TRACK_WIDTH)
motor_degrees = self._distance_to_motor_degrees(required_distance)
self.BP.set_motor_position_relative(self.LEFT_MOTOR, motor_degrees)
self.BP.set_motor_position_relative(self.RIGHT_MOTOR, -motor_degrees)

def rotate_right(self, angle):
"""
Rotates the robot on the spot by the angle in degrees by rotating one
motor forward and the other backward by an equal amount.
"""
fraction_of_circle = angle / 360
required_distance = fraction_of_circle * (math.pi * self.WHEEL_TRACK_WIDTH)
motor_degrees = self._distance_to_motor_degrees(required_distance)
self.BP.set_motor_position_relative(self.LEFT_MOTOR, -motor_degrees)
self.BP.set_motor_position_relative(self.RIGHT_MOTOR, motor_degrees)

def turn_left(self, angle):
"""
Turns the robot by stopping one wheel and driving the wheel on the opposite side
by a certain amount.
"""
fraction_of_circle = angle / 360
required_distance = fraction_of_circle * (math.pi * 2 * self.WHEEL_TRACK_WIDTH)
motor_degrees = self._distance_to_motor_degrees(required_distance)
self.BP.set_motor_position_relative(self.LEFT_MOTOR, 0)
self.BP.set_motor_position_relative(self.RIGHT_MOTOR, motor_degrees)

def turn_right(self, angle):
"""
Turns the robot by stopping one wheel and driving the wheel on the opposite side
by a certain amount.
"""
fraction_of_circle = angle / 360
required_distance = fraction_of_circle * (math.pi * 2 * self.WHEEL_TRACK_WIDTH)
motor_degrees = self._distance_to_motor_degrees(required_distance)
self.BP.set_motor_position_relative(self.LEFT_MOTOR, motor_degrees)
self.BP.set_motor_position_relative(self.RIGHT_MOTOR, 0)

def move_forward(self, distance):
motor_degrees = self._distance_to_motor_degrees(distance)
self.BP.set_motor_position_relative(self.LEFT_MOTOR, motor_degrees)
self.BP.set_motor_position_relative(self.RIGHT_MOTOR, motor_degrees)

def move_backward(self, distance):
motor_degrees = self._distance_to_motor_degrees(distance)
self.BP.set_motor_position_relative(self.LEFT_MOTOR, -motor_degrees)
self.BP.set_motor_position_relative(self.RIGHT_MOTOR, -motor_degrees)

def stop_both(self):
self.BP.set_motor_position_relative(self.LEFT_MOTOR, 0)
self.BP.set_motor_position_relative(self.RIGHT_MOTOR, 0)

def wait_for_motors_to_stop(self, timeout_seconds = 10):
"""
This function pauses (time.sleep) until the motors have stopped moving. This
is useful for blocking execution until the motors have rotated to a specified
angle.

Note: Includes timeouts, so only designed for use with turn_left\right,
rotate_left\right functions, move_forward or move_backward functions.
"""
timeout_time_to_start = time.time() + 0.5
while not self.is_moving():
time.sleep(0.05)
if time.time() > timeout_time_to_start:
print("WARN: Timeout waiting for motors to start")
break
timeout_time_to_stop = time.time() + timeout_seconds
while self.is_moving():
time.sleep(0.1)
if time.time() > timeout_time_to_stop:
print("WARN: Timeout waiting for motors to stop")
break

def is_moving(self):
return (self.BP.get_motor_status(self.LEFT_MOTOR)[3] != 0) or (self.BP.get_motor_status(self.RIGHT_MOTOR)[3] != 0)



and my main file calls this class as follows:

import brickpi3
BP = brickpi3.BrickPi3()

LEFT_MOTOR = BP.PORT_B
RIGHT_MOTOR = BP.PORT_C
M = robot_movement.Movement(BP, LEFT_MOTOR, RIGHT_MOTOR)

<snip>
while True:
# Default action
action = 'forward'

if left_distance < 20 or right_distance < 20:
if left_distance < right_distance:
action='right'
else:
action='left'

if center_distance < 20:
action='reverse'

if action == 'forward':
BP.set_motor_dps(LEFT_MOTOR, 200)
BP.set_motor_dps(RIGHT_MOTOR, 200)
elif action == 'reverse':
M.move_backward(30)
M.wait_for_motors_to_stop()
if random.choice([True, False]):
M.rotate_left(90)
M.wait_for_motors_to_stop()
else:
M.rotate_right(90)
M.wait_for_motors_to_stop()
elif action == 'right':
M.rotate_right(90)
M.wait_for_motors_to_stop()
elif action == 'left':
M.rotate_left(90)
M.wait_for_motors_to_stop()


Works great, the robot successfully avoids obstacles Now to put some spinning blades on it, and get it to cut my lawn!