How to tell when BrickPi3 has completed command

BrickPi
1

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

2

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()
3

“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.

4

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
5

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.

6

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!

7

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.

8

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
        
    def drive_in_circle(self, isRightHand, radius, speed):
        """
        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.
        Note: ratio(WheelSpeed1:WheelSpeed2) = ratio(CircleRadius:(CircleRadius+TrackWidth))
        """

        if radius == 0:
            outer_wheel_speed = speed
            inner_wheel_speed = 0
        elif radius > 0:
            fraction = radius / (radius + self.WHEEL_TRACK_WIDTH)
            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:
        left_distance, center_distance, right_distance = read_ultrasonics()
        # 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 :slight_smile: Now to put some spinning blades on it, and get it to cut my lawn!