def calibrate_charlie():
rightArm.set_stall_detection(True)
leftArm.set_stall_detection(True)
leftArm.start(20)
rightArm.start(-20)
wait_for_seconds(1)
rightArm.run_to_position(0, 'clockwise', 50)
leftArm.run_to_position(0, 'counterclockwise', 50)
leftArm.set_degrees_counted(0)
rightArm.set_degrees_counted(0)
async def on_blue(vm, stack):
ms_hub.status_light.on("blue")
scared_frames = [hub.Image(frame) for frame in scared_animation]
vm.system.display.show(scared_frames,
clear=False,
delay=round(1000 / 8),
loop=True,
fade=1)
vm.system.sound.play("/extra_files/Scared")
wheels.move(-20, speed=80)
wait_for_seconds(1)
async def on_start(vm, stack):
ms_hub.status_light.on("black")
rotate_hub_display_to_value("3")
blinking_frames = [hub.Image(frame) for frame in blinking_animation]
vm.system.display.show(blinking_frames,
clear=False,
delay=round(1000 / 8),
loop=True,
fade=1)
left_hand = Motor("B")
right_hand = Motor("F")
# Calibrate
left_hand.run_to_position(0)
right_hand.run_to_position(0)
vm.system.sound.play("/extra_files/Humming")
for i in range(2):
right_hand.run_for_degrees(-100, 50)
right_hand.run_for_degrees(100, 50)
left_hand.run_for_degrees(100, 50)
left_hand.run_for_degrees(-100, 50)
left_turn = MotorPair("B", "E")
right_turn = MotorPair("A", "F")
vm.system.sound.play("/extra_files/Humming")
for i in range(2):
right_turn.move(100, "degrees", steering=100, speed=50)
right_turn.move(-100, "degrees", steering=100, speed=50)
left_turn.move(100, "degrees", steering=-100, speed=50)
left_turn.move(-100, "degrees", steering=-100, speed=50)
wheels = MotorPair("A", "E")
wheels.move(14 * math.pi, "cm", steering=100, speed=80)
wheels.move(14 * math.pi, "cm", steering=-100, speed=80)
right_hand.run_to_position(270, direction="counterclockwise")
wait_for_seconds(1)
right_hand.run_for_seconds(1, 50)
await vm.system.sound.play_async("/extra_files/Tadaa")
wait_for_seconds(1)
right_hand.run_to_position(0, direction="counterclockwise")
def the_loop(self):
self.hub.light_matrix.write('GO')
self.hub.status_light.on('green')
loop_time_diff = 0
loop_start_time = time.time()
turn_time_diff = 0
turn_start_time = None
while loop_time_diff < self.settings['how_long_run']:
how_far = self.dist_sens.get_distance_cm()
rear_speed = self.settings.get('motor_rear_speed')
if self.turn:
self.hub.status_light.on('yellow')
turn_time_control = time.time()
turn_time_diff = turn_time_control - turn_start_time
if isinstance(
how_far, int
) and how_far <= self.settings['dist_threshold_low']:
self.go_ahead = False
self.end_turn()
elif turn_time_diff >= self.settings['turn_in_seconds']:
self.end_turn()
elif self.go_ahead:
self.hub.status_light.on('green')
rear_speed = rear_speed * -1
if isinstance(
how_far, int
) and how_far <= self.settings['dist_threshold_low']:
self.go_ahead = False
else:
self.hub.status_light.on('blue')
if isinstance(
how_far, int
) and how_far >= self.settings['dist_threshold_high']:
self.turn = True
turn_start_time = time.time()
self.motor_front.run_for_degrees(self.turn_angle)
self.go_ahead = True
self.motor_rear.start(rear_speed)
wait_for_seconds(0.25)
loop_time_control = time.time()
loop_time_diff = loop_time_control - loop_start_time
self.motor_rear.stop()
self.calibrate()
self.hub.light_matrix.write('STOP')
print("DONE!")
# %% [markdown]
# # Make Charlie respond to color
# If red was detected, we get out of the while loop and execute the following code.
# In other words, this is Charlie's response to the color.
# %%
# Stop movement.
motors_wheels.stop()
# %%
hub.light_matrix.show_image('HAPPY')
app.play_sound('Doorbell 1')
# %%
# Open and close Charlie's hatch.
motor_right_arm.run_for_seconds(1)
wait_for_seconds(3)
app.play_sound('Tada')
motor_right_arm.run_to_position(0, direction='counterclockwise')
wait_for_seconds(3)
# %%
# Turn off everything.
distance_sensor.light_up_all(0)
color_sensor.light_up_all(0)
# %%
print("-" * 15 + " Execution ended " + "-" * 15 + "\n")
def play_drums(bars=4, tempo=100):
"""
Makes Charlie play the drums.
I had to read a bit into music to get the concept of bars and tempo.
More importantly, it uses coroutines to simulate async execution
of the arm motors. Both of them are a first time for me.
My apologies if my explantion of any of them isn't completely accurate.
Parameters
----------
bars:
Number of bars (in our case, simple number of cycles).
Default value is 4.
tempo:
Playing tempo (in bpm).
Default value is 100
Returns
-------
None
"""
# First, we will calculate how much should each beat last.
# We will convert the tempo [beats per minute] to period [s].
t_beat = 60 * (1 / tempo)
print("t_beat = " + str(t_beat))
# Now, we will calculate how much each bar should last.
# We will assume a 4/4 tune.
t_bar = t_beat * 4
print("t_bar = " + str(t_bar))
# Lastly, we will calculate how much the whole drumming should last.
t_drumming = t_bar * bars
print("t_drumming = " + str(t_drumming))
# Now, this is where things get interesting. Bear with me.
# First, we need to define the coroutines.
# We need to define two: one for each arm (pretty much identical).
# Notice how the definition is very similar to that of a function.
# Coroutines also have input parameters.
# However, there is no "output" (i.e., return), but actually a yield.
def background_left_arm(t):
"""
Parameters
----------
t:
Time (in seconds) for which the left arm will execute this action.
"""
background_timer_left.reset() # Make sure timer is on 0.
# Here, we check if the execution time of this arm has exceeded
# the desired duration (given by t).
while background_timer_left.now() < t:
# If it hasn't, 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 background_right_arm(t):
"""
Parameters
----------
t:
Time (in seconds) for which the right arm will execute this action.
"""
background_timer_right.reset()
while background_timer_right.now() < t:
yield
def drum_left_hand():
while True:
# This is how we receive a parameter.
# In this case, it corresponds to the time the action should last.
t_action = yield
# We make sure we only execute code if the received
# value was transmitted correctly.
if not t_action == None:
# We will start to move the arm downwards...
motor_left_arm.start_at_power(50)
# ...and check if its duration exceeded the maximum allowed.
# Notice that we need to divide t_action by 2, since the action
# is composed of moving the arm downwards (first half)...
yield from background_left_arm(t_action / 2)
# ...and upwards (second half, same process).
motor_left_arm.start_at_power(-50)
yield from background_left_arm(t_action / 2)
# We assume that the movement is immediate and takes no time.
# This isn't completely true, but for now it works.
def drum_right_hand():
while True:
t_action = yield
if not t_action == None:
# It is worth mentioning a few things regarding the right arm's movement.
# - First, that we multiply t_action by 4 (since we are in a 4/4 tune).
# This is because we want the right arm to take the time of 4 beats.
# - Then, notice that the right arm also uses a lower power. Otherwise
# its trajectory is much longer. Originally, I wanted to try with 12.5 (rounded to 13)
# (a reduction by a factor of 4). Unfortunately, that wasn't enough to move
# the arm at all. Thus, I settled for a factor of 2.
motor_right_arm.start_at_power(-25)
yield from background_right_arm((t_action / 2) * 4)
motor_right_arm.start_at_power(25)
yield from background_right_arm((t_action / 2) * 4)
# Since the drum_left_hand() and drum_right_hand() are coroutines and use yield
# (i.e., they are not functions and thus have no return), they will NOT
# run here when we call them. Instead, they will just be created as generator objects.
# These generators will be used to run the functions one yield (i.e., step) at a time.
left_generator = drum_left_hand()
right_generator = drum_right_hand()
# Now we will actually start the task.
# The task (playing the drums) will be run as long as its
# execution time is shorter than the allowed max duration (t_drumming).
timer_drums.reset()
while timer_drums.now() < t_drumming:
next(left_generator)
left_generator.send(t_beat) # We send t_beat to the generator.
next(right_generator)
right_generator.send(t_beat)
wait_for_seconds(0.01) # Small pause between steps.
# We also print the time in console.
print(str(timer_drums.now()))
return None
def drive(left_motor, right_motor, seconds):
motor_left.start_at_power(check_range(-left_motor))
motor_right.start_at_power(check_range(right_motor))
wait_for_seconds(seconds)
if guessed_color == chosen_color:
# If the user guessed the color, provide positive feedback.
# (S)he won!
app.start_sound('Tada')
hub.light_matrix.show_image('YES')
correct_guess = True # This makes sure we won't go through the loop again.
print("That is correct :) ! The chosen color was " +
guessed_color + ". You win!")
else:
# If the user didn't guess the color, provide negative feedback.
app.start_sound('Lose')
hub.light_matrix.show_image('NO')
wait_for_seconds(4)
hub.light_matrix.off()
# To make things easier for the player, we will remove the wrong
# color of the list, so it isn't displayed again.
colors.pop(color_idx)
n_colors = len(colors)
if color_idx == n_colors:
# If the guessed color was the last one, we will reset the counter,
# to make sure that the next color to be displayed is the first one.
counter = 0
else:
# If the guessed color wasn't the last one, we set the counter
# to the color index, to account for the removed color.
# Get the color value.
color = color_sensor.get_color()
# We need to make sure that Charlie reacts only when he perceives a color.
# To do so, we check what color Charlie perceived. If he didn't perceive
# a color, color_sensor.get_color() returns None.
if not color == None:
print("Reacting to color " + color + "...")
# Turn on the center button of the corresponding color.
hub.status_light.on(color)
# Let's give it a short pause.
wait_for_seconds(1)
# Define reactions to each color.
if color == 'green':
hub.light_matrix.show_image('HAPPY')
motors_movement.move(40, unit='cm', steering=100)
motors_movement.move(40, unit='cm', steering=-100)
elif color == 'yellow':
hub.light_matrix.show_image('SURPRISED')
motors_movement.move(-20, unit='cm', steering=0)
elif color == 'red':
hub.light_matrix.show_image('ANGRY')
motors_movement.move(10, unit='cm', steering=0)
# Displaying an image is quite simple. We just need to define which pixels we will turn on and at what intensity.
# The pixel definition is done in a string in the shape
#
# `00000:00000:00000:00000:00000`
#
# where each number corresponds to a pixel. Each pixel can have a value between `0` (off) to `9` (on at full intensity).
# Each group of numbers (from left to right) correspond to a row of the hub (from top to bottom).
# Notice the groups (i.e., rows) are separated by a colon `:`.
#
# Therefore, if we want to turn on the central pixel of the hub at full intensity, we can do the following:
# %%
print("Displaying example image...")
img_example = hub.Image('00000:00000:00900:00000:00000')
hub.display.show(img_example)
wait_for_seconds(5)
print("DONE!")
# %% [markdown]
# # How to display an animation
# After displaying an image, displaying an animation is quite straightforward, since an animation is
# basically displaying a succession of images.
#
# In this example, we will display a very simple animation: a dot moving from top to bottom (with a tail).
# However, the basic principle can be translated to more complicated animations.
#
# I am sure there are plenty of ways to display an animation, but I found a simple way to do this is the following.
#
# First, we will define the frame of the animation in a list.
# %%
app.start_sound('Triumph')
hub.light_matrix.show_image('MUSIC_QUAVER')
motor_left_arm.run_to_position(15)
motor_right_arm.run_to_position(345)
play_drums(bars=4, tempo=80)
play_drums(bars=4, tempo=130)
# %%
motor_left_arm.run_to_position(15)
motor_right_arm.run_to_position(345)
for ii in range(0, 8):
wait_for_seconds(0.1)
motors_arms.start_at_power(50, steering=-100)
wait_for_seconds(0.1)
motors_arms.start_at_power(-50, steering=-100)
app.play_sound('Tada')
motor_left_arm.run_to_position(0, direction='shortest path')
motor_right_arm.run_to_position(0, direction='shortest path')
# %%
print("DONE!")
# %% [markdown]
# ## Actual `drum_master` part
# %%
flw = Motor("A") # Front Right Wheel
frw = Motor("B")
blw = Motor("E")
brw = Motor("F")
light = 100
while 1:
if rcv.is_connected():
speed, turn, strafe = rcv.controller_state(R_STICK_VER, R_STICK_HOR, L_STICK_HOR)
if rcv.button_pressed(1):
light = 100
if rcv.button_pressed(2):
light = 50
if rcv.button_pressed(3):
light = 0
if rcv.button_pressed(8):
break
ls.light_up_all(light)
flw.start_at_power(clamp_int(-speed - turn - strafe))
frw.start_at_power(clamp_int( speed - turn - strafe))
blw.start_at_power(clamp_int(-speed + turn - strafe))
brw.start_at_power(clamp_int( speed + turn - strafe))
else:
flw.start_at_power(0)
frw.start_at_power(0)
blw.start_at_power(0)
brw.start_at_power(0)
print("Waiting for connection...")
wait_for_seconds(0.3)
def code():
rightArm = Motor('A')
rightArm.start(50)
wait_for_seconds(2)
rightArm.stop()
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
app.start_sound('Robot 3')
motors_arms.move(-90, unit='degrees', steering=0) # Raise arms
motors_wheels.move(5, unit='cm', steering=-100) # Turn back
motors_arms.move(90, unit='degrees', steering=0) # Lower arms
return None
# %% [markdown]
# ## Execution functions
# Now we can call the functions
# %%
print("Should we make Charlie happy?")
charlie_happy()
wait_for_seconds(2)
print("DONE!")
print("Should we make Charlie silly?")
charlie_silly()
wait_for_seconds(2)
print("DONE!")
print("Should we make Charlie scared?")
charlie_scared()
wait_for_seconds(2)
print("DONE!")
# %%
print("-" * 15 + " Execution ended " + "-" * 15 + "\n")
'PACMAN', 'PITCHFORK', 'RABBIT', 'ROLLERSKATE', 'SAD', 'SILLY', 'SKULL',
'SMILE', 'SNAKE', 'SQUARE', 'SQUARE_SMALL', 'STICKFIGURE', 'SURPRISED',
'SWORD', 'TARGET', 'TORTOISE', 'TRIANGLE', 'TRIANGLE_LEFT', 'TSHIRT',
'UMBRELLA', 'XMAS', 'YES'
]
# %%
n_images = len(images)
print("Total number of images: " + str(n_images))
# %% [markdown]
# # Display images
# To choose the images to display, we will generate a random index
# using [`random.randint`](https://docs.python.org/3/library/random.html#random.randint)
# %%
for ii in range(1, 4):
# Generate a random number
idx = random.randint(0, n_images - 1)
# Display the random image
image = images[idx]
print("Displaying image " + str(ii) + " with index " + str(idx) + " ('" +
image + "')...")
hub.light_matrix.show_image(image)
print("DONE!")
# Insert a pause so we can see the image
wait_for_seconds(2.5)
if rcv.button_pressed(7):
break
timer.rate = speed*20
baseline = turn/2
else:
if turn_timer.time > 4000:
dist = port.A.device.get()[0]
print(dist)
if dist:
if dist < 25:
baseline = 30
turn_timer.reset()
else:
baseline = 0
timer.rate = 1000
ds.light_up_all(eyes)
snake_body.update_motor_pwms(timer.time, baseline=baseline)
seg_1_link.write(repr(timer.time))
wait_for_seconds(0.020)
seg_2_link.write(repr(timer.time))
wait_for_seconds(0.020)
if button.center.is_pressed():
break
seg_1_link.disconnect()
seg_2_link.disconnect()
raise SystemExit
# %% [markdown]
# # Move shopper Charlie
#
# So far, we've used `move` to move the wheel motors.
# However, when using `move`, the program will not continue until the specified value is reached.
# This is a problem, since we want to move the wheels *and* the arms at the same time.
# Therefore, we will use `start` and `stop`. This comes at the cost of being unable to
# define a stopping condition based on the motors alone (e.g., distance, time).
# We have to stop them at certain point of the program (as we do here) or based on a
# sensory input.
#
# I wonder if there's a way to perform async execution using the vanilla (Micro)Python.
# I'll look into that in the future.
# %%
print("Moving shopper Charlie...")
motors_wheels.start()
wait_for_seconds(1) # This delays moving the arms, but doesn't affect the already moving wheel motors.
for ii in range(0, 5):
motors_arms.move(0.3, unit='seconds', speed=40)
motors_arms.move(0.2, unit='seconds', speed=-40)
motors_wheels.stop()
print("DONE!")
# %%
print("-"*15 + " Execution ended " + "-"*15 + "\n")
"""
# This code is meant as a boilerplate for your remote control project.
# Connect to it using a steering wheel or app:
# https://play.google.com/store/apps/details?id=com.antonsmindstorms.mindstormsrc&hl=nl&gl=US
from projects.mpy_robot_tools.rc import (RCReceiver, L_STICK_HOR, L_STICK_VER,
R_STICK_HOR, R_STICK_VER, L_TRIGGER,
R_TRIGGER, SETTING1, SETTING2)
from projects.mpy_robot_tools.helpers import clamp_int
from mindstorms import MSHub, ColorSensor, Motor, DistanceSensor
from mindstorms.control import wait_for_seconds
import math
# Create your objects here.
rcv = RCReceiver(name="robot")
ma = Motor("A")
hub = MSHub()
# Write your program here.
while True: # Forever
if rcv.is_connected():
speed, turn = rcv.controller_state(R_STICK_VER, R_STICK_HOR)
if rcv.button_pressed(1):
hub.speaker.beep()
ma.start_at_power(clamp_int(speed))
else:
# Stop all motors when not connected
ma.stop()
print("Waiting for connection...")
wait_for_seconds(0.3) # async wait, better than sleep_ms
def play_drums(bars=4, tempo=100):
"""
Makes Charlie play the drums.
Parameters
----------
bars:
Number of bars (in our case, simple number of cycles).
Default value is 4.
tempo:
Playing tempo (in bpm).
Default value is 100
Returns
-------
None
"""
t_beat = 60 * (1 / tempo)
print("t_beat = " + str(t_beat))
t_bar = t_beat * 4
print("t_bar = " + str(t_bar))
t_drumming = t_bar * bars
print("t_drumming = " + str(t_drumming))
def background_left_arm(t):
"""
Parameters
----------
t:
Time (in seconds) for which the left arm will execute this action.
"""
background_timer_left.reset() # Make sure timer is on 0.
while background_timer_left.now() < t:
yield
def background_right_arm(t):
"""
Parameters
----------
t:
Time (in seconds) for which the right arm will execute this action.
"""
background_timer_right.reset()
while background_timer_right.now() < t:
yield
def drum_left_hand():
while True:
t_action = yield
if not t_action == None:
motor_left_arm.start_at_power(50)
yield from background_left_arm(t_action / 2)
motor_left_arm.start_at_power(-50)
yield from background_left_arm(t_action / 2)
def drum_right_hand():
while True:
t_action = yield
if not t_action == None:
motor_right_arm.start_at_power(-25)
yield from background_right_arm((t_action / 2) * 4)
motor_right_arm.start_at_power(25)
yield from background_right_arm((t_action / 2) * 4)
left_generator = drum_left_hand()
right_generator = drum_right_hand()
timer_drums.reset()
while timer_drums.now() < t_drumming:
next(left_generator)
left_generator.send(t_beat)
next(right_generator)
right_generator.send(t_beat)
wait_for_seconds(0.01)
print(str(timer_drums.now()))
return None
print("Skiing...")
# Original value is 6.
# I set this to 3 due to space constraints.
n_pushes = 3
for ii in range(0, n_pushes):
# Now, we move the arm motors back to position 0 sequentially.
# This doesn't affect the movement, but looks a bit clumsy.
# An alternative to this could be using coroutines to
# make them run "parallel", as in the drum_solo project.
motor_left_arm.run_to_position(0, direction='shortest path')
motor_right_arm.run_to_position(0, direction='shortest path')
motors_arms.move(85, unit='degrees')
motor_right_arm.start()
t_wait = 0.1
wait_for_seconds(t_wait)
motor_right_arm.stop()
motor_left_arm.run_to_position(0, direction='shortest path')
motor_right_arm.run_to_position(0, direction='shortest path')
app.play_sound('Skid')
print("DONE!")
# %%
print("-" * 15 + " Execution ended " + "-" * 15 + "\n")
