def show_text(string, font_name='courB24', font_width=15, font_height=24):
lcd = Display()
lcd.clear()
strings = wrap(string, width=int(180/font_width))
for i in range(len(strings)):
x_val = 89-font_width/2*len(strings[i])
y_val = 63-(font_height+1)*(len(strings)/2-i)
lcd.text_pixels(strings[i], False, x_val, y_val, font=font_name)
lcd.update()
def show_text(string,
font_name='courB24',
font_width=15,
font_height=24): # A function to show text on the robot's screen
'''Function to show a text on EV3 screen.
This code is copied from ev3python.com'''
lcd = Display() # Defining screen
lcd.clear() # Clearing the screen so there isnt already text
strings = wrap(string, width=int(180 / font_width)) #
for i in range(len(strings)):
x_val = 89 - font_width / 2 * len(strings[i])
y_val = 63 - (font_height + 1) * (len(strings) / 2 - i)
lcd.text_pixels(strings[i], False, x_val, y_val, font=font_name)
lcd.update()
sleep(1)
exit()
'''
console.reset_console()
console.text_at('Hello World!', column=1, row=5, reset_console=True, inverse=True)
sleep(2)
'''
# 在屏幕上显示字体
if True:
#显示有效的Display字体
print(fonts.available(), file=sys.stderr)
sleep(2)
exit()
disp.clear()
disp.draw.text((10, 10), 'Hello World!', font=fonts.load('courB18'))
disp.update()
# 改变面板显示灯的颜色
leds.set_color("LEFT", "GREEN")
leds.set_color("RIGHT", "RED")
# 英文转语音
sleep(1)
sounds.speak(text='Welcome to the E V 3 dev project!')
sleep(1)
# 控制电机转动
m = LargeMotor(OUTPUT_A)
#m.on_for_rotations(SpeedPercent(75), 1)
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.sensor.lego import Sensor
from ev3dev2.sensor import INPUT_1, INPUT_2
from ev3dev2.display import Display
lcd = Display()
# Connect Pixy camera
pixy = Sensor()
# Connect TouchSensor
ts = TouchSensor(address=INPUT_1)
# Set mode
pixy.mode = 'ALL'
while not ts.value():
lcd.clear()
if pixy.value(0) != 0: # Object with SIG1 detected
x = pixy.value(1)
y = pixy.value(2)
w = pixy.value(3)
h = pixy.value(4)
dx = int(w / 2) # Half of the width of the rectangle
dy = int(h / 2) # Half of the height of the rectangle
xb = x + int(w / 2) # X-coordinate of bottom-right corner
yb = y - int(h / 2) # Y-coordinate of the bottom-right corner
lcd.draw.rectangle((dx, dy, xb, yb), fill='black')
lcd.update()
for i in range(0, 4):
print(pixy.value(i), file=stderr)
def cmToRotations(cm):
return cm/wheelCircumference_cm
# inches to millimeters:
def inToMillimeters(inches):
return inches * 25.4
# centimeters to millimeters:
def cmToMillimeters(cm): #hhm it works... questionable -- no syntax errors!
return cm * 10 # Yay, no syntax errors!
def drive_cm(power, cm):
rt = cmToRotations(cm)
tank_drive.on_for_rotations(SpeedPercent(power), SpeedPercent(power), int(rt) )
def drive_cm_new(power, cm):
rt = cmToRotations(cm)
tank_drive.on_for_rotations(SpeedPercent(power), SpeedPercent(power), rt)
#---------------------------------------------------------------------------------------------------------------------------------------------
while True:
if (btn.down):
break
pass
btn.wait_for_released('enter')
finish = time.time()
Display_.clear()
Display_.draw.text((0,0), str(finish - start), font=fonts.load('helvR24'))
Display_.update()
time.sleep(5)
drive_cm_new(50, 50)
time.sleep(5)
drive_cm_new(-50, 50)
start = time.time()
class Ui(threading.Thread):
M_FONT = 'helvB12'
def __init__(self, config):
threading.Thread.__init__(self)
self.roboId = config['id']
self.threadID = 1
self.name = 'ui-thread'
self.isRunning = True
self.messageText = '-'
self.statusText = '-'
self.powerSupplyText = '-'
self.lcd = Display()
self.btn = Button()
self.sound = Sound()
self.leds = Leds()
self.theFont = fonts.load(Ui.M_FONT)
self.lcd.clear()
self.drawText()
self.lcd.update()
def drawText(self):
self.lcd.draw.text((0, 0), 'RoboRinth', font=self.theFont)
self.lcd.draw.text((0, 14), 'ID: ' + self.roboId, font=self.theFont)
self.lcd.draw.text((0, 28),
'Status: ' + self.statusText,
font=self.theFont)
self.lcd.draw.text((0, 42),
'Msg: ' + self.messageText,
font=self.theFont)
# self.lcd.draw.text((0,56), 'Pwr: ' + self.powerSupplyText, font=self.theFont)
def run(self):
while self.isRunning:
self.lcd.clear()
self.drawText()
self.lcd.update()
self.btn.process()
sleep(1)
# sleep(0.5)
self.lcd.clear()
self.lcd.draw.rectangle((0, 0, 178, 128), fill='white')
self.lcd.update()
def registerBackspaceHandler(self, backspaceHandler):
self.btn.on_backspace = backspaceHandler
def stop(self):
self.isRunning = False
self.join()
def setMessageText(self, text):
self.messageText = text
def setStatusText(self, text):
self.statusText = text
def setPowerSupplyText(self, text):
self.powerSupplyText = text
def playStartSound(self):
self.sound.tone([(800, 200, 0), (1200, 400, 100)])
def setStatusLed(self, color):
if color == 'green':
self.leds.set_color('RIGHT', 'GREEN')
elif color == 'orange':
self.leds.set_color('RIGHT', 'ORANGE')
else:
print('unsupported color: ' + str(color))
###################################
### load probot_empty.ttt scene ###
###################################
from ev3dev2.led import Leds
from ev3dev2.display import Display
from time import sleep
leds = Leds()
display = Display()
leds.all_off()
display.clear()
sleep(2)
display.text_grid("POLICE!!!", y=3, x=9)
for i in range(10):
if i % 2:
leds.set_color('RIGHT', (0, 0, 1))
leds.set_color('LEFT', (1, 0, 0))
else:
leds.set_color('RIGHT', (1, 0, 0))
leds.set_color('LEFT', (0, 0, 1))
sleep(0.3)
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 master_function():
cl = ColorSensor()
ts = TouchSensor()
ir = InfraredSensor()
sound = Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
medium_motor = MediumMotor()
pixy = Sensor(address=INPUT_2)
# assert pixy.connected, "Error while connecting Pixy camera to port 1"
# pixy.mode = 'SIG1'
# lcd = Sensor.Screen()
lcd = Display()
def top_left_channel_1_action(state):
move()
def bottom_left_channel_1_action(state):
move()
def top_right_channel_1_action(state):
move()
def bottom_right_channel_1_action(state):
move()
def move():
buttons = ir.buttons_pressed() # a list
if len(buttons) == 1:
medium_motor.off()
if buttons == ['top_left']:
steer_pair.on(steering=0, speed=40)
elif buttons == ['bottom_left']:
steer_pair.on(steering=0, speed=-40)
elif buttons == ['top_right']:
steer_pair.on(steering=100, speed=30)
elif buttons == ['bottom_right']:
steer_pair.on(steering=-100, speed=30)
elif len(buttons) == 2:
steer_pair.off()
if buttons == ['top_left', 'top_right']:
medium_motor.on(speed_pct=10)
elif buttons == ['bottom_left', 'bottom_right']:
medium_motor.on(speed_pct=-10)
else: # len(buttons)==0
medium_motor.off()
steer_pair.off()
# Associate the event handlers with the functions defined above
ir.on_channel1_top_left = top_left_channel_1_action
ir.on_channel1_bottom_left = bottom_left_channel_1_action
ir.on_channel1_top_right = top_right_channel_1_action
ir.on_channel1_bottom_right = bottom_right_channel_1_action
opts = '-a 200 -s 150 -p 70 -v'
speech_pause = 0
while not ts.is_pressed:
# rgb is a tuple containing three integers
ir.process()
red = cl.rgb[0]
green = cl.rgb[1]
blue = cl.rgb[2]
intensity = cl.reflected_light_intensity
print('{4} Red: {0}\tGreen: {1}\tBlue: {2}\tIntensity: {3}'.format(
str(red), str(green), str(blue), str(intensity), speech_pause),
file=stderr)
lcd.clear()
print(pixy.mode, file=stderr)
if pixy.value(0) != 0: # Object with SIG1 detected
x = pixy.value(1)
y = pixy.value(2)
w = pixy.value(3)
h = pixy.value(4)
dx = int(w / 2) # Half of the width of the rectangle
dy = int(h / 2) # Half of the height of the rectangle
xb = x + int(w / 2) # X-coordinate of bottom-right corner
yb = y - int(h / 2) # Y-coordinate of the bottom-right corner
lcd.draw.rectangle((xa, ya, xb, yb), fill='black')
lcd.update()
speech_pause += 1
if speech_pause == 200:
# try replacing with ir.distance(), ir.heading()
# or ir.heading_and_distance()
distance = ir.heading_and_distance()
if distance == None:
# distance() returns None if no beacon detected
str_en = 'Beacon off?'
else:
str_en = 'Beacon heading {0} and distance {1}'.format(
distance[0], distance[1])
sound.speak(str_en, espeak_opts=opts + 'en+f5')
print(str_en, file=stderr)
speech_pause = 0
str_en = 'Terminating program'
sound.speak(str_en, espeak_opts=opts + 'en+f5')
log.info('prepare for the initial position')
# prepare for the initial position
# detect the upper position of the lifter
lm_lifter.on( -100, brake=True)
while( not ts.is_pressed ):
sleep(0.05)
# approaching the initial position with high speed
lm_lifter.on_for_rotations(90, 7)
# nearly approached the initial position, approaching with lower speed
lm_lifter.on_for_degrees(20, 240)
# clear the lcd display
lcd.clear()
# show the steps
lcd.text_pixels( str(steps), True, 80, 50, font='courB18')
lcd.update()
log.info('wait user to supply the steps')
# wait user to supply the steps to go
while( True ):
if(not btn.buttons_pressed):
sleep(0.01)
continue
if btn.check_buttons(buttons=['up']):
steps += 1
elif(btn.check_buttons(buttons=['down']) ):
class utils:
def int2SpeakColor(self, colornr):
if colornr == 0:
#print("NoColor")
self.mSpeak('This is not a color')
elif colornr == 1:
#print("Black")
self.mSpeak('Black')
elif colornr == 2:
#print("Blue")
self.mSpeak('Blue')
elif colornr == 3:
#print("Green")
self.mSpeak('Green')
elif colornr == 4:
#print("Yellow")
self.mSpeak('Yellow')
elif colornr == 5:
#print("Red")
self.mSpeak('Red')
elif colornr == 6:
#print("White")
self.mSpeak('White')
elif colornr == 7:
#print("Brown")
self.mSpeak('Brown')
else:
#print("No valid value")
self.mSpeak('Value not valid!')
def checkColor(self):
newColor = self.colorSensor.color
if newColor != self.lastColor:
self.lastColor = newColor
self.int2SpeakColor(newColor)
return self.lastColor
# Moderated speech
def mSpeak(self, string):
if self.playDebugSound:
#print(string)
self.s.speak(string,
volume=50,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
def mBeep(self):
if self.playDebugSound:
self.s.beep(play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
def checkTouchL(self):
return self.touchL.is_pressed
def checkTouchR(self):
return self.touchR.is_pressed
def checkDistance(self):
return self.usSensor.value()
def __init__(self):
self.playDebugSound = False
self.s = Sound()
self.display = Display()
self.display.clear()
self.colorSensor = ColorSensor(INPUT_2)
self.lastColor = 0
self.usSensor = UltrasonicSensor(INPUT_3)
self.usSensor.mode = 'US-DIST-CM'
self.touchL = TouchSensor(INPUT_1)
self.touchR = TouchSensor(INPUT_4)
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)
while indx > 0-cnt:
tmpX=Sx[indx]
tmpY=Sy[indx]
if( tmpX==x and tmpY==y):
return True
indx -= 3
return False
# subscribe the button events
btn.on_left = left
btn.on_right = right
btn.on_up = up
btn.on_down = down
createBuger()
display.clear()
# draw hanburger
display.draw.ellipse((hx,hy,hx+step,hy+step))
while True:
# draw score on the screen
display.draw.text((0,2), 'Score {0}'.format(length), fill='black')
# process the button event
btn.process()
# start the game while any direction is given
if dx!=0 or dy!=0:
# Calculate the new head position
x=x+dx*step
for i in calibration_values_green:
avg[1] += i
avg[1] = avg[1] // len(calibration_values_green)
for i in calibration_values_blue:
avg[2] += i
avg[2] = avg[2] // len(calibration_values_blue)
# write to file
with open('max_rgb.txt', 'w') as fh:
fh.write("red %s\n" % avg[0])
fh.write("green %s\n" % avg[1])
fh.write("blue %s\n" % avg[2])
while True:
dpl.clear()
leds.set_color('LEFT', 'RED')
leds.set_color('RIGHT', 'RED')
write_text('Start?', '-> Taste drücken')
pressed = wait_for_button_press()
if pressed:
dpl.clear()
# logging.basicConfig(filename='rubiks.log',
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s %(filename)12s %(levelname)8s: %(message)s'
)
log = logging.getLogger(__name__)
# Color the errors and warnings in red
