def menu(choices, before_run_function=None, after_run_function=None, skip_to_next_page=True):
"""
Console Menu that accepts choices and corresponding functions to call.
The user must press the same button twice: once to see their choice highlited,
a second time to confirm and run the function. The EV3 LEDs show each state change:
Green = Ready for button, Amber = Ready for second button, Red = Running
Parameters:
- `choices` a dictionary of tuples "button-name": ("mission-name", function-to-call)
NOTE: Don't call functions with parentheses, unless preceded by lambda: to defer the call
- `before_run_function` when not None, call this function before each mission run, passed with mission-name
- `after_run_function` when not None, call this function after each mission run, passed with mission-name
"""
global proc
console = Console()
leds = Leds()
button = Button()
leds.all_off()
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "GREEN")
menu_positions = get_positions(console)
last = None # the last choice--initialize to None
while True:
# display the menu of choices, but show the last choice in inverse
console.reset_console()
for btn, (name, _) in choices.items():
align, col, row = menu_positions[btn]
console.text_at(name, col, row, inverse=(btn == last), alignment=align)
pressed = wait_for_button_press(button)
# get the choice for the button pressed
if pressed in choices:
if last == pressed: # was same button pressed?
console.reset_console()
leds.set_color("LEFT", "RED")
leds.set_color("RIGHT", "RED")
# call the user's subroutine to run the mission, but catch any errors
try:
name, mission_function = choices[pressed]
if before_run_function is not None:
before_run_function(name)
if name in ('NEXT', 'BACK', 'OFF'):
mission_function()
else:
# launch a sub process so it could be canceled with the enter button
# store the subprocess in self to reference in the stop function
proc = Process(target=mission_function)
debug('Starting {}'.format(name))
proc.start()
debug('Just started {} in proc {}'.format(name, proc.pid))
sleep(1)
proc.terminate()
# TODO: Need to figure out when to call self.proc.join
except Exception as e:
print("**** Exception when running")
debug('Exception when running {}'.format(e))
finally:
if after_run_function is not None:
after_run_function(name)
last = None
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "GREEN")
else: # different button pressed
last = pressed
leds.set_color("LEFT", "AMBER")
leds.set_color("RIGHT", "AMBER")
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement in sync with music tempo.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
#self.left_motor = LargeMotor(OUTPUT_D)
#self.hand_motor = MediumMotor(OUTPUT_A)
# Gadget states
self.bpm = 0
self.trigger_bpm = "off"
self.ir = InfraredSensor()
self.ir.on_channel1_top_left = self.onRedTopChannel1
self.ir.on_channel1_top_left = self.onRedTopChannel1
self.ir.on_channel1_bottom_left = self.onRedBottomChannel1
self.ir.on_channel1_top_right = self.onBlueTopChannel1
self.ir.on_channel1_bottom_right = self.onBlueBottomChannel1
threading.Thread(target=self._proximity_thread, daemon=True).start()
def onRedTopChannel1(self, state):
if state:
print("Red Top Channel 1. State: " + str(state))
self._send_event("buttonPress", {'direction': 'up'})
def onBlueTopChannel1(self, state):
if state:
print("Blue Top Channel 1. State: " + str(state))
self._send_event("buttonPress", {'direction': 'down'})
def onRedBottomChannel1(self, state):
if state:
print("Red Bottom Channel 1. State: " + str(state))
self._send_event("buttonPress", {'direction': 'left'})
def onBlueBottomChannel1(self, state):
if state:
print("Blue Bottom Channel 1. State: " + str(state))
self._send_event("buttonPress", {'direction': 'right'})
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param friendly_name: the friendly name of the gadget that has connected to the echo device
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param friendly_name: the friendly name of the gadget that has disconnected from the echo device
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
except KeyError:
print("Missing expected parameters: {}".format(directive))
def on_alexa_gadget_statelistener_stateupdate(self, directive):
"""
Listens for the wakeword state change and react by turning on the LED.
:param directive: contains a payload with the updated state information from Alexa
"""
color_list = ['BLACK', 'AMBER', 'YELLOW', 'GREEN']
for state in directive.payload.states:
if state.name == 'wakeword':
if state.value == 'active':
print("Wake word active - turn on LED")
self.sound.play_song((('A3', 'e'), ('C5', 'e')))
for i in range(0, 4, 1):
self.leds.set_color("LEFT", color_list[i], (i * 0.25))
self.leds.set_color("RIGHT", color_list[i], (i * 0.25))
time.sleep(0.25)
elif state.value == 'cleared':
print("Wake word cleared - turn off LED")
self.sound.play_song((('C5', 'e'), ('A3', 'e')))
for i in range(3, -1, -1):
self.leds.set_color("LEFT", color_list[i], (i * 0.25))
self.leds.set_color("RIGHT", color_list[i], (i * 0.25))
time.sleep(0.25)
def _send_event(self, name, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name, payload)
def _proximity_thread(self):
"""
Monitors the distance between the robot and an obstacle when sentry mode is activated.
If the minimum distance is breached, send a custom event to trigger action on
the Alexa skill.
"""
count = 0
while True:
while True:
# distance = self.ir.proximity
# print("processing IR")
self.ir.process()
# print("Proximity: {}".format(distance))
# count = count + 1 if distance < 10 else 0
# if count > 3:
# print("Proximity breached. Sending event to skill")
# self.leds.set_color("LEFT", "RED", 1)
# self.leds.set_color("RIGHT", "RED", 1)
#
# self._send_event("proximity", {'distance': distance})
time.sleep(0.1)
# time.sleep(1)
def _dance_loop(self, bpm):
"""
Perform motor movement in sync with the beat per minute value from tempo data.
:param bpm: beat per minute from AGT
"""
color_list = ["GREEN", "RED", "AMBER", "YELLOW"]
led_color = random.choice(color_list)
motor_speed = 400
milli_per_beat = min(1000, (round(60000 / bpm)) * 0.65)
print("Adjusted milli_per_beat: {}".format(milli_per_beat))
while self.trigger_bpm == "on":
# Alternate led color and motor direction
led_color = "BLACK" if led_color != "BLACK" else random.choice(
color_list)
motor_speed = -motor_speed
self.leds.set_color("LEFT", led_color)
self.leds.set_color("RIGHT", led_color)
self.right_motor.run_timed(speed_sp=motor_speed, time_sp=150)
self.left_motor.run_timed(speed_sp=-motor_speed, time_sp=150)
time.sleep(milli_per_beat / 1000)
print("Exiting BPM process.")
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Robot state
self.guard_mode = False
# Connect two large motors on output ports A and D
self.drive = MoveTank(OUTPUT_A, OUTPUT_D)
self.mouth = MediumMotor(OUTPUT_B)
self.sound = Sound()
self.leds = Leds()
self.ir = InfraredSensor()
# Start threads
threading.Thread(target=self._guard_thread, daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(
self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]),
int(payload["speed"]))
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking),
file=sys.stderr)
if direction in Direction.FORWARD.value:
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive.on_for_seconds(SpeedPercent(-speed),
SpeedPercent(-speed),
duration,
block=is_blocking)
if direction in (Direction.RIGHT.value + Direction.LEFT.value):
self._turn(direction, speed)
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.STOP.value:
self.drive.off()
self.patrol_mode = False
def _activate(self, command, speed=50):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed),
file=sys.stderr)
if command in Command.GUARD.value:
self.guard_mode = True
self._send_event(EventName.SPEECH,
{'speechOut': "Guard mode activated"})
self.leds.set_color("LEFT", "YELLOW", 1)
self.leds.set_color("RIGHT", "YELLOW", 1)
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)
if direction in Direction.RIGHT.value:
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a guard action.
:param name: the name of the custom event
:param payload: the guard JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _guard_thread(self):
"""
Monitors the distance between the robot and an obstacle when guard mode is activated.
If the minimum distance is breached, send a custom event to trigger action on
the Alexa skill.
"""
count = 0
while True:
while self.guard_mode:
distance = self.ir.proximity
print("Proximity: {}".format(distance), file=sys.stderr)
count = count + 1 if distance < 10 else 0
if count > 3:
print("Proximity breached. Sending event to skill",
file=sys.stderr)
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
for i in range(3):
self.mouth.on_for_rotations(SpeedPercent(70), 0.15)
self.mouth.on_for_rotations(SpeedPercent(70), -0.15)
self._send_event(EventName.GUARD, {'distance': distance})
self.guard_mode = False
time.sleep(0.2)
time.sleep(1)
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.led import Leds
ts = TouchSensor(INPUT_1)
leds = Leds()
print("Press the touch sensor to change the LED color!")
while True:
if ts.is_pressed:
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "GREEN")
else:
leds.set_color("LEFT", "RED")
leds.set_color("RIGHT", "RED")
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Robot state
self.ask_mode = False
# Connect two large motors on output ports B and C
self.sound = Sound()
self.leds = Leds()
self.p1 = TouchSensor(INPUT_1)
self.p2 = TouchSensor(INPUT_2)
self.p3 = TouchSensor(INPUT_3)
self.p4 = TouchSensor(INPUT_4)
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(
self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "ask":
self._ask()
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _ask(self):
print("Entering ASK", file=sys.stderr)
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
time.sleep(2.5)
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
p1 = False
p2 = False
p3 = False
p4 = False
while True:
if self.p1.is_released:
p1 = True
if self.p2.is_released:
p2 = True
if self.p3.is_released:
p3 = True
if self.p4.is_released:
p4 = True
if self.p1.is_pressed and p1:
self._send_event(EventName.ANSWER, {'player': 1})
break
if self.p2.is_pressed and p2:
self._send_event(EventName.ANSWER, {'player': 2})
break
if self.p3.is_pressed and p3:
self._send_event(EventName.ANSWER, {'player': 3})
break
if self.p4.is_pressed and p4:
self._send_event(EventName.ANSWER, {'player': 4})
break
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("Exiting ASK", file=sys.stderr)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
class Bot:
def __init__(self, wheel_radius, wheel_spacing):
self._container_motor = MediumMotor(OUTPUT_C)
self._steering_drive = MoveSteering(OUTPUT_D, OUTPUT_A)
self._touch_sensor_front = TouchSensor(INPUT_1)
self._touch_sensor_top = TouchSensor(INPUT_2)
#self._ultrasonic_sensor = UltrasonicSensor(INPUT_2)
self._color_sensor = ColorSensor(INPUT_3)
#self._color_sensor.calibrate_white()
#self._color_sensor.mode = "RGB-RAW"
self._color_sensor.mode = "COL-REFLECT"
self._leds = Leds()
self._sound = Sound()
self.WHEEL_RADIUS = wheel_radius
self.WHEEL_SPACING = wheel_spacing
def empty_container(self):
self._container_motor.on_for_rotations(-15, 2)
def read_touch_front(self):
return self._touch_sensor_front.is_pressed
def read_touch_top(self):
return self._touch_sensor_top.is_pressed
def read_ultrasonic(self):
return self._ultrasonic_sensor.distance_centimeters
def set_color_sensor_reflect(self):
self._color_sensor.mode = "COL-REFLECT"
def set_color_sensor_rgb(self):
self._color_sensor.mode = "RGB-RAW"
def read_color(self):
if self._color_sensor.mode == "RGB-RAW":
return tuple(map(self._color_sensor.value, [0, 1, 2]))
elif self._color_sensor.mode == "COL-REFLECT":
return self._color_sensor.reflected_light_intensity
def detect_red_tape(self):
tape_found = False
self.set_color_sensor_rgb()
while not tape_found:
self.move_forward(1, 10, blocking=False)
red, green, blue = self.read_color()
print("red {} green {} blue {}".format(red, green, blue))
if red > 120 and green < 80 and blue < 80:
tape_found = True
self.stop()
self.set_color_sensor_reflect()
def _cm_movement_to_rotations(self, distance):
return distance / (pi * 2 * self.WHEEL_RADIUS) * 1.667
def move_forward(self, distance, speed_percent, blocking=True):
rots = self._cm_movement_to_rotations(distance)
self._steering_drive.on_for_rotations(FORWARD,
-speed_percent,
rots,
block=blocking)
def move_backward(self, distance, speed_percent, blocking=True):
self.move_forward(distance, -speed_percent, blocking=blocking)
def stop(self):
self._steering_drive.off()
def rotate_left(self, degrees, speed_percent, blocking=True):
distance = self.WHEEL_SPACING * pi * degrees / 360
rots = self._cm_movement_to_rotations(distance)
self._steering_drive.on_for_rotations(LEFT_ROTATION,
speed_percent,
rots,
block=blocking)
def rotate_right(self, degrees, speed_percent, blocking=True):
distance = self.WHEEL_SPACING * pi * degrees / 360
rots = self._cm_movement_to_rotations(distance)
self._steering_drive.on_for_rotations(RIGHT_ROTATION,
speed_percent,
rots,
block=blocking)
def wav_processor(self):
#self._sound.play('sounds/t2_learning_computer_x.wav')
self._sound.play('sounds/breadcrumbs.wav')
def tts(self, text):
self._sound.speak(text, volume=100)
def set_led_color(self, side, color):
self._leds.set_color(side, color)
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Robot state
self.sentry_mode = False
self.patrol_mode = False
# Connect two large motors on output ports B and C
self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
self.weapon = MediumMotor(OUTPUT_A)
self.sound = Sound()
self.leds = Leds()
self.ir = InfraredSensor()
# Start threads
threading.Thread(target=self._patrol_thread, daemon=True).start()
threading.Thread(target=self._proximity_thread, daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]),
int(payload["speed"]))
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive))
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking))
if direction in Direction.FORWARD.value:
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive.on_for_seconds(SpeedPercent(-speed),
SpeedPercent(-speed),
duration,
block=is_blocking)
if direction in (Direction.RIGHT.value + Direction.LEFT.value):
self._turn(direction, speed)
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.STOP.value:
self.drive.off()
self.patrol_mode = False
def _activate(self, command, speed=50):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed))
if command in Command.MOVE_CIRCLE.value:
self.drive.on_for_seconds(SpeedPercent(int(speed)),
SpeedPercent(5), 12)
if command in Command.MOVE_SQUARE.value:
for i in range(4):
self._move("right", 2, speed, is_blocking=True)
if command in Command.PATROL.value:
# Set patrol mode to resume patrol thread processing
self.patrol_mode = True
if command in Command.SENTRY.value:
self.sentry_mode = True
self._send_event(EventName.SPEECH,
{'speechOut': "Sentry mode activated"})
# Perform Shuffle posture
self.drive.on_for_seconds(SpeedPercent(80), SpeedPercent(-80), 0.2)
time.sleep(0.3)
self.drive.on_for_seconds(SpeedPercent(-40), SpeedPercent(40), 0.2)
self.leds.set_color("LEFT", "YELLOW", 1)
self.leds.set_color("RIGHT", "YELLOW", 1)
if command in Command.FIRE_ONE.value:
print("Fire one")
self.weapon.on_for_rotations(SpeedPercent(100), 3)
self._send_event(EventName.SENTRY, {'fire': 1})
self.sentry_mode = False
print("Sent sentry event - 1 shot, alarm reset")
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
if command in Command.FIRE_ALL.value:
print("Fire all")
self.weapon.on_for_rotations(SpeedPercent(100), 10)
self._send_event(EventName.SENTRY, {'fire': 3})
self.sentry_mode = False
print("sent sentry event - 3 shots, alarm reset")
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)
if direction in Direction.RIGHT.value:
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _proximity_thread(self):
"""
Monitors the distance between the robot and an obstacle when sentry mode is activated.
If the minimum distance is breached, send a custom event to trigger action on
the Alexa skill.
"""
count = 0
while True:
while self.sentry_mode:
distance = self.ir.proximity
print("Proximity: {}".format(distance))
count = count + 1 if distance < 10 else 0
if count > 3:
print("Proximity breached. Sending event to skill")
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
self._send_event(EventName.PROXIMITY,
{'distance': distance})
self.sentry_mode = False
time.sleep(0.2)
time.sleep(1)
def _patrol_thread(self):
"""
Performs random movement when patrol mode is activated.
"""
while True:
while self.patrol_mode:
print("Patrol mode activated randomly picks a path")
direction = random.choice(list(Direction))
duration = random.randint(1, 5)
speed = random.randint(1, 4) * 25
while direction == Direction.STOP:
direction = random.choice(list(Direction))
# direction: all except stop, duration: 1-5s, speed: 25, 50, 75, 100
self._move(direction.value[0], duration, speed)
time.sleep(duration)
time.sleep(1)
if __name__ == '__main__':
# ------------- MQTT -------------- #
client = mqtt.Client()
client.connect("192.168.0.103",1883,60)
client.on_connect = on_connect
client.on_message = on_message
client.username_pw_set("admin", "1234")
# ------------- main program threading -------------- #
threading.Thread(target=main, daemon=True).start()
sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
leds.set_color(leds.LEFT, leds.GREEN)
leds.set_color(leds.RIGHT, leds.GREEN)
if eve3Init["name"] != "RH":
print("Start MQTT client Loop")
client.loop_forever()
else:
threading.Thread(target=client.loop_forever, daemon=True).start()
print("Start Gadget main")
gadget = MindstormsGadget()
gadget.main()
# ------------- Shutdown sequence -------------- #
mainLoop = False
client.loop_stop()
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Robot initial position
self.botposition = RobotPosition.ROBOT_AT_BASE.value
self.sound = Sound()
self.leds = Leds()
self.nav_map = NavegationMap()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
# Retrieve directive
control_type = payload["type"]
# Retrieve robot position
self.botposition = payload["botposition"]
# Process all directives
if control_type == DirectiveName.READ_CONDITIONS.value:
self._read_conditions(payload["condition"])
if control_type == DirectiveName.RETURN_BASE.value:
self._return_base()
if control_type == DirectiveName.VERIFY_COLOR.value:
self._verify_color()
if control_type == DirectiveName.EXPLORING_TOWERS.value:
self._exploring_towers(payload["towerColorA"],
payload["towerColorB"])
except KeyError:
print("Missing expected parameters: {}".format(directive))
def _read_ambiente_temperature(self):
"""
Read temperature at tower using temperature sensor
"""
temperature = TemperatureSensor()
self._send_event(
EventName.TEMPERATURE.value, {
'speechOut':
"Temperature at " + self.botposition +
" is {0:0.2f} ".format(temperature.read_temperature_f()) +
" degrees fahrenheit"
})
def _read_relative_humidity(self):
"""
Read humidity at tower using humidity sensor
"""
humidity = HumiditySensor()
self._send_event(
EventName.HUMIDITY.value, {
'speechOut':
"Relative humidity at " + self.botposition +
" is {0:0.2f} ".format(humidity.read_humidity()) + " percent"
})
def _read_gps_position(self):
"""
Read GPS latitude and longitude coordinates
"""
gpsdata = GPSSensor()
self._send_event(
EventName.GPS.value, {
'speechOut':
"GPS coordinates at " + self.botposition + " are latitude " +
gpsdata.read_latitude().strip() + " and longitude " +
gpsdata.read_longitude()
})
def _read_all_conditions(self):
"""
Read all conditions
"""
temperature = TemperatureSensor()
humidity = HumiditySensor()
gpsdata = GPSSensor()
self._send_event(
EventName.ALLCONDITIONS.value, {
'speechOut':
"At " + self.botposition + " Temperature is {0:0.2f} ".format(
temperature.read_temperature_f()) +
" degrees fahrenheit.,,," +
"Relative humidity is {0:0.2f} ".format(
humidity.read_humidity()) + " percent.,,," +
"GPS coordinates are latitude, " +
gpsdata.read_latitude().strip() + " and longitude, " +
gpsdata.read_longitude()
})
def _read_conditions(self, condition):
"""
Read one or all conditions at each tower
"""
if (condition == ConditionName.AMBIENTE_TEMPERATURE.value):
self._read_ambiente_temperature()
if (condition == ConditionName.RELATIVE_HUMIDITY.value):
self._read_relative_humidity()
if (condition == ConditionName.GPS_POSITION.value):
self._read_gps_position()
if (condition == ConditionName.ALL_CONDITIONS.value):
self._read_all_conditions()
def _return_base(self):
"""
Robot return to base from current tower
"""
if (self.botposition == RobotPosition.ROBOT_AT_RED_TOWER.value):
self.nav_map.return_from_red_tower()
if (self.botposition == RobotPosition.ROBOT_AT_BLUE_TOWER.value):
self.nav_map.return_from_blue_tower()
self._send_event(
EventName.ARRIVE_BASE.value, {
'speechOut': "Explorer arrive at base",
'botPosition': RobotPosition.ROBOT_AT_BASE.value
})
def _verify_color(self):
"""
Verify scanned color at each tower
"""
color_arm = ColorArm()
tower_color = color_arm.scan_color()
self._send_event(
EventName.COLOR.value, {
'speechOut':
"The scanned color at " + self.botposition + " is " +
tower_color
})
def _exploring_towers(self, towerColorA, towerColorB):
"""
If directive have one color tower to go is a manual explorer workflow.
If it have two color towers to go is a autonomous explorer towers
"""
# if not exist towerB color is manual explorer action based in user directives
if (towerColorB == ""):
self.nav_map.set_order_list(
tower_order_list=[towerColorA.capitalize()])
logging.info('Begin manual exploring towers ...')
# Scan all initial towers position
self.nav_map.scan_finding_towers()
logging.info('Going ' + towerColorA + ' tower')
self.nav_map.go_color_tower(towerColorA.capitalize())
logging.info('Explorer arrive at tower...' + towerColorA)
self._send_event(
EventName.ARRIVE_TOWER.value, {
'speechOut':
"Explorer arrive at " + towerColorA + " tower",
'botPosition': towerColorA + " tower"
})
# Autonomous explorer workflow
else:
logging.info('Begin autonomous exploring towers ...')
self.nav_map.set_order_list(tower_order_list=[
towerColorA.capitalize(),
towerColorB.capitalize()
])
# Scan all initial towers position
self.nav_map.scan_finding_towers()
for tower_color in self.nav_map.tower_order_list:
# Robot go each tower
logging.info('Going ' + tower_color + ' tower')
self._send_event(
EventName.GOING_TOWER.value, {
'speechOut':
"Explorer is going to the " + tower_color + " tower"
})
self.nav_map.go_color_tower(tower_color)
self._send_event(
EventName.ARRIVE_TOWER_AUTO.value, {
'speechOut':
"Explorer arrive " + tower_color + " tower",
'botPosition': tower_color + " tower"
})
sleep(10)
# At arrive tower read all conditions
self._send_event(
EventName.ALLCONDITIONS.value, {
'speechOut':
"Reading all conditions at " + tower_color + " tower"
})
self._read_conditions(ConditionName.ALL_CONDITIONS.value)
sleep(20)
# And return base
self._send_event(
EventName.RETURN_BASE.value,
{'speechOut': "Explorer is returning to the base"})
self._return_base()
def _send_event(self, name, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name, payload)
from ev3dev2.led import Leds
import time
lights = Leds()
while True:
lights.set_color("LEFT", "RED")
lights.set_color("RIGHT", "RED")
time.sleep(1)
lights.set_color("LEFT", "AMBER")
lights.set_color("RIGHT", "AMBER")
time.sleep(1)
lights.set_color("LEFT", "GREEN")
lights.set_color("RIGHT", "GREEN")
time.sleep(1)
from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor.lego import GyroSensor, TouchSensor
from ev3dev2.display import Display
from ev3dev2.led import Leds
# Connect two large motors to port B and port C
tank_pair = MoveTank(OUTPUT_B, OUTPUT_C)
gy = GyroSensor()
ts = TouchSensor()
d = Display()
led = Leds()
start_angle = gy.value()
angle = start_angle
led.set_color('BOTH', (0, 0, 1))
while abs(angle - start_angle) < 180:
angle = gy.value()
# you can see the results on the screen
print(angle, abs(angle - start_angle))
tank_pair.on(-10, 10)
led.set_color('BOTH', 'GREEN')
while not ts.is_pressed:
angle = gy.value()
# you can see the results on the screen
print(angle, abs(angle - start_angle), ts.is_pressed)
d.text_grid("Touch Sensor pressed: " + str(ts.is_pressed))
tank_pair.on(-20, -20)
led.set_color('BOTH', 'RED')
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
Two types of commands are supported, directional movement and preset.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Gadget state
self.in_progress = False
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.drive = MoveTank(OUTPUT_B, OUTPUT_A)
self.stick = MediumMotor(OUTPUT_C)
self.cs = ColorSensor(INPUT_4)
self.cs.mode = 'COL-REFLECT'
self.floor_light = self.cs.value()
print("floor light intensity = {}".format(self.floor_light))
self.speed = 20
self.kickAngle = 170
paho.Client.connected_flag = False
self.client = paho.Client()
self.client.loop_start()
self.client.on_connect = self.mqtt_connected
self.client.connect('broker.hivemq.com', 1883)
while not self.client.connected_flag:
time.sleep(1)
self.client.subscribe('/hockeybot/game/over')
self.client.on_message = self.mqtt_on_message
# Start threads
threading.Thread(target=self.check_for_obstacles_thread,
daemon=True).start()
def mqtt_connected(self, client, data, flags, rc):
print("connected to mqtt broker")
client.connected_flag = True
def mqtt_on_message(self, client, data, message):
print("message received from MQTT")
print(str(message.payload.decode("utf-8")))
if message.topic == '/hockeybot/game/over':
try:
print("game over message received")
jsondata = json.loads(str(message.payload.decode("utf-8")))
self._send_event(EventName.GAME_OVER,
{'score': jsondata['score']})
print("event posted to alexa")
except Exception as e:
print(e)
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
#self.sound.speak("Connected to Echo device")
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_hockeybot_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
control_type = payload["type"]
self.in_progress = True
if control_type == "start":
#Notify scoreboard to start count down
data = {'duration': 300}
self.client.publish('/hockeybot/game/start', json.dumps(data))
time.sleep(3)
#self._move(payload["direction"], int(payload["duration"]), int(payload["speed"]))
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]),
int(payload["speed"]))
if control_type == "command":
# Expected params: [command]
self._kick(payload["direction"])
except KeyError:
print("Missing expected parameters: {}".format(directive))
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
self.forwardMode = False
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking))
if direction in Direction.FORWARD.value:
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive.on_for_seconds(SpeedPercent(-speed),
SpeedPercent(-speed),
duration,
block=is_blocking)
if direction in (Direction.RIGHT.value + Direction.LEFT.value):
self._turn(direction, speed)
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.STOP.value:
self.drive.off()
self.patrol_mode = False
def _kick(self, direction):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Kick command: ", str(self.kickAngle))
self.drive.off()
if direction in Direction.LEFT.value:
self.stick.on_for_rotations(SpeedPercent(50), -1)
if direction in Direction.RIGHT.value:
self.stick.on_for_rotations(SpeedPercent(50), 1)
#self.kickAngle = self.kickAngle*-1
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)
if direction in Direction.RIGHT.value:
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)
def check_for_obstacles_thread(self):
while True:
read = self.cs.value()
#print(str(read))
#ground boundary
if read < 10:
self.drive.off()
self._turn('left', self.speed)
#self.drive.on_for_seconds(SpeedPercent(self.speed), SpeedPercent(self.speed), 5)
time.sleep(0.2)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Hockeybot.Gadget', name.value, payload)
rc.on_channel1_bottom_left = roll(lmotor, 'LEFT', -1)
rc.on_channel1_top_right = roll(rmotor, 'RIGHT', 1)
rc.on_channel1_bottom_right = roll(rmotor, 'RIGHT', -1)
print("Robot Starting")
# Enter event processing loop
while not button.any():
rc.process()
# Backup when bumped an obstacle
if ts.is_pressed:
spkr.speak('Oops, excuse me!')
for motor in (lmotor, rmotor):
motor.stop(stop_action='brake')
# Turn red lights on
for light in ('LEFT', 'RIGHT'):
leds.set_color(light, 'RED')
# Run both motors backwards for 0.5 seconds
for motor in (lmotor, rmotor):
motor.run_timed(speed_sp=-600, time_sp=500)
# Wait 0.5 seconds while motors are rolling
sleep(0.5)
leds.all_off()
sleep(0.01)
from ev3dev2.sensor import INPUT_1, INPUT_2
from ev3dev2.sensor.lego import TouchSensor, GyroSensor
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
import time
# INITIALIZE
m1 = LargeMotor(OUTPUT_A)
m2 = LargeMotor(OUTPUT_B)
m1.degrees
g = GyroSensor(INPUT_1)
g_off = 0
l = Leds()
l.set_color('LEFT', 'AMBER')
s = Sound()
s.set_volume(100)
g_off = 0
f = open("data.txt", "w")
CM_PER_ROT = 21.375
right_start = True
# s.speak("Hello")
def accelerate(forwards, r):
if forwards:
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
Two types of commands are supported, directional movement and preset.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Gadget state
self.patrol_mode = False
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.turnMotor = LargeMotor(OUTPUT_B)
self.cardsMotor = LargeMotor(OUTPUT_D)
# Start threads
# threading.Thread(target=self._patrol_thread, daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "deal-initial":
# Expected params: [game, playerCount]
self._dealCardOnGameStart(payload["game"], int(payload["playerCount"]))
if control_type == "deal-turn":
# Expected params: [game, playerCount, playerTurn]
self._dealCardOnGameTurn(payload["game"], int(payload["playerCount"]), int(payload["playerTurn"]), payload["gameCommand"])
except KeyError:
print("Missing expected parameters: {}".format(directive), file=sys.stderr)
def _dealCardOnGameStart(self, game, playerCount: int):
"""
Handles dealing the cards based on game type and the number of players
"""
print("Dealing cards: ({}, {})".format(game, playerCount), file=sys.stderr)
if game in GameType.BLACKJACK.value:
for i in range(playerCount):
print("Dealing card to: ({})".format(i+1), file=sys.stderr)
self._moveToPlayer(i+1, playerCount, False)
self._dispenseCard()
self._moveToBase(playerCount, playerCount)
for i in range(playerCount):
print("Dealing card to: ({})".format(i+1), file=sys.stderr)
self._moveToPlayer(i+1, playerCount, False)
self._dispenseCard()
self._moveToBase(playerCount, playerCount)
if game in GameType.UNO.value:
for k in range(4):
for i in range(playerCount):
print("Dealing card to: ({})".format(i+1), file=sys.stderr)
self._moveToPlayer(i+1, playerCount, False)
self._dispenseCard()
self._moveToBase(playerCount, playerCount)
if game in GameType.SHUFFLE_CARDS.value:
for k in range(5):
self._moveToPlayer(1, 2, True)
cardsToDispense = random.randint(1, 3)
for k in range(cardsToDispense):
self._dispenseCard()
self._moveToBase(1, 2)
self._moveToPlayer(2, 2, True)
cardsToDispense = random.randint(1, 3)
for k in range(cardsToDispense):
self._dispenseCard()
self._moveToBase(2, 2)
def _dealCardOnGameTurn(self, game, playerCount: int, playerTurn: int, gameCommand):
"""
Handles dealing the card based on game type and the turn in the game
"""
print("Dealing cards: ({}, {}, {})".format(game, playerCount, playerTurn), file=sys.stderr)
if game in GameType.BLACKJACK.value:
if gameCommand in GameCommand.BLACKJACK_HIT.value:
print("Dealing card to: ({})".format(playerTurn), file=sys.stderr)
self._moveToPlayer(playerTurn, playerCount, True)
self._dispenseCard()
self._moveToBase(playerTurn, playerCount)
if game in GameType.UNO.value:
if gameCommand in GameCommand.UNO_DEAL_ONCE.value:
print("Dealing card to: ({})".format(playerTurn), file=sys.stderr)
cardsToDispense = random.randint(0, 3)
if cardsToDispense > 0:
for k in range(cardsToDispense):
self._dispenseCard()
if gameCommand in GameCommand.UNO_DEAL_TWICE.value:
print("Dealing card to: ({})".format(playerTurn), file=sys.stderr)
cardsToDispense = random.randint(0, 3)
if cardsToDispense > 0:
for k in range(cardsToDispense):
self._dispenseCard()
cardsToDispense = random.randint(0, 3)
if cardsToDispense > 0:
for k in range(cardsToDispense):
self._dispenseCard()
def _moveToPlayer(self, playerIndex: int, playerCount: int, oneTimeMove: bool):
angle = -180 / playerCount
turnAngle = angle
print("Moving to player: ({}) out of ({})".format(playerIndex, playerCount), file=sys.stderr)
if oneTimeMove == True:
if playerIndex > 1:
turnAngle = angle * (playerIndex - 1)
if playerIndex == 1:
turnAngle = 0
print("Angle: ({})".format(turnAngle), file=sys.stderr)
self.turnMotor.on_for_degrees(SpeedPercent(15), turnAngle, True)
time.sleep(.25)
def _moveToBase(self, playerIndex: int, playerCount: int):
print("Reset to base", file=sys.stderr)
time.sleep(.50)
angle = 180 / playerCount
turnAngle = angle
if playerIndex > 1:
turnAngle = angle * (playerIndex - 1)
if playerIndex == 1:
turnAngle = 0
print("Angle: ({})".format(turnAngle), file=sys.stderr)
self.turnMotor.on_for_degrees(SpeedPercent(15), turnAngle, True)
def _dispenseCard(self):
self.cardsMotor.on_for_rotations(SpeedPercent(-50), 0.5, True)
time.sleep(.25)
self.cardsMotor.on_for_rotations(SpeedPercent(-50), 0.2, True)
time.sleep(.25)
self.cardsMotor.on_for_rotations(SpeedPercent(50), 0.7, True)
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Robot state
self.patrol_mode = False
self.enemy_not_detected = True
print("+++++ self.patrol_mode = {} y self.enemy_not_detected = {}".
format(self.patrol_mode, self.enemy_not_detected))
self.positionX = 0
self.positionY = 0
self.direction = ['forward', 'right', 'backward', 'left']
self.offset = [0, 1, 0, -1]
self.index = 0
self.pointing = self.direction[self.index]
# Connect two large motors on output ports B and C
self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
self.weapon = MediumMotor(OUTPUT_A)
self.sound = Sound()
self.leds = Leds()
self.ir = InfraredSensor()
# Start threads
threading.Thread(target=self._patrol_thread, daemon=True).start()
threading.Thread(target=self._proximity_thread, daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
control_type = payload["type"]
if control_type == "moveSteps":
# Expected params: [direction, steps, speed]
self.enemy_not_detected = True
self._moveSteps(payload["direction"], int(payload["steps"]),
int(payload["speed"]))
if control_type == "patrol":
self.patrol_mode = True
self.enemy_not_detected = True
if control_type == "smash":
print("SMASHING")
self.weapon.on_for_degrees(SpeedPercent(100), 300)
self.weapon.on_for_degrees(SpeedPercent(-50), 300)
time.sleep(5)
self._send_event(EventName.SMASH, {'Smashed': 1})
#self.smash_mode = False
print("1 smashed")
self.enemy_not_detected = True
print(
"+++++ self.patrol_mode = {} y self.enemy_not_detected = {}"
.format(self.patrol_mode, self.enemy_not_detected))
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
except KeyError:
print("Missing expected parameters: {}".format(directive))
def _moveSteps(self, direction, steps: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param steps: the duration in steps that translate into number of rotations
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed, steps,
is_blocking))
if direction in Direction.FORWARD.value:
self.drive.on_for_rotations(SpeedPercent(speed),
SpeedPercent(speed),
steps,
block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive.on_for_rotations(SpeedPercent(-speed),
SpeedPercent(-speed),
steps,
block=is_blocking)
if direction in Direction.LEFT.value:
self._turn(direction, speed)
self.drive.on_for_rotations(SpeedPercent(speed),
SpeedPercent(speed),
steps,
block=is_blocking)
offset = -1
self.index = self.new_index(self.index, offset)
self.pointing = self.direction[self.index]
if direction in Direction.RIGHT.value:
self._turn(direction, speed)
self.drive.on_for_rotations(SpeedPercent(speed),
SpeedPercent(speed),
steps,
block=is_blocking)
offset = 1
self.index = self.new_index(self.index, offset)
self.pointing = self.direction[self.index]
if direction in Direction.STOP.value:
self.drive.off()
self.patrol_mode = False
self.enemy_not_detected = False
print("STOP!! patrol mode = {} y enemy not detected = {}".format(
self.patrol_mode, self.enemy_not_detected))
if direction in Direction.PAUSE.value:
self.drive.off()
print("Pause to kill the enemy")
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
self.drive.on_for_degrees(SpeedPercent(speed),
SpeedPercent(-speed),
490,
block=True)
if direction in Direction.RIGHT.value:
self.drive.on_for_degrees(SpeedPercent(-speed),
SpeedPercent(speed),
490,
block=True)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _proximity_thread(self):
"""
Monitors the distance between the robot and an obstacle when sentry mode is activated.
If the minimum distance is breached, send a custom event to trigger action on
the Alexa skill.
"""
count = 0
while True:
print(
"---------------- Analizo si debo detectar enemigos - PROXIMITY"
)
print("----- DEBO?? ::::> self.enemy_not_detected = {}".format(
self.enemy_not_detected))
while self.enemy_not_detected:
distance = self.ir.proximity
#print("Proximity: {}".format(distance))
count = count + 1 if distance < 30 else 0
if count > 2:
print("Proximity breached. Sending event to skill")
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
self.enemy_not_detected = False
print(
"+++++ self.patrol_mode = {} y self.enemy_not_detected = {}"
.format(self.patrol_mode, self.enemy_not_detected))
self._moveSteps('pause', 0, 0)
count = 0
self._send_event(EventName.PROXIMITY,
{'distance': distance})
time.sleep(5)
time.sleep(0.2)
time.sleep(1)
def _patrol_thread(self):
"""
Performs random movement when patrol mode is activated.
It will be moving with steps from 1 to 3, within a square of 6 steps size,
and it can move from 25% or 75% speed
"""
while True:
print("---------------- Analizo si debo hacer patruya - PATROL")
print("----- DEBO?? ::::> self.patrol_mode = {}".format(
self.patrol_mode))
while self.patrol_mode:
print(
"---------------- Analizo si debo detectar enemigos - PROXIMITY"
)
print("----- DEBO?? ::::> self.enemy_not_detected = {}".format(
self.enemy_not_detected))
while self.enemy_not_detected:
print("Patrol mode activated randomly picks a path")
direction = random.choice(list(DirectionPatrol))
steps = random.randint(2, 3)
speed = random.randint(2, 3) * 20
notInZone = self.calculate_in_zone(direction.value[0],
steps)
while notInZone:
direction = random.choice(list(DirectionPatrol))
steps = random.randint(2, 3)
speed = random.randint(2, 3) * 20
notInZone = self.calculate_in_zone(
direction.value[0], steps)
# direction: all except stop and backwards, duration: 2-3steps, speed: 40, 60
self._moveSteps(direction.value[0], steps, speed)
time.sleep(2 + (steps * (100 - speed) / 75))
print(
"###Enemigo detectado, no entro en patruya hasta que self.enemy_not_detected = {}"
.format(self.enemy_not_detected))
time.sleep(1)
print(
"###Salgo de la patruya, no entro en patruya hasta que self.patrol_mode = {}"
.format(self.patrol_mode))
def new_index(self, index, offset):
index = index + offset
if (index < 0):
index = 3
if (index > 3):
index = 0
return index
def calculate_in_zone(self, direction, steps):
'''
There is a square of 6x6 steps and the robot cannot go away the boundaries
So this will let the robot know if the next move action will lead him to go out of the square
positionX and positionY can be between -3 and +3 value, starting always in 0,0
'''
realDirectionIndex = self.new_index(
self.index, self.offset[self.direction.index(direction)])
realDirection = self.direction[realDirectionIndex]
if (realDirection == 'forward'):
targetX = self.positionX
targetY = self.positionY + steps
elif (realDirection == 'backward'):
targetX = self.positionX
targetY = self.positionY - steps
elif (realDirection == 'left'):
targetX = self.positionX - steps
targetY = self.positionY
elif (realDirection == 'right'):
targetX = self.positionX + steps
targetY = self.positionY
if ((abs(targetX) > 3) or (abs(targetY) > 3)):
return True
else:
self.positionX = targetX
self.positionY = targetY
print("Target position Move command: ({}, {})".format(
targetX, targetY))
print("Direction: {}".format(direction))
print("Pointing: {}".format(self.pointing))
print("realDirection: {}".format(realDirection))
return False
class MindstormsGadget(AlexaGadget):
"""
An Mindstorms gadget that will react to the Alexa wake word.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
self.leds = Leds()
self.sound = Sound()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_alexa_gadget_statelistener_stateupdate(self, directive):
"""
Listens for the wakeword state change and react by turning on the LED.
:param directive: contains a payload with the updated state information from Alexa
"""
color_list = ['BLACK', 'AMBER', 'YELLOW', 'GREEN']
for state in directive.payload.states:
if state.name == 'wakeword':
if state.value == 'active':
print("Wake word active - turn on LED")
self.sound.play_song((('A3', 'e'), ('C5', 'e')))
for i in range(0, 4, 1):
self.leds.set_color("LEFT", color_list[i], (i * 0.25))
self.leds.set_color("RIGHT", color_list[i], (i * 0.25))
time.sleep(0.25)
elif state.value == 'cleared':
print("Wake word cleared - turn off LED")
self.sound.play_song((('C5', 'e'), ('A3', 'e')))
for i in range(3, -1, -1):
self.leds.set_color("LEFT", color_list[i], (i * 0.25))
self.leds.set_color("RIGHT", color_list[i], (i * 0.25))
time.sleep(0.25)
from ev3dev2.led import Leds
import time
leds = Leds()
leds.all_off()
time.sleep(0.5)
leds.set_color('RIGHT', 'GREEN')
for i in range(6):
amb = i / 6
leds.set_color('LEFT', (amb, 1.0))
time.sleep(0.5)
time.sleep(1.0)
leds.all_off()
class Robot(AlexaGadget):
def __init__(self):
super().__init__()
# initialize all of the motors
print('Initializing devices')
self.leds = Leds()
self.motor_hand = LargeMotor(address='outA')
self.motor_claw = MediumMotor(address='outC')
# called when the EV3 brick connects to an Alexa device
def on_connected(self, device_addr):
self.leds.set_color('LEFT', 'GREEN')
self.leds.set_color('RIGHT', 'GREEN')
print("{} connected to Echo device".format(self.friendly_name))
# called when the EV3 brick disconnects from an Alexa device
def on_disconnected(self, device_addr):
self.leds.set_color('LEFT', 'BLACK')
self.leds.set_color('RIGHT', 'BLACK')
print("{} disconnected from Echo device".format(self.friendly_name))
# the function called to receive gadget control directives from the Alexa Skill through the connected Alexa device
def on_custom_mindstorms_gadget_control(self, directive):
# decode the directive payload into a JSON object
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
# determine which command to be executed
control_type = payload['type']
if control_type == 'Go':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_claw.on(20)
time.sleep(2)
self.motors_claw.off(brake=True)
elif control_type == 'Back':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_claw.on(-20)
time.sleep(2)
self.motors_claw.off(brake=True)
elif control_type == 'Left':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_hand.on(20)
time.sleep(2)
self.motors_hand.off(brake=True)
elif control_type == 'Right':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_hand.on(-20)
time.sleep(2)
self.motors_hand.off(brake=True)
elif control_type == 'Turn':
# get the source and destination states for this command
src_state = State[payload['state']]
while(danceflag == 1):
self.motors_hand.on(20)
time.sleep(2)
self.motors_hand.on(-20)
self.motors_hand.off(brake=True)
# move the robot straight back at a certain speed for a certain number of rotations
def move_back(self, speed=0.2, distance=1.6):
self.motor_left.on_for_rotations(round(speed * 100),
distance,
block=False)
self.motor_right.on_for_rotations(round(speed * 100), distance)
# turn off all motors and lights
def poweroff(self):
self.motor_hand.off(brake=False)
self.motor_claw.off(brake=False)
def changeLEDS(self):
leds = Leds()
leds.set_color('LEFT','RED')
STEER_DRIVER = MoveSteering(left_motor_port=OUTPUT_B,
right_motor_port=OUTPUT_C,
motor_class=LargeMotor)
TOUCH_SENSOR = TouchSensor(address=INPUT_1)
LEDS = Leds()
SPEAKER = Sound()
SCREEN = Display()
while True:
SCREEN.image_filename(filename='/home/robot/image/ZZZ.bmp',
clear_screen=True)
SCREEN.update()
LEDS.set_color(group=Leds.LEFT, color=Leds.ORANGE, pct=1)
LEDS.set_color(group=Leds.RIGHT, color=Leds.ORANGE, pct=1)
# TODO: parallel process/thread
while not TOUCH_SENSOR.is_pressed:
SPEAKER.play_file(wav_file='/home/robot/sound/Snoring.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
SCREEN.image_filename(filename='/home/robot/image/Winking.bmp',
clear_screen=True)
SCREEN.update()
SPEAKER.play_file(wav_file='/home/robot/sound/Activate.wav',
volume=100,
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
Four types of commands are supported: sit, stay, come, speak, heel.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Gadget state
self.heel_mode = False
self.patrol_mode = False
self.sitting = False
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
# Connect infrared and touch sensors.
self.ir = InfraredSensor()
self.ts = TouchSensor()
# Init display
self.screen = Display()
# Connect medium motor on output port A:
self.medium_motor = MediumMotor(OUTPUT_A)
# Connect two large motors on output ports B and C:
self.left_motor = LargeMotor(OUTPUT_B)
self.right_motor = LargeMotor(OUTPUT_C)
# Gadget states
self.bpm = 0
self.trigger_bpm = "off"
# Start threads
threading.Thread(target=self._patrol_thread, daemon=True).start()
threading.Thread(target=self._heel_thread, daemon=True).start()
threading.Thread(target=self._touchsensor_thread, daemon=True).start()
# ------------------------------------------------
# Callbacks
# ------------------------------------------------
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
print("{} Connected to Echo device".format(self.friendly_name))
# Draw blinking eyes of the puppy
threading.Thread(target=self._draweyes, daemon=True).start()
# Turn lights on:
for light in ('LEFT', 'RIGHT'):
self.leds.set_color(light, 'GREEN')
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
# Turn lights off:
for light in ('LEFT', 'RIGHT'):
self.leds.set_color(light, 'BLACK')
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
control_type = payload["type"]
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive))
# On Amazon music play
def on_alexa_gadget_musicdata_tempo(self, directive):
"""
Provides the music tempo of the song currently playing on the Echo device.
:param directive: the music data directive containing the beat per minute value
"""
tempo_data = directive.payload.tempoData
for tempo in tempo_data:
print("tempo value: {}".format(tempo.value))
if tempo.value > 0:
# dance pose
#self.drive.on_for_seconds(SpeedPercent(5), SpeedPercent(25), 1)
self.right_motor.run_timed(speed_sp=750, time_sp=2500)
self.left_motor.run_timed(speed_sp=-750, time_sp=2500)
# shake ev3 head
threading.Thread(target=self._sitdown).start()
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
time.sleep(3)
# starts the dance loop
self.trigger_bpm = "on"
threading.Thread(target=self._dance_loop,
args=(tempo.value, )).start()
elif tempo.value == 0:
# stops the dance loop
self.trigger_bpm = "off"
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
def _dance_loop(self, bpm):
"""
Perform motor movement in sync with the beat per minute value from tempo data.
:param bpm: beat per minute from AGT
"""
color_list = ["GREEN", "RED", "AMBER", "YELLOW"]
led_color = random.choice(color_list)
motor_speed = 400
milli_per_beat = min(1000, (round(60000 / bpm)) * 0.65)
print("Adjusted milli_per_beat: {}".format(milli_per_beat))
while self.trigger_bpm == "on":
# Alternate led color and motor direction
led_color = "BLACK" if led_color != "BLACK" else random.choice(
color_list)
motor_speed = -motor_speed
self.leds.set_color("LEFT", led_color)
self.leds.set_color("RIGHT", led_color)
self.right_motor.run_timed(speed_sp=motor_speed, time_sp=150)
self.left_motor.run_timed(speed_sp=-motor_speed, time_sp=150)
time.sleep(milli_per_beat / 1000)
self.left_motor.run_timed(speed_sp=-motor_speed, time_sp=150)
self.right_motor.run_timed(speed_sp=motor_speed, time_sp=150)
time.sleep(milli_per_beat / 1000)
self.right_motor.run_timed(speed_sp=350, time_sp=300)
self.left_motor.run_timed(speed_sp=-350, time_sp=300)
time.sleep(milli_per_beat / 1000)
self.right_motor.run_timed(speed_sp=motor_speed, time_sp=150)
self.left_motor.run_timed(speed_sp=-motor_speed, time_sp=150)
time.sleep(milli_per_beat / 1000)
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking))
if direction in Direction.FORWARD.value:
#self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking)
self.right_motor.run_timed(speed_sp=-750, time_sp=2500)
self.left_motor.run_timed(speed_sp=-750, time_sp=2500)
if direction in Direction.BACKWARD.value:
#self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(-speed), duration, block=is_blocking)
self.right_motor.run_timed(speed_sp=750, time_sp=2500)
self.left_motor.run_timed(speed_sp=750, time_sp=2500)
if direction in (Direction.RIGHT.value + Direction.LEFT.value):
self._turn(direction, speed)
#self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking)
self.right_motor.run_timed(speed_sp=750, time_sp=2500)
self.left_motor.run_timed(speed_sp=-750, time_sp=2500)
if direction in Direction.STOP.value:
#self.drive.off()
self.right_motor.stop
self.left_motor.stop
self.heel_mode = False
self.patrol_mode = False
def _activate(self, command):
"""
Handles preset commands.
:param command: the preset command
"""
print("Activate command: ({}".format(command))
if command in Command.COME.value:
#call _come method
self.right_motor.run_timed(speed_sp=750, time_sp=2500)
self.left_motor.run_timed(speed_sp=50, time_sp=100)
if command in Command.HEEL.value:
#call _hell method
self.heel_mode = True
if command in Command.SIT.value:
# call _sit method
self.heel_mode = False
self._sitdown()
if command in Command.STAY.value:
# call _stay method
self.heel_mode = False
self._standup()
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
#self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)
self.right_motor.run_timed(speed_sp=0, time_sp=100)
self.left_motor.run_timed(speed_sp=750, time_sp=100)
if direction in Direction.RIGHT.value:
#self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)
self.right_motor.run_timed(speed_sp=750, time_sp=100)
self.left_motor.run_timed(speed_sp=0, time_sp=100)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _heel_thread(self):
"""
Monitors the distance between the puppy and an obstacle when heel command called.
If the maximum distance is breached, decrease the distance by following an obstancle
"""
while True:
while self.heel_mode:
distance = self.ir.proximity
print("Proximity distance: {}".format(distance))
# keep distance and make step back from the object
if distance < 45:
threading.Thread(target=self.__movebackwards).start()
self._send_event(EventName.BARK, {'distance': distance})
# follow the object
if distance > 60:
threading.Thread(target=self.__moveforwards).start()
# otherwise stay still
else:
threading.Thread(target=self.__stay).start()
time.sleep(0.2)
time.sleep(1)
def _touchsensor_thread(self):
print("Touch sensor activated")
while True:
if self.ts.is_pressed:
self.leds.set_color("LEFT", "RED")
self.leds.set_color("RIGHT", "RED")
if (self.sitting):
threading.Thread(target=self._standup).start()
self.sitting = False
else:
threading.Thread(target=self._sitdown).start()
self.sitting = True
else:
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
def _sitdown(self):
self.medium_motor.on_for_rotations(SpeedPercent(20), 0.5)
def _standup(self):
# run the wheels backwards to help the puppy to stand up.
threading.Thread(target=self.__back).start()
self.medium_motor.on_for_rotations(SpeedPercent(50), -0.5)
def __back(self):
self.right_motor.run_timed(speed_sp=-350, time_sp=1000)
self.left_motor.run_timed(speed_sp=-350, time_sp=1000)
def __movebackwards(self):
self.right_motor.run_timed(speed_sp=-650, time_sp=1000)
self.left_motor.run_timed(speed_sp=-650, time_sp=1000)
def __moveforwards(self):
self.right_motor.run_timed(speed_sp=650, time_sp=1000)
self.left_motor.run_timed(speed_sp=650, time_sp=1000)
def __stay(self):
self.right_motor.run_timed(speed_sp=0, time_sp=1000)
self.left_motor.run_timed(speed_sp=0, time_sp=1000)
def _draweyes(self):
close = True
while True:
self.screen.clear()
if close:
#self.screen.draw.ellipse(( 5, 30, 75, 50),fill='white')
#self.screen.draw.ellipse((103, 30, 173, 50), fill='white')
self.screen.draw.rectangle((5, 60, 75, 50), fill='black')
self.screen.draw.rectangle((103, 60, 173, 50), fill='black')
#self.screen.draw.rectangle(( 5, 30, 75, 50), fill='black')
#self.screen.draw.rectangle((103, 30, 173, 50), fill='black')
time.sleep(10)
else:
#self.screen.draw.ellipse(( 5, 30, 75, 100))
#self.screen.draw.ellipse((103, 30, 173, 100))
#self.screen.draw.ellipse(( 35, 30, 105, 30),fill='black')
#self.screen.draw.ellipse((133, 30, 203, 30), fill='black')
self.screen.draw.rectangle((5, 10, 75, 100), fill='black')
self.screen.draw.rectangle((103, 10, 173, 100), fill='black')
close = not close # toggle between True and False
# Update screen display
# Applies pending changes to the screen.
# Nothing will be drawn on the screen screen
# until this function is called.
self.screen.update()
time.sleep(1)
def _patrol_thread(self):
"""
Performs random movement when patrol mode is activated.
"""
while True:
while self.patrol_mode:
print("Patrol mode activated randomly picks a path")
direction = random.choice(list(Direction))
duration = random.randint(1, 5)
speed = random.randint(1, 4) * 25
while direction == Direction.STOP:
direction = random.choice(list(Direction))
# direction: all except stop, duration: 1-5s, speed: 25, 50, 75, 100
self._move(direction.value[0], duration, speed)
time.sleep(duration)
time.sleep(1)
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# the default I2C address of the sensor
self.I2C_ADDRESS = 0x21
# setup the buses
self.airqualitybus = SMBus(3)
self.temperaturebus = SMBus(4)
#setup the moisbus and relaybus
self.airqualitybus.write_byte_data(self.I2C_ADDRESS, 0x42, 0x01)
self.temperaturebus.write_byte_data(self.I2C_ADDRESS, 0x42, 0x01)
#setup the lastmois so we can track it well
self.lastairquality = 0
self.lasttemperature = 0
self.count = 0
self.speed = 0
# Robot state
self.auto_mode = False
self.filterwarning = False
self.sound = Sound()
self.leds = Leds()
self.color = ColorSensor()
self.touch = TouchSensor()
#Motor
self.fan = MediumMotor(OUTPUT_A)
#screen
self.screen = ev3.Screen()
# Start threads
threading.Thread(target=self._autofan_thread, daemon=True).start()
threading.Thread(target=self._manual_button_thread,
daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(
self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "airquality":
self._airquality_handler()
elif control_type == "temperature":
self._temperature_handler(payload["unit"])
elif control_type == "speed":
self._speed_handler(payload["speed"])
elif control_type == "auto":
self._auto_handler(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _speed_handler(self, speed):
print(speed)
self.speed = speed
if speed != 0:
self.fan.on(SpeedPercent(-speed))
else:
self.fan.on(SpeedPercent(speed))
self.fan.off()
def _airquality_handler(self):
print(self.lastairquality)
if self.lastairquality > 700:
if self.auto_mode == True:
self._send_event(
EventName.AIRQUALITY, {
'request':
0,
'speech':
"We are currently experiencing high pollution, air filter is set to high automatically"
})
else:
self._send_event(
EventName.AIRQUALITY, {
'request':
1,
'speech':
"We are currently experiencing high pollution, would you like to set the air purifier to high mode?"
})
elif self.lastairquality > 300:
if self.auto_mode == True:
self._send_event(
EventName.AIRQUALITY, {
'request':
0,
'speech':
"We are currently experiencing low pollution, air filter is set to high automatically"
})
else:
self._send_event(
EventName.AIRQUALITY, {
'request':
1,
'speech':
"We are currently experiencing low pollution, would you like to set the air purifier to high mode?"
})
else:
self._send_event(EventName.AIRQUALITY, {
'request': 0,
'speech': "The air quality is fresh and clean."
})
def _temperature_handler(self, unit):
print(self.lasttemperature)
if unit.lower() == 'fahrenheit':
fahrenheit = self.lasttemperature * 9 / 5 + 32
print(fahrenheit)
self._send_event(
EventName.TEMPERATURE, {
'speech':
"The temperature in the room is " + str(int(fahrenheit)) +
" degrees fahrenheit"
})
else:
self._send_event(
EventName.TEMPERATURE, {
'speech':
"The temperature in the room is " +
str(int(self.lasttemperature)) + " degrees celcius"
})
def _auto_handler(self, onoff):
if onoff == "on":
self.auto_mode = True
else:
self.auto_mode = False
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
print(name.value)
print(payload)
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _autofan_thread(self):
"""
Performs random movement when patrol mode is activated.
"""
print('fan thread started')
while True:
self.screen.clear()
#Air Quality
self.screen.draw.rectangle((0, 0, 177, 40), fill='black')
part1 = self.airqualitybus.read_byte_data(self.I2C_ADDRESS, 0x44)
part2 = self.airqualitybus.read_byte_data(self.I2C_ADDRESS, 0x45)
aq = (part1 << 2) + part2
print("Air Quality:" + str(aq))
self.screen.draw.text((36, 13),
"Air Quality:" + str(aq),
fill='white')
#temperature
part3 = self.temperaturebus.read_byte_data(self.I2C_ADDRESS, 0x44)
part4 = self.temperaturebus.read_byte_data(self.I2C_ADDRESS, 0x45)
temp = (part3 << 2) + part4
R = 1023.0 / temp - 1.0
R = R0 * R
temperature = 1.0 / (math.log(R / R0) / B + 1 / 298.15) - 273.15
print("Temperature:" + str(temperature))
self.screen.draw.text(
(36, 60), "Temperature:" + str(int(temperature)) + " C")
print("color:" + self.color.color_name)
self.screen.draw.text((36, 90), "Filter:" + self.color.color_name)
self.screen.update()
if self.auto_mode and aq >= 300 and self.lastairquality < 300:
#got dirty
self._speed_handler(100)
self._send_event(EventName.FANSPEED, {
'speech':
'Pollution detected, auto setting fan speed to high'
})
elif self.auto_mode and aq <= 300 and self.lastairquality > 300:
#got clean
self._speed_handler(25)
self._send_event(EventName.FANSPEED, {
'speech':
'Air is clean now, auto setting fan speed to low'
})
if aq >= 700:
gadget.leds.set_color("LEFT", "RED")
elif aq >= 300:
gadget.leds.set_color("LEFT", "YELLOW")
else:
gadget.leds.set_color("LEFT", "GREEN")
self.lastairquality = aq
self.lasttemperature = temperature
#Filter maintenance
if self.color.color == ColorSensor.COLOR_WHITE or self.color.color == ColorSensor.COLOR_YELLOW:
self.filterwarning = False
gadget.leds.set_color("RIGHT", "GREEN")
else:
#reset
gadget.leds.set_color("RIGHT", "YELLOW")
if self.filterwarning == False:
self._send_event(
EventName.FILTER, {
'speech':
'The filter seems dirty, please check it and see if it needs to be replaced'
})
self.filterwarning = True
time.sleep(1)
def _manual_button_thread(self):
pressed = False
while True:
if self.touch.is_pressed == True:
pressed = True
if pressed == True and self.touch.is_released == True:
#confirming pressed the button once
pressed = False
if self.speed == 0:
#it's currently off
self._speed_handler(25)
self._send_event(
EventName.FANSPEED,
{'speech': 'Air purifier is setted to low manually'})
elif self.speed < 60:
self._speed_handler(60)
self._send_event(EventName.FANSPEED, {
'speech':
'Air purifier is setted to medium manually'
})
elif self.speed < 100:
self._speed_handler(100)
self._send_event(
EventName.FANSPEED,
{'speech': 'Air purifier is setted to high manually'})
else:
self._speed_handler(0)
self._send_event(
EventName.FANSPEED,
{'speech': 'Air purifier is turned off manually'})
time.sleep(0.1)
#!/usr/bin/env python3
from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor
from ev3dev2.led import Leds
from time import sleep
# Connect ultrasonic and touch sensors to any sensor port
us = UltrasonicSensor()
ts = TouchSensor()
leds = Leds()
leds.all_off() # stop the LEDs flashing (as well as turn them off)
while not ts.is_pressed:
if us.distance_centimeters < 30: # to detect objects closer than 40cm
# In the above line you can also use inches: us.distance_inches < 16
leds.set_color('LEFT', 'RED')
leds.set_color('RIGHT', 'RED')
else:
leds.set_color('LEFT', 'GREEN')
leds.set_color('RIGHT', 'GREEN')
sleep(0.01) # Give the CPU a rest
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
Two types of commands are supported, directional movement and preset.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.motor = Motor(OUTPUT_A)
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
print("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
print("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]),
int(payload["speed"]))
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
except KeyError:
print("Missing expected parameters: {}".format(directive))
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in rotations
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking))
position = duration * 360
if direction in Direction.FORWARD.value:
self.motor.run_to_rel_pos(position_sp=(position),
speed_sp=(speed),
stop_action="hold")
if direction in Direction.BACKWARD.value:
self.motor.run_to_rel_pos(position_sp=(-position),
speed_sp=(speed),
stop_action="hold")
if direction in Direction.STOP.value:
self.motor.stop(stop_action="hold")
def _activate(self, command, speed=500):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed))
if command in Command.RAISE_BLIND.value:
self.motor.run_to_rel_pos(position_sp=5000,
speed_sp=500,
stop_action="hold")
print('running raise blinds')
if command in Command.LOWER_BLIND.value:
self.motor.run_to_rel_pos(position_sp=-5000,
speed_sp=500,
stop_action="hold")
print('running lower blinds')
Gyro2.mode = 'GYRO-ANG'
# units = us.units
units1 = Gyro1.units
units2 = Gyro2.units
Kp = 0.001
while not TouchSensor1.is_pressed:
# distance = us.value() / 10 # convert mm to cm
Gyro1_output = Gyro1.value()
Gyro2_output = Gyro2.value()
# print(str(distance) + " " + units)
print("Gyro 1: %.6f and Gyro 2: %.6f" % (Gyro1_output, Gyro2_output))
if Gyro1_output < 60: # This is an inconveniently large distance
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "GREEN")
else:
leds.set_color("LEFT", "RED")
leds.set_color("RIGHT", "RED")
Motor3.duty_cycle_sp = (Gyro1_output / 180) * 100
Motor3.run_direct()
sleep(0.01)
Motor3.stop()
Sound.beep('finished')
leds.set_color("LEFT", "GREEN") # set left led green before exiting
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Connect two large motors on output ports B and C
self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
self.grip = MediumMotor(OUTPUT_A)
self.sound = Sound()
self.leds = Leds()
self.ir = InfraredSensor()
self.touch = TouchSensor()
self.gyro = GyroSensor()
self.isComing = False
self.isTaking = False
self.isBringing = False
self.isTurning = False
# Start threads
threading.Thread(target=self._proximity_thread, daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(
self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]),
int(payload["speed"]))
elif control_type == "come":
self._come()
elif control_type == "take":
self._take()
elif control_type == "bring":
self._bring()
except KeyError:
print("Missing expected parameters: {}".format(directive),
file=sys.stderr)
def _come(self, duration=10, speed=50):
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
self.isComing = True
self._move(Direction.FORWARD.value[0], duration, speed)
def _take(self, duration=20, speed=50):
self.grip.on_for_rotations(SpeedPercent(100), 1)
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
self.gyro.reset()
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.isTurning = True
self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40)
self.isTaking = True
self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), duration)
def _bring(self, duration=20):
self.isTurning = True
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40)
self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), 1.5)
self.grip.on_for_rotations(SpeedPercent(100), 1)
self.isTurning = True
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40)
self.isBringing = True
self.now = time.time()
self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), duration)
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed,
duration, is_blocking),
file=sys.stderr)
if direction in Direction.FORWARD.value:
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive.on_for_seconds(SpeedPercent(-speed),
SpeedPercent(-speed),
duration,
block=is_blocking)
if direction in (Direction.RIGHT.value + Direction.LEFT.value):
self._turn(direction, speed)
self.drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(speed),
duration,
block=is_blocking)
if direction in Direction.STOP.value:
self.drive.off()
self.patrol_mode = False
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)
if direction in Direction.RIGHT.value:
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _proximity_thread(self):
"""
Monitors the distance between the robot and an obstacle when sentry mode is activated.
If the minimum distance is breached, send a custom event to trigger action on
the Alexa skill.
"""
while True:
distance = self.ir.proximity
angle = self.gyro.angle
#print("Proximity: {}".format(distance), file=sys.stderr)
if self.isTurning == True:
print("angle: {}".format(angle), file=sys.stderr)
self.leds.set_color("LEFT", "YELLOW", 1)
self.leds.set_color("RIGHT", "YELLOW", 1)
if abs(angle) >= 179 or abs(angle) <= -181:
self.isTurning = False
self._move(
Direction.STOP.value[0],
0,
0,
)
self.gyro.reset()
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
if distance <= 55:
"""
When the bot is coming, it will stop and open up it's arm
"""
if self.isComing == True:
self.isComing = False
print("Proximity breached, stopping")
self._move(
Direction.STOP.value[0],
0,
0,
)
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
while not self.touch.is_pressed:
self.grip.on_for_degrees(SpeedPercent(10), -90)
print("Lowered the grip")
elif self.isTaking == True:
self.isTaking = False
print("Proximity breached, stopping")
self._move(
Direction.STOP.value[0],
0,
0,
)
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
while not self.touch.is_pressed:
self.grip.on_for_degrees(SpeedPercent(10), -90)
print("Dropping Item")
self.drive.on_for_seconds(SpeedPercent(-50),
SpeedPercent(-50), 1)
self.gyro.reset()
self.isTurning = True
self.gyro.mode = 'GYRO-RATE'
self.gyro.mode = 'GYRO-ANG'
self.drive.on_for_seconds(SpeedPercent(100),
SpeedPercent(-100), 1.3)
self.drive.on_for_seconds(SpeedPercent(4),
SpeedPercent(-4),
40,
block=False)
self.leds.set_color("LEFT", "GREEN", 1)
self.leds.set_color("RIGHT", "GREEN", 1)
elif self.isBringing == True:
self.isBringing = False
print("Proximity breached, stopping")
self._move(Direction.STOP.value[0], 0, 0)
self.later = time.time()
self.leds.set_color("LEFT", "RED", 1)
self.leds.set_color("RIGHT", "RED", 1)
while not self.touch.is_pressed:
self.grip.on_for_degrees(SpeedPercent(10), -90)
print("Dropping Item")
difference = int(self.later - self.now)
self.drive.on_for_seconds(SpeedPercent(-50),
SpeedPercent(-50),
difference - 0.5)
time.sleep(0.2)
def doGyro():
"""Test code for using the gyro sensor"""
leds = Leds()
gyroSensor = GyroSensor()
# Reset gyro
sleep(2)
calibrateGyro(gyroSensor)
while True:
# Fetch the current angle from the gyro
angle = gyroSensor.angle
if angle > 1:
# veering to the right
leds.set_color('LEFT', 'AMBER')
leds.set_color('RIGHT', 'RED')
message = "Veering RIGHT {} deg"
elif angle < -1:
# veering to the left
leds.set_color('LEFT', 'RED')
leds.set_color('RIGHT', 'AMBER')
message = "Veering LEFT {} deg"
elif angle == 0:
# straight
leds.set_color('LEFT', 'GREEN')
leds.set_color('RIGHT', 'GREEN')
message = "STRAIGHT"
else:
# angle is a little off
leds.set_color('LEFT', 'AMBER')
leds.set_color('RIGHT', 'AMBER')
message = "Off by {} deg"
# Display results
myLogger.log(message.format(angle))
# wait for a bit before sampling next gyro reading
sleep(0.5)
#!/usr/bin/env python3
from ev3dev2.button import Button
from ev3dev2.led import Leds
btn = Button()
leds = Leds()
leds.all_off()
while True:
if btn.up:
break
elif btn.right:
leds.set_color('LEFT', 'AMBER')
elif btn.left:
leds.set_color('LEFT', 'GREEN')
leds.all_off()
sleep(1000)
print("Turning Off")
from ev3dev2.led import Leds
import time
leds=Leds()
while True:
leds.set_color("RIGHT", "GREEN")
time.sleep(1)
leds.set_color("RIGHT", "AMBER")
time.sleep(1)
