class Robot(): def __init__(self, baseSpeed=500, dt=50): self.leftMotor = LargeMotor(OUTPUT_C) self.rightMotor = LargeMotor(OUTPUT_A) self.steeringDrive = MoveSteering(OUTPUT_C, OUTPUT_A) self.craneMotor = MediumMotor(OUTPUT_B) self.baseSpeed = baseSpeed self.dt = dt def steer(self, controlSignal): # ograniczenie sterowania leftMotorU = max(-1000, min(self.baseSpeed + controlSignal, 1000)) rightMotorU = max(-1000, min(self.baseSpeed - controlSignal, 1000)) # sterowanie silnikami self.leftMotor.run_timed(time_sp=self.dt, speed_sp=leftMotorU, stop_action="coast") self.rightMotor.run_timed(time_sp=self.dt, speed_sp=rightMotorU, stop_action="coast") sleep(self.dt / 1000) def rotateLeft(self): self.steeringDrive.on_for_rotations(-72, 40, 1) sleep(1) def rotateRight(self): self.steeringDrive.on_for_rotations(72, 40, 1) sleep(1) def liftCrane(self): self.craneMotor.on_for_degrees(20, 45) sleep(1) def dipCrane(self): self.craneMotor.on_for_degrees(-20, 45) sleep(1)
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_B) self.right_motor = LargeMotor(OUTPUT_C) # Gadget states self.bpm = 0 self.trigger_bpm = "off" 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_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.right_motor.run_timed(speed_sp=750, time_sp=2500) self.left_motor.run_timed(speed_sp=-750, time_sp=2500) 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) print("Exiting BPM process.")
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_B) self.right_motor = LargeMotor(OUTPUT_C) self.hand_motor = MediumMotor(OUTPUT_A) # Gadget states self.bpm = 0 self.trigger_bpm = "off" self.drive = MoveTank(OUTPUT_B, OUTPUT_C) self.weapon = MediumMotor(OUTPUT_A) 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")) print("Control payload: {}".format(payload)) control_type = payload["type"] if control_type == "spin": print("Move command found") self.hand_motor.run_timed(speed_sp=1050, time_sp=2500) if control_type == "movef": print("Move Forward command found") self.right_motor.run_timed(speed_sp=1050, time_sp=1000) self.left_motor.run_timed(speed_sp=1050, time_sp=1000) if control_type == "moveb": print("Move backward command found") self.right_motor.run_timed(speed_sp=-1050, time_sp=1000) self.left_motor.run_timed(speed_sp=-1050, time_sp=1000) if control_type == "mover": print("Move right command found") self.right_motor.run_timed(speed_sp=-1050, time_sp=1000) self.left_motor.run_timed(speed_sp=1050, time_sp=1000) if control_type == "movel": print("Move left command found") self.right_motor.run_timed(speed_sp=1050, time_sp=1000) self.left_motor.run_timed(speed_sp=-1050, time_sp=1000) if control_type == "command": self.hand_motor.run_timed(speed_sp=1050, time_sp=2500) except KeyError: print("Missing expected parameters: {}".format(directive)) 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.right_motor.run_timed(speed_sp=750, time_sp=2500) self.left_motor.run_timed(speed_sp=-750, time_sp=2500) self.hand_motor.run_timed(speed_sp=750, time_sp=2500) 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 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 _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.")
from ev3dev2.motor import LargeMotor, OUTPUT_B, SpeedPercent, MoveTank, OUTPUT_A, OUTPUT_C, MediumMotor, OUTPUT_D l_speed = 500 r_speed = 50 print(l_speed, r_speed) left_motor = LargeMotor(OUTPUT_A) right_motor = LargeMotor(OUTPUT_D) left_motor.stop() right_motor.stop() left_motor.run_timed(speed_sp=l_speed, time_sp=3000) right_motor.run_timed(speed_sp=r_speed, time_sp=3000)
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.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() self.dance = False self.sound = Sound() self.drive = MoveTank(OUTPUT_B, OUTPUT_C) self.sound.speak('Hello, my name is Beipas!') # 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" self.eyes = True # Start threads threading.Thread(target=self._dance_thread, daemon=True).start() 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() threading.Thread(target=self._eyes_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": speed = random.randint(3, 4) * 25 # Expected params: [direction, duration, speed] self._move(payload["direction"], int(payload["duration"]), speed) if control_type == "command": # Expected params: [command] self._activate(payload["command"]) except KeyError: print("Missing expected parameters: {}".format(directive), file=sys.stderr) def _dance_thread(self): """ Perform motor movement in sync with the beat per minute value from tempo data. :param bpm: beat per minute from AGT """ bpm = 100 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 True: while self.dance == True: print("Dancing") # 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=70, 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): self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(speed), duration, block=is_blocking) if direction in (Direction.LEFT.value): 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 self.dance = 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=750, time_sp=2500) 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.trigger_bpm == "on" self._sitdown() if command in Command.SENTRY.value: # call _stay method self.trigger_bpm == "on" self.heel_mode = False self._standup() if command in Command.STAY.value: # call _stay method self.heel_mode = False self._standup() if command in Command.ANGRY.value: # call _stay method self._angrybark() if command in Command.CUTE.value: # call _stay method self._cutebark() if command in Command.COFFIN.value: # call _stay method self.dance = True self.trigger_bpm = "on" self._coffinbark() self.dance = False if command in Command.DANCE.value: # call _stay method self.trigger_bpm = "on" self.dance = True print(self.dance) 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 _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) 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 < 35: threading.Thread(target=self.__movebackwards).start() # self._send_event(EventName.BARK, {'distance': distance}) # follow the object if distance > 50: 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 _eyes_thread(self): print("Drawing Eyes") while True: while self.eyes: self._draweyes() 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=-750, time_sp=2500) self.left_motor.run_timed(speed_sp=-750, time_sp=2500) def __moveforwards(self): self.right_motor.run_timed(speed_sp=750, time_sp=2500) self.left_motor.run_timed(speed_sp=750, time_sp=2500) 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 _angrybark(self): self.sound.play_file('angry_bark.wav') def _cutebark(self): self.sound.play_file('cute_bark.wav') def _coffinbark(self): self.sound.play_file('coffin_dance.wav') def _draweyes(self): close = True while True: self.screen.clear() # if close: # self.screen.draw.line(50,65,90, 100, width=5) # self.screen.draw.line(50,105,90, 105, width=5) # self.screen.draw.line(120,100,160,65, width=5) # self.screen.draw.line(120,105,160,105, width=5) # time.sleep(10) # else: # self.screen.draw.rectangle(50,45,90, 125, radius=10, fill_color='white') # self.screen.draw.rectangle(120,45,160,125, radius=10, fill_color='white') # self.screen.draw.rectangle(65,65,80, 105, radius=7, fill_color='black') # self.screen.draw.rectangle(130,65,145,105, radius=7, fill_color='black') ## alt # self.screen.draw.line(50,105,90, 105, width=5).rotate(135) # self.screen.draw.line(50,105,90, 105, width=5) # self.screen.draw.line(50,105,90, 105, width=5).rotate(45) # self.screen.draw.line(50,105,90, 105, width=5) 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 run(self): # sensors cs = ColorSensor() cs.mode = 'COL-REFLECT' # measure light intensity # motors lm = LargeMotor('outC') rm = LargeMotor('outB') speed = 360 / 2 # deg/sec, [-1000, 1000] dt = 500 # milliseconds stop_action = "coast" # PID tuning Kp = 1 # proportional gain Ki = 0.002 # integral gain Kd = 0.01 # derivative gain integral = 0 previous_error = 0 # initial measurment target_value = 35 # Start the main loop while not self.shut_down: # Calculate steering using PID algorithm error = target_value - cs.value() if previous_error > 0 and error < 0: integral = 0 integral += (error * dt) derivative = (error - previous_error) / dt # u zero: on target, drive forward # u positive: too bright, turn right # u negative: too dark, turn left u = (Kp * error) + (Ki * integral) + (Kd * derivative) # limit u to safe values: [-1000, 1000] deg/sec if speed + abs(u) > 1000: if u >= 0: u = 1000 - speed else: u = speed - 1000 # run motors if u >= 0: lm.run_timed(time_sp=dt, speed_sp=speed + abs(u), stop_action=stop_action) rm.run_timed(time_sp=dt, speed_sp=0, stop_action=stop_action) sleep(dt / 2000) else: lm.run_timed(time_sp=dt, speed_sp=0, stop_action=stop_action) rm.run_timed(time_sp=dt, speed_sp=speed + abs(u), stop_action=stop_action) sleep(dt / 2000) previous_error = error
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)