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)
