def draw_leds(vmin, vmax):
# vmin should be in [0, -1]
# vmax should be in [0, 1]
global pulse, samples_since_last_pulse, last_pulse_blink
# stop blinking
if not bool(led_mode):
return
# update led bar
if "bar" in led_mode:
for i in reversed(range(6)):
leds.prep_hsv(
5 + i, COLORS[5 + i] if vmin <= 0 and i <= vmin * -6 else
(0, 0, 0))
for i in reversed(range(6)):
leds.prep_hsv(
i, COLORS[i] if vmax >= 0 and 5 - i <= vmax * 6 else (0, 0, 0))
# blink red on pulse
if ("pulse" in led_mode and pulse > 0
and samples_since_last_pulse < last_pulse_blink):
for i in range(4):
leds.prep(11 + i, (255, 0, 0))
elif "pulse" in led_mode:
for i in range(4):
leds.prep(11 + i, (0, 0, 0))
last_pulse_blink = samples_since_last_pulse
leds.update()
def check_Border():
# Q = [0,0]
# Q[0] = DispPosition[0]/6 # Berrechnung des X Quadranten
# Q[1] = DispPosition[1]/6 # Berrechnung des Y Quadranten
# ColorValues = [[255,186,255], [222,135,255], [164,85,255], [104,29,255], [16,0,202], [0,0,152]]
# if (Q[0] >= 0 and Q[0] <= 3.34):
# leds.prep(14, [])
if DispPosition[0] > 120:
leds.prep(12, [255, 0, 0])
leds.prep(13, [255, 0, 0])
elif DispPosition[0] < 10:
leds.prep(11, [255, 0, 0])
leds.prep(14, [255, 0, 0])
else:
leds.prep(12, [0, 0, 0])
leds.prep(13, [0, 0, 0])
if DispPosition[1] < 10:
leds.prep(13, [255, 0, 0])
leds.prep(14, [255, 0, 0])
elif DispPosition[1] > 60:
leds.prep(11, [255, 0, 0])
leds.prep(12, [255, 0, 0])
else:
leds.prep(12, [0, 0, 0])
leds.prep(14, [0, 0, 0])
leds.update()
def wall_follow_demo():
velocity.init(0.22, 40, 0.5, 0.1)
leds.init()
pose.init()
motion.init()
neighbors.init(NBR_PERIOD)
state = STATE_IDLE
wall_time = 0
while True:
# Do updates
leds.update()
pose.update()
velocity.update()
new_nbrs = neighbors.update()
nbrList = neighbors.get_neighbors()
tv = 0
rv = 0
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if new_nbrs:
print "idle"
if rone.button_get_value('r'):
state = STATE_LOOK_FOR_WALL
elif state == STATE_LOOK_FOR_WALL:
leds.set_pattern('r', 'blink_fast', LED_BRIGHTNESS)
if new_nbrs:
print "look for wall"
tv = MOTION_TV
obs = neighbors.get_obstacles()
if (obs != None):
state = STATE_WALL_FOLLOW
elif state == STATE_WALL_FOLLOW:
leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS)
if new_nbrs:
print "wall follow"
# follow the wall
(tv, rv, active) = wall_follow(MOTION_TV / 2)
if active == True:
wall_time = sys.time()
if sys.time() > (wall_time + WALL_TIMEOUT):
state = STATE_LOOK_FOR_WALL
# end of the FSM
# set the velocities
velocity.set_tvrv(tv, rv)
#set the message
hba.set_msg(0, 0, 0)
def follow_the_leader():
mode = MODE_IDLE
velocity.init(0.22, 40, 0.3, 0.1)
leds.init()
neighborsX.init(NEIGHBOR_PERIOD, compute_bearing, compute_orientation)
while mode == MODE_IDLE:
# First, wait for the user to press a button, then select states
# The user is already pressing a button. wait until they release
while check_buttons() != '':
leds_blink_all(LED_BRIGHTNESS)
# wait for a button press
while check_buttons() == '':
leds_blink_all(LED_BRIGHTNESS)
# finally, process the button information
buttons = check_buttons()
print buttons
if 'rg' == buttons:
mode = MODE_SORTED
elif 'bg' == buttons:
mode = MODE_FLOCK
elif 'rb' == buttons:
mode = MODE_ALIGN
elif 'r' in buttons:
mode = MODE_REMOTE
elif 'g' in buttons:
mode = MODE_LEADER
elif 'b' in buttons:
mode = MODE_FOLLOWER
# Now that you know your mode, run the main loop
while True:
# run the neighbor system
neighborsX.update()
# run the velocity controller
velocity.update()
# update the led animations
leds.update()
if mode == MODE_REMOTE:
FTL_remote()
elif mode == MODE_LEADER:
FTL_leader()
elif mode == MODE_FOLLOWER:
FTL_follower()
elif mode == MODE_SORTED:
FTL_sorted()
elif mode == MODE_FLOCK:
flock()
elif mode == MODE_ALIGN:
match_orientation()
def step(self):
# set every LED to its current color value
for i in range(0, self.LED_COUNT):
leds.prep_hsv(i, [self.states[i], 1, 0.5])
leds.update()
leds.dim_top(3)
# wait a certain amount of time
utime.sleep(0.1)
# prepare the next color for each LED
for i in range(0, self.LED_COUNT):
self.states[i] = (self.states[i] + self.STEP) % self.SMTH
def flashlight(status=True):
'''Turns on or off the flashlight, which is all leds at bright white
Args:
status (boolean): The status of the flashlight, True for on, False for off. Defaults to True
'''
if status:
leds.set_powersave(True)
leds.set_all([color.WHITE] * 18)
leds.dim_bottom(8)
leds.dim_top(8)
else:
leds.dim_bottom(0)
leds.dim_top(0)
leds.update()
def pov_char(char, reverse=False):
if reverse:
col_seq = font[char][::-1]
else:
col_seq = font[char]
for col in col_seq:
for i in range(8):
if masks[i] & col:
leds.prep(i + 1, color.GREEN)
else:
leds.prep(i + 1, color.BLACK)
leds.update()
utime.sleep(0.0025)
for i in range(8):
leds.prep(i + 1, color.BLACK)
leds.update()
utime.sleep(0.0025)
def AirQualityLED(gas_quality, numLEDs):
if (gas_quality < 50):
QualityColor = htmlcolor.GREEN
elif (gas_quality < 100):
QualityColor = htmlcolor.YELLOW
elif (gas_quality < 150):
QualityColor = htmlcolor.ORANGE
elif (gas_quality < 200):
QualityColor = htmlcolor.RED
elif (gas_quality < 250):
QualityColor = htmlcolor.CRIMSON
else:
QualityColor = htmlcolor.PURPLE
LEDcount = round(gas_quality / 200 * numLEDs)
leds.clear()
for i in range(0, 10 - LEDcount):
leds.set(i, QualityColor)
leds.update()
def prepare(monitor, top, bottom, left, right, order, inverted):
colors = []
for o in range(len(order)):
if order[o] == 'top':
if inverted[o]:
colors += top[::-1] # Add inverted partial list to final list
else:
colors += top
elif order[o] == 'bottom':
if inverted[o]:
colors += bottom[::-1]
else:
colors += bottom
elif order[o] == 'left':
if inverted[o]:
colors += left[::-1]
else:
colors += left
elif order[o] == 'right':
if inverted[o]:
colors += right[::-1]
else:
colors += right
leds.update(monitor, colors)
def update():
leds.update()
pose.update()
motion.update()
velocity.update()
return neighbors.update()
def render_nickname(title, sub, fg, bg, fg_sub, bg_sub, main_bg):
anim = 0
posy = 30
if sub != '':
posy = 18
r = 255
g = 0
b = 0
r_sub = sub
last_btn_poll = utime.time() - 2
while True:
sleep = 0.5
if sub == '#time':
r_sub = get_time()
dark = 0
if light_sensor.get_reading() < 40:
dark = 1
r_fg_color = fg[dark]
r_bg_color = bg[dark]
r_fg_sub_color = fg_sub[dark]
r_bg_sub_color = bg_sub[dark]
r_bg = main_bg[dark]
# Button handling
pressed = buttons.read(
buttons.BOTTOM_LEFT | buttons.BOTTOM_RIGHT
)
if utime.time() - last_btn_poll >= 1:
last_btn_poll = utime.time()
if pressed & buttons.BOTTOM_RIGHT != 0:
anim = anim + 1
if anim >= len(ANIM_TYPES):
anim = 0
if pressed & buttons.BOTTOM_LEFT != 0:
anim = anim - 1
if anim < 0:
anim = len(ANIM_TYPES) - 1
# Animations
if ANIM_TYPES[anim] == 'fade':
sleep = 0.1
leds.clear()
toggle_rockets(False)
if r > 0 and b == 0:
r = r - 1
g = g + 1
if g > 0 and r == 0:
g = g - 1
b = b + 1
if b > 0 and g == 0:
r = r + 1
b = b - 1
r_bg = [r, g, b]
r_bg_color = r_bg
r_bg_sub_color = r_bg
if ANIM_TYPES[anim] == 'led':
if dark == 1:
for i in range(0, 11):
leds.prep(i, r_bg)
leds.update()
leds.dim_top(4)
toggle_rockets(True)
else:
leds.clear()
toggle_rockets(False)
if ANIM_TYPES[anim] == 'gay':
toggle_rockets(False)
leds.gay(0.4)
if ANIM_TYPES[anim] == 'none':
leds.clear()
toggle_rockets(False)
with display.open() as disp:
disp.rect(0, 0, 160, 80, col=r_bg, filled=True)
disp.print(title, fg=r_fg_color, bg=r_bg_color, posx=80 - round(len(title) / 2 * 14), posy=posy)
if r_sub != '':
disp.print(r_sub, fg=r_fg_sub_color, bg=r_bg_sub_color, posx=80 - round(len(r_sub) / 2 * 14), posy=42)
disp.update()
disp.close()
utime.sleep(sleep)
def render_nickname(title, sub, fg, bg, fg_sub, bg_sub, main_bg):
anim = 'led'
posy = 30
if sub != '':
posy = 18
r = 255
g = 0
b = 0
while True:
dark = 0
if light_sensor.get_reading() < 30:
dark = 1
r_fg_color = fg[dark]
r_bg_color = bg[dark]
r_fg_sub_color = fg_sub[dark]
r_bg_sub_color = bg_sub[dark]
r_bg = main_bg[dark]
if anim == 'fade':
if r > 0 and b == 0:
r = r - 1
g = g + 1
if g > 0 and r == 0:
g = g - 1
b = b + 1
if b > 0 and g == 0:
r = r + 1
b = b - 1
r_bg = [r, g, b]
if anim == 'led':
for i in range(0, 11):
leds.prep(i, r_bg)
leds.update()
leds.dim_top(3)
leds.set_rocket(0, 15)
leds.set_rocket(1, 15)
leds.set_rocket(2, 15)
if anim == 'none':
leds.clear()
leds.set_rocket(0, 0)
leds.set_rocket(1, 0)
leds.set_rocket(2, 0)
with display.open() as disp:
disp.rect(0, 0, 160, 80, col=r_bg, filled=True)
disp.print(title,
fg=r_fg_color,
bg=r_bg_color,
posx=80 - round(len(title) / 2 * 14),
posy=posy)
if sub != '':
disp.print(sub,
fg=r_fg_sub_color,
bg=r_bg_sub_color,
posx=80 - round(len(sub) / 2 * 14),
posy=42)
disp.update()
disp.close()
pressed = buttons.read(buttons.BOTTOM_LEFT | buttons.BOTTOM_RIGHT)
if pressed & buttons.BOTTOM_LEFT != 0:
anim = ANIM_TYPES[1]
if pressed & buttons.BOTTOM_RIGHT != 0:
anim = ANIM_TYPES[0]
utime.sleep(0.3)
def waypoint_motion():
velocity.init(0.22, 40, 0.5, 0.1)
leds.init()
poseX.init(pose_update)
motionX.init(compute_goal_distance_and_heading, motion_controller_tv,
motion_controller_rv)
pose_estimator_print_time = sys.time()
mode = MODE_INACTIVE
pose_old = (0.0, 0.0, 0.0)
waypoint_list = []
while True:
# update the LED animations
leds.update()
# update the pose estimator
poseX.update()
# update the motion controller
(tv, rv) = motionX.update()
velocity.set_tvrv(tv, rv)
# update the velocity controller if you are active, otherwise coast so the robot can be pushed
if mode == MODE_ACTIVE:
velocity.update()
else:
rone.motor_set_pwm('l', 0)
rone.motor_set_pwm('r', 0)
# print status every 500ms
current_time = sys.time()
if sys.time() > pose_estimator_print_time:
pose_estimator_print_time += 250
print 'goal', motionX.get_goal(), 'pose', poseX.get_pose(
), 'odo', poseX.get_odometer()
if mode == MODE_INACTIVE:
if (math2.pose_subtract(poseX.get_pose(), pose_old) !=
(0.0, 0.0, 0.0)):
# We're moving! Yay! Blink excitedly!
leds.set_pattern('r', 'blink_fast',
int(LED_BRIGHTNESS * 1.5))
else:
# not moving. sad face.
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
pose_old = poseX.get_pose()
# check the buttons. If the red button is pressed, load the waypoint list
if rone.button_get_value('r'):
if mode == MODE_INACTIVE:
poseX.set_pose(0, 0, 0)
#waypoint_list = [(608, 0), (608, 304), (0, 304), (0, 0)]
waypoint_list = [(700, 0), (700, 700), (0, 700), (0, 0)]
mode = MODE_ACTIVE
# check to see if you are at your waypoint. If so, go to the next one
if mode == MODE_ACTIVE:
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
if motionX.is_done():
## Do we have another waypoint?
if len(waypoint_list) > 0:
leds.set_pattern('rgb', 'group', LED_BRIGHTNESS)
sys.sleep(250)
waypoint = waypoint_list.pop(0)
print 'waypoint', waypoint
motionX.set_goal(waypoint, MOTION_TV)
else:
print 'waypoint list empty'
mode = MODE_INACTIVE
velocity.set_tvrv(0, 0)
def render_nickname(title, sub, fg, bg, fg_sub, bg_sub, main_bg, mode, bat):
"""
Main function to render the nickname on screen.
Pretty ugly but not time for cleanup right now (and some APIs missing)
:param title: first row of text
:param sub: second row of text
:param fg: tuple of (day, night) rgb for title text color
:param bg: tuple of (day, night) rgb for title background color
:param fg_sub: tuple of (day, night) rgb for subtitle text color
:param bg_sub: tuple of (day, night) rgb for subtitle background color
:param main_bg: tuple of (day, night) rgb for general background color
:param mode: default animation to start in (index of ANIM_TYPES array)
:param bat: battery config tuple (boolean: indicator on/off, array: good rgb, array: ok rgb, array: bad rgb)
"""
anim = mode
posy = 30
if sub != "":
posy = 18
r = 255
g = 0
b = 0
rainbow_led_pos = 0
r_sub = sub
last_btn_poll = utime.time() - 2
while True:
sleep = 0.5
if sub == "#time":
r_sub = get_time()
dark = 0
if light_sensor.get_reading() < 30:
dark = 1
r_fg_color = fg[dark]
r_bg_color = bg[dark]
r_fg_sub_color = fg_sub[dark]
r_bg_sub_color = bg_sub[dark]
r_bg = main_bg[dark]
# Button handling
pressed = buttons.read(buttons.BOTTOM_LEFT | buttons.BOTTOM_RIGHT)
if utime.time() - last_btn_poll >= 1:
last_btn_poll = utime.time()
if pressed & buttons.BOTTOM_RIGHT != 0:
anim = anim + 1
if anim >= len(ANIM_TYPES):
anim = 0
blink_led(0)
if pressed & buttons.BOTTOM_LEFT != 0:
anim = anim - 1
if anim < 0:
anim = len(ANIM_TYPES) - 1
blink_led(0)
# Animations
if ANIM_TYPES[anim] == "fade":
sleep = 0.1
leds.clear()
toggle_rockets(False)
if r > 0 and b == 0:
r -= 1
g += 1
if g > 0 and r == 0:
g -= 1
b += 1
if b > 0 and g == 0:
r += 1
b -= 1
r_bg = [r, g, b]
r_bg_color = r_bg
r_bg_sub_color = r_bg
if ANIM_TYPES[anim] == "led":
if dark == 1:
for i in range(0, 11):
leds.prep(i, r_bg)
leds.update()
leds.dim_top(4)
toggle_rockets(True)
else:
leds.clear()
toggle_rockets(False)
if ANIM_TYPES[anim] == "rnd_led":
if dark == 1:
for i in range(0, 11):
leds.prep(i, random_rgb())
leds.update()
leds.dim_top(4)
toggle_rockets(True)
else:
leds.clear()
toggle_rockets(False)
if ANIM_TYPES[anim] == "gay":
toggle_rockets(False)
leds.gay(0.4)
if ANIM_TYPES[anim] == "rainbow":
for i in range(0, 11):
lr, lg, lb = wheel(rainbow_led_pos + i * 3)
leds.prep(i, [lr, lg, lb])
rainbow_led_pos += 1
if rainbow_led_pos > 255:
rainbow_led_pos = 0
leds.update()
leds.dim_top(3)
toggle_rockets(True)
if ANIM_TYPES[anim] == "none":
leds.clear()
toggle_rockets(False)
with display.open() as disp:
disp.rect(0, 0, 160, 80, col=r_bg, filled=True)
if bat[0]:
render_battery(disp, bat)
disp.print(
title,
fg=r_fg_color,
bg=r_bg_color,
posx=80 - round(len(title) / 2 * 14),
posy=posy,
)
if r_sub != "":
disp.print(
r_sub,
fg=r_fg_sub_color,
bg=r_bg_sub_color,
posx=80 - round(len(r_sub) / 2 * 14),
posy=42,
)
disp.update()
disp.close()
utime.sleep(sleep)
def update():
leds.update()
pose.update()
motion.update()
velocity.update()
return neighbors.update()
import math
import leds
import utime
import os
while True:
# leds.set_all(os.urandom(3))
for i in range(0, 11):
leds.prep(i, os.urandom(3))
leds.update()
utime.sleep_ms(5)
def draw_leds(val):
# val should be in [0, 11]
for i in range(11):
leds.prep_hsv(10 - i, COLORS[10 - i] if i < val else (0, 0, 0))
leds.update()
