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)