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.
You can find the code in the accompanying
.py file. To get it running, simply copy and paste it in a new Mindstorms project.
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
print("-"*15 + " Execution started " + "-"*15 + "\n")
Notice we won't be using the standard
MSHub, but rather the "raw"
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.
# hub = MSHub()
# hub.status_light.on('black') hub.led(0, 0, 0)
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
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) print("DONE!")
print("Initializing distance sensor...") distance_sensor = DistanceSensor('D') print("DONE!")
The tank will move until the distance sensor detects an obstacle.
The steering is given by
A value between
90 will steer the tank to the left.
0will make the tank turn wider.
90will make the tank turn tighter.
A value between
360 will steer the tank to the right.
270will make the tank turn tighter.
360will make the tank turn wider.
POSITION = 270 print("Steering...") motor_steer.run_to_position(POSITION, speed=35) print("DONE!")
The tank speed is given by
SPEED. It should have a value between
Recommended value is
SPEED = -50 print("Moving...") motor_power.start(SPEED) print("DONE!")
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".
As part of the program, we need to continuously check if the measured distance is less than 10 cm.
OBSTACLE_DISTANCE = 10 # [cm]
However, if the sensor reads no measure, it will return a
in turn will generate an error (since we cannot do a comparision
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
This will allow us to safely do the comparision.
def my_get_distance_cm(): """ Parameters ---------- None Returns ------- dist: 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.
def move_turret(): """ Moves the AAT MS5 turrent. Parameters ---------- None Returns ------- None """ # 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): """ Parameters ---------- 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. motor_turret.set_degrees_counted(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. yield 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... motor_turret.start(10) # ...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). motor_turret.start(-10) 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: next(turret_generator) turret_generator.send(TURRET_ANGLE) 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!
TURRET_ANGLE = 40 print("Initializing turret with angle " + str(TURRET_ANGLE) + "...") motor_turret.set_default_speed(10) motor_turret.run_for_degrees(-TURRET_ANGLE) print("DONE!") print("Starting patrolling...") move_turret() print("DONE!")
Once it finds an enemy (i.e, it detects an obstacle), it will stop and center the turret.
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.
print("Defining animation frames...") frames = ['00000:00000:00000:00000:00000', '00900:00000:00000:00000:00000', '00700:00900:00000:00000:00000', '00500:00700:00900:00000:00000', '00000:00500:00700:00900:00000', '00000:00000:00500:00700:00900', '00000:00000:00000:00500:00700', '00000:00000:00000:00000:00500', '00000:00000:00000:00000:00000'] n_frames = len(frames) t_pause = 0.05 # Pause between frames (in seconds) print("DONE!")
Then, let's proceed with the actual fire!
print("Firing blasters...") n_blasters = 3 for ii in range(0, n_blasters): # Play blaster sound. hub.sound.play("/extra_files/Laser") # Display blaster animation. for ii in range(0, n_frames): img = hub.Image(frames[ii]) hub.display.show(img) wait_for_seconds(t_pause) wait_for_seconds(0.5) print("DONE!")
print("-"*15 + " Execution ended " + "-"*15 + "\n")