Python program to control the AAT MS5 robot on patrol mode. The tank will move in circles, while the droid in the blaster will be looking for any sympathisants of the Republic. When the distance sensor detects a target, the tank will stop and the droid will center the blasters to fire!

You can find a video of the robot functioning here.

Required robot

Source code

You can find the code in the accompanying .py file. To get it running, simply copy and paste it in a new Mindstorms project.


In [ ]:
from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App
from mindstorms.control import wait_for_seconds, wait_until, Timer
from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to
import math

import hub
In [ ]:
print("-"*15 + " Execution started " + "-"*15 + "\n")

Initialize hub

Notice we won't be using the standard MSHub, but rather the "raw" hub. It is a little lower level, but it allows us making more things. Fore more information, see Maarten Pennings brilliant explanation and unofficial documentation about it.

In [ ]:
# hub = MSHub()
In [ ]:
# hub.status_light.on('black')
hub.led(0, 0, 0)

Initialize motors

In [ ]:
print("Configuring motors...")
motor_steer = Motor('A') # Front wheels (for steering)
motor_power = Motor('C') # Back wheels (for moving)

motor_turret = Motor('B') # Turrent spinning
In [ ]:
print("Setting motors to position 0...")
motor_steer.run_to_position(45, speed=100)
motor_steer.run_to_position(0, speed=100)

motor_turret.run_to_position(0, speed=75)

Initialize distance sensor

In [ ]:
print("Initializing distance sensor...")
distance_sensor = DistanceSensor('D')

Put the AAT MS5 in motion

The tank will move until the distance sensor detects an obstacle.

The steering is given by POSITION.

  • A value between 0 and 90 will steer the tank to the left.

    • A value closer to 0 will make the tank turn wider.
    • A value closer to 90 will make the tank turn tighter.
  • A value between 270 and 360 will steer the tank to the right.

    • A value closer to 270 will make the tank turn tighter.
    • A value closer to 360 will make the tank turn wider.
In [ ]:

motor_steer.run_to_position(POSITION, speed=35)

The tank speed is given by SPEED. It should have a value between -100 and 100.

  • A negative value will move the tank forward.
  • A positive value will move the tank backwards.

Recommended value is -50

In [ ]:
SPEED = -50


Configure the patrolling

We will move the turret constantly. It will go from left to right and from right to left. When an obstacle is detected, the turret will go back to the initial position and "fire".

Define distance function

As part of the program, we need to continuously check if the measured distance is less than 10 cm.

In [ ]:

However, if the sensor reads no measure, it will return a None, which in turn will generate an error (since we cannot do a comparision between a None and something else).

To solve this, we will define our own cuestom distance function. This way, when the sensor has no reading, we will just return a (simulated) very long distance (instead of returning a None). This will allow us to safely do the comparision.

In [ ]:
def my_get_distance_cm():
        Distance value (in cm).
        If the sensor returns a None, it returns a very large value (1000).
    distance = distance_sensor.get_distance_cm()
    if distance == None:
        distance = 10000
    return distance


Now, in order to be able to stop the turret at any moment (and not until the motor has completed a whole sweep), we will use co-routines.

This is a simplified version David Lechner's trick, which I've used before in Charlie's drum_solo. In this case, we are only controlling one motor (the turret) and we don't depend on time (but rather on the motor position). Thus, we don't need a timer.

We need to define a function for moving the turret. Pay attention to the comments, since they explain how using co-routines work. It isn't very hard, but it isn't trivial either.

In [ ]:
def move_turret():

    Moves the AAT MS5 turrent.
    # First, we need to define the coroutine.
    # In this case, we only need one (corresponding to the turret motor).
    # Notice how the definition is very similar to that of a function.
    # Coroutines also have input parameters.
    # However, they have no "output" (i.e., return), but actually a yield.
    def background_turret(angle):
            The angle at which the turret turns. 
            In practice, this value is twice the original angle, since
            it moves completely from one side to the other (and not from
            the center to one side). That is why it is passed to this 
            function multiplied by two.
            In degrees.
        # We want to make sure counted degrees are initialized at 0.
        # Notice that we use the absolute value of the counted degrees.
        # This is to ensure that it works on the way back (when the measured
        # degrees would be negative).
        curr_turret_position = math.fabs(motor_turret.get_degrees_counted())

        # Here, we check if the motor has reached the desired angle.
        while curr_turret_position < angle:
            # If you wish to see the current turret position and the target angle,
            # uncomment the following line.
            # print(str(curr_turret_position) + " - " +  str(angle))
            # We update the turret current position.
            curr_turret_position = math.fabs(motor_turret.get_degrees_counted())
            # If the turret hasn't reached the desired angle, we reach this yield.
            # yield lets the rest of the program run until we come back
            # here again later to check if the condition was met.

    def turret_patrol():
        while True:

            # This is how we receive a parameter.
            # In this case, it corresponds to the angle the motor should move.
            angle_action = yield

            # We make sure we only execute code if the received
            # value was transmitted correctly.
            if not angle_action == None:
                # We will start to move the turret...
                # ...and check if its angle exceeded the maximum allowed.
                # First we move the turret from left to right...
                yield from background_turret(angle_action*2)
                hub.sound.beep(150, 200, hub.sound.SOUND_SIN) # Play simple tone

                # ...and from right to left (exactly same process, but inverted speed).
                yield from background_turret(angle_action*2)
                # hub.sound.play("/extra_files/Ping")
                hub.sound.beep(150, 200, hub.sound.SOUND_SIN) # Play simple tone

                # We assume that the movement is immediate and takes no time.
                # This isn't completely true, but for now it works.

    # Since turret_patrol() is a coroutine and uses yield
    # (i.e., it isn't a function and thus has no return), it will NOT
    # run here when we call it. Instead, it will just be created as a generator object.
    # This generator will be used to run the functions one yield (i.e., step) at a time.
    turret_generator = turret_patrol()

    # Now we will actually start the task.
    # The task (turret patrolling) will be run as long as the distance sensor
    # doesn't detect an obstacle.
    while my_get_distance_cm() > OBSTACLE_DISTANCE:

        wait_for_seconds(0.01) # Small pause between steps.

    return None

After we have defined the turret movement, we can now make the AAT MS5 patrol until it finds those pesky Republic supporters!

In [ ]:

print("Initializing turret with angle " + str(TURRET_ANGLE) + "...")

print("Starting patrolling...")

Once it finds an enemy (i.e, it detects an obstacle), it will stop and center the turret.

In [ ]:
print("Enemy detected! Attack!")
motor_power.stop() # Stop the movement
motor_turret.run_to_position(0, speed=75) # Center the turret

Then, it will fire three blasters. Each blaster will come with a sound and an animation of the blaster moving in the hub.

First, lets define the frames of the animation.

In [ ]:
print("Defining animation frames...")

frames = ['00000:00000:00000:00000:00000',

n_frames = len(frames)
t_pause = 0.05 # Pause between frames (in seconds)


Then, let's proceed with the actual fire!

In [ ]:
print("Firing blasters...")

n_blasters = 3

for ii in range(0, n_blasters):

    # Play blaster sound.

    # Display blaster animation.
    for ii in range(0, n_frames):
        img = hub.Image(frames[ii])


In [ ]:
print("Target eliminated.")
In [ ]:
print("-"*15 + " Execution ended " + "-"*15 + "\n")