def bump_beh(tv):
# act on the information from the message. Note that this might be
# information stored from the last message we received, because message
# information remains active for a while
global bump_time
global bump_state
bump_bits = rone.bump_sensors_get_value()
# check bump sensors
if bump_left_get_value(bump_bits):
bump_state = BUMP_STATE_TURN_RIGHT
bump_time = sys.time() + BUMP_TIME_SIDE
elif bump_right_get_value(bump_bits):
bump_state = BUMP_STATE_TURN_LEFT
bump_time = sys.time() + BUMP_TIME_SIDE
elif bump_front_get_value(bump_bits):
bump_state = BUMP_STATE_ROTATE
bump_time = sys.time() + BUMP_TIME_ROTATE
if sys.time() > bump_time:
bump_state = BUMP_STATE_IDLE
# control the robot
beh_out = BEH_INACTIVE
if bump_state == BUMP_STATE_TURN_RIGHT:
beh_out = (0, -tv * TVRV_RATIO, True)
elif bump_state == BUMP_STATE_TURN_LEFT:
beh_out = (0, tv * TVRV_RATIO, True)
elif bump_state == BUMP_STATE_ROTATE:
beh_out = (0, tv * TVRV_RATIO, True)
return beh_out
def bump_beh(tv):
# act on the information from the message. Note that this might be
# information stored from the last message we received, because message
# information remains active for a while
global bump_time
global bump_state
bump_bits = rone.bump_sensors_get_value()
# check bump sensors
if bump_left_get_value(bump_bits):
bump_state = BUMP_STATE_TURN_RIGHT
bump_time = sys.time() + BUMP_TIME_SIDE
elif bump_right_get_value(bump_bits):
bump_state = BUMP_STATE_TURN_LEFT
bump_time = sys.time() + BUMP_TIME_SIDE
elif bump_front_get_value(bump_bits):
bump_state = BUMP_STATE_ROTATE
bump_time = sys.time() + BUMP_TIME_ROTATE
if sys.time() > bump_time:
bump_state = BUMP_STATE_IDLE
# control the robot
beh_out = BEH_INACTIVE
if bump_state == BUMP_STATE_TURN_RIGHT:
beh_out = (0, -tv * TVRV_RATIO, True)
elif bump_state == BUMP_STATE_TURN_LEFT:
beh_out = (0, tv * TVRV_RATIO, True)
elif bump_state == BUMP_STATE_ROTATE:
beh_out = (0, tv * TVRV_RATIO, True)
return beh_out
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 init(nbr_period):
_nbr_state['time_ir_xmit'] = sys.time()
_nbr_state['time_ir_xmit_offset'] = 0
_nbr_state['nbr_period'] = nbr_period
_nbr_state['nbr_timeout'] = 3 * nbr_period
_nbr_state['message'] = ''
_nbr_state['xmit_enable'] = True
_nbr_state['nbr_list'] = []
_nbr_state['obstacles'] = None
_nbr_state['obstacles_time'] = sys.time()
def init(nbr_period, compute_bearing_func, compute_ornt_func):
_nbr_state['time_ir_xmit'] = sys.time()
_nbr_state['time_ir_xmit_offset'] = 0
_nbr_state['nbr_period'] = nbr_period
_nbr_state['nbr_timeout'] = 3 * nbr_period
_nbr_state['message'] = ''
_nbr_state['xmit_enable'] = True
_nbr_state['nbr_list'] = []
_nbr_state['obstacles'] = None
_nbr_state['obstacles_time'] = sys.time()
global _compute_brg
global _compute_ornt
_compute_brg = compute_bearing_func
_compute_ornt = compute_ornt_func
def main():
# Saves the editmode state and go's out of
# editmode if its enabled, we cant make
# changes to the mesh data while in editmode.
is_editmode = Window.EditMode()
if is_editmode:
Window.EditMode(0)
Window.WaitCursor(1)
t = sys.time()
# Restore editmode if it was enabled
if is_editmode:
Window.EditMode(1)
print "ActionScript 3.0 Exporter Script finished in %.2f seconds" % (sys.time() - t)
Window.WaitCursor(0)
def FTL_leader():
global radio_msg_time, FTL_leader_tv, FTL_leader_rv
radio_msg = radio_get_message()
if radio_msg != None:
# we have a message! put lights in active mode
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
radio_msg_time = sys.time()
(FTL_leader_tv, FTL_leader_rv) = leader_motion_controller(radio_msg)
else:
# no message. check for radio message timeout
if sys.time() > (radio_msg_time + REMOTE_XMIT_DELAY * 3):
# message timeout. stop the motors
FTL_leader_tv = 0
FTL_leader_rv = 0
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
velocity.set_tvrv(FTL_leader_tv, FTL_leader_rv)
def leds_blink_all(brightness):
if (sys.time() % 500) < 250:
rone.led_set_group('r', brightness)
rone.led_set_group('g', brightness)
rone.led_set_group('b', brightness)
else:
rone.led_set_group('r', 0)
rone.led_set_group('g', 0)
rone.led_set_group('b', 0)
def receive_beh():
global receive_msg_time, receive_msg_tv, receive_msg_rv
radio_msg = rone.radio_get_message()
active = True
if radio_msg != None:
# we have a message! put lights in active mode
leds.set_pattern('g','blink_fast',LED_BRIGHTNESS)
receive_msg_time = sys.time()
(receive_msg_tv, receive_msg_rv) = remote_motion_controller(radio_msg)
else:
# no message. check for radio message timeout
if sys.time() > (receive_msg_time + REMOTE_XMIT_DELAY * 3):
# message timeout. stop the motors
receive_msg_tv = 0
receive_msg_rv = 0
active = False
leds.set_pattern('g','circle',LED_BRIGHTNESS)
return (receive_msg_tv, receive_msg_rv, active)
def leds_blink_all(brightness):
if (sys.time() % 500) < 250:
rone.led_set_group('r', brightness)
rone.led_set_group('g', brightness)
rone.led_set_group('b', brightness)
else:
rone.led_set_group('r', 0)
rone.led_set_group('g', 0)
rone.led_set_group('b', 0)
def init():
global pose_state
pose_state['ticksL'] = rone.encoder_get_ticks('l')
pose_state['ticksR'] = rone.encoder_get_ticks('r')
#pose_state['pose'] = (0.0, 0.0, 0.0)
pose_state['x'] = 0.0
pose_state['y'] = 0.0
pose_state['theta'] = 0.0
pose_state['odometer'] = 0.0
pose_state['update_time'] = sys.time()
def init(kff, kff_offset, kp, ki):
_vcstate['l'] = {}
_vcstate['r'] = {}
# K terms
_vcstate['kff'] = kff
_vcstate['kff_offset'] = kff_offset
_vcstate['kp'] = kp
_vcstate['ki'] = ki
_vcstate['update_time'] = sys.time()
_vcstate['tv_ramp'] = 0
_vcstate['tv_update_time'] = sys.time()
for motor in ['l', 'r']:
_vcstate[motor]['ticks'] = rone.encoder_get_ticks(motor) # Position
_vcstate[motor]['time'] = sys.time() # Time
_vcstate[motor]['iterm'] = 0.0 # Iterm
_vcstate[motor]['goalvel'] = 0 # Goal velocity
_vcstate[motor]['vel'] = 0 # Current velocity
def init(kff, kff_offset, kp, ki):
_vcstate["l"] = {}
_vcstate["r"] = {}
# K terms
_vcstate["kff"] = kff
_vcstate["kff_offset"] = kff_offset
_vcstate["kp"] = kp
_vcstate["ki"] = ki
_vcstate["update_time"] = sys.time()
_vcstate["tv_ramp"] = 0
_vcstate["tv_update_time"] = sys.time()
for motor in ["l", "r"]:
_vcstate[motor]["ticks"] = rone.encoder_get_ticks(motor) # Position
_vcstate[motor]["time"] = sys.time() # Time
_vcstate[motor]["iterm"] = 0.0 # Iterm
_vcstate[motor]["goalvel"] = 0 # Goal velocity
_vcstate[motor]["vel"] = 0 # Current velocity
def init(kff, kff_offset, kp, ki):
_vcstate['l'] = {}
_vcstate['r'] = {}
# K terms
_vcstate['kff'] = kff
_vcstate['kff_offset'] = kff_offset
_vcstate['kp'] = kp
_vcstate['ki'] = ki
_vcstate['update_time'] = sys.time()
_vcstate['tv_ramp'] = 0
_vcstate['tv_update_time'] = sys.time()
for motor in ['l', 'r']:
_vcstate[motor]['ticks'] = rone.encoder_get_ticks(motor) # Position
_vcstate[motor]['time'] = sys.time() # Time
_vcstate[motor]['iterm'] = 0.0 # Iterm
_vcstate[motor]['goalvel'] = 0 # Goal velocity
_vcstate[motor]['vel'] = 0 # Current velocity
def update():
global pose_state
global _update_pose
current_time = sys.time()
update_time = pose_state['update_time']
if current_time > update_time:
update_time += POSE_UPDATE_PERIOD
# advance time if there have been delays in calling update
if update_time < current_time:
update_time = current_time + POSE_UPDATE_PERIOD
pose_state['update_time'] = update_time
_update_pose(pose_state)
def node_initialize(node):
sp = pxssh.pxssh()
counter = 0
print(node["node-name"] + " -> Logging into SP")
if (not sp.login(
node["sp-ip"], node["user"], node["password"],
auto_prompt_reset=False)):
print(node["node-name"] + " -> SSH session failed on login.")
print(str(sp))
else:
print(node["node-name"] + " -> SSH session login successful")
# Switching to system console
exec_ssh(sp, "^SP.*>", "system console", node["node-name"])
exec_ssh(sp, None, "", node["node-name"])
# Wait for loader prompt and boot into boot menu
counter = 0
while not exec_ssh(sp, "LOADER-[AB]{1}>", "boot_ontap menu", node["node-name"]) and \
(counter < 12):
print(node["node-name"] + " -> waiting 10 seconds...")
counter = counter + 1
sys.time(10)
if (counter >= 12):
print(node["node-name"] + " -> Could not set prompt. Exiting NOW!")
sys.exit(1)
# selecting option for initialize in boot menu
counter = 0
while not exec_ssh(sp, "Selection .*\?", "4", node["node-name"]) and \
(counter < 12):
print(node["node-name"] + " -> waiting 10 seconds...")
counter = counter + 1
sys.time(10)
if (counter >= 12):
print(node["node-name"] + " -> Could not set prompt. Exiting NOW!")
sys.exit(1)
# Confirming warnings
counter = 0
while not exec_ssh(sp, "Zero disks, reset config and install a new file system\?:", "y", node["node-name"]) and \
(counter < 12):
print(node["node-name"] + " -> waiting 10 seconds...")
counter = counter + 1
sys.time(10)
if (counter >= 12):
print(node["node-name"] + " -> Could not set prompt. Exiting NOW!")
sys.exit(1)
else:
exec_ssh(
sp,
"This will erase all the data on the disks, are you sure\?:",
"y", node["node-name"])
exec_ssh(sp, None, "d", node["node-name"])
print(node["node-name"] + " -> logging out...")
sp.logout()
def set_tvrv(tv, rv):
tv = int(tv)
if sys.time() > _vcstate["tv_update_time"]:
_vcstate["tv_update_time"] += _TV_UPDATE_PERIOD
if tv < _vcstate["tv_ramp"]:
_vcstate["tv_ramp"] -= TV_RAMP_DOWN
if tv > _vcstate["tv_ramp"]:
_vcstate["tv_ramp"] = tv
if tv > _vcstate["tv_ramp"]:
_vcstate["tv_ramp"] += TV_RAMP_UP
if _vcstate["tv_ramp"] > tv:
_vcstate["tv_ramp"] = tv
tv = _vcstate["tv_ramp"]
left_vel = tv - rv * WHEEL_BASE / 2000
right_vel = tv + rv * WHEEL_BASE / 2000
left_vel = clamp(left_vel, SPEED_MAX)
right_vel = clamp(right_vel, SPEED_MAX)
set("l", left_vel)
set("r", right_vel)
def set_tvrv(tv, rv):
tv = int(tv)
if sys.time() > _vcstate['tv_update_time']:
_vcstate['tv_update_time'] += _TV_UPDATE_PERIOD
if tv < _vcstate['tv_ramp']:
_vcstate['tv_ramp'] -= TV_RAMP_DOWN
if tv > _vcstate['tv_ramp']:
_vcstate['tv_ramp'] = tv
if tv > _vcstate['tv_ramp']:
_vcstate['tv_ramp'] += TV_RAMP_UP
if _vcstate['tv_ramp'] > tv:
_vcstate['tv_ramp'] = tv
tv = _vcstate['tv_ramp']
left_vel = tv - rv * WHEEL_BASE / 2000
right_vel = tv + rv * WHEEL_BASE / 2000
left_vel = clamp(left_vel, SPEED_MAX)
right_vel = clamp(right_vel, SPEED_MAX)
set('l', left_vel)
set('r', right_vel)
def _velocity(motor):
## vel_goal in mm/s
## motor either 'l' or 'r'
## iterm_old passed in from previous function runs (sum of the errors * ki)
## ticks_old is the old position of the given wheel
## time_old is the time of the last reading of the encoders
vel_goal = _vcstate[motor]['goalvel']
iterm_old = _vcstate[motor]['iterm']
ticks_old = _vcstate[motor]['ticks']
time_old = _vcstate[motor]['time']
# compute distance
ticks_new = rone.encoder_get_ticks(motor)
distance = _compute_distance(ticks_new, ticks_old)
# compute velocity_controller
time_new = sys.time()
time_delta = time_new - time_old
velocity = _compute_velocity(distance, time_delta)
_vcstate[motor]['vel'] = velocity
# some debug printing. Don't leave it in, it slows down the computer
# compute feedback terms
error = vel_goal - velocity
feedforward_term = _feedforward_compute(vel_goal)
proportional_term = _proportional_compute(error)
integral_term = _integral_compute(error, iterm_old)
pwm = feedforward_term + proportional_term + integral_term
pwm = int(pwm)
pwm = clamp(pwm, 100)
rone.motor_set_pwm(motor, pwm)
#Some example debugging output. Don't print this always, it will slow things down too much
#print 'motor=%s ff_term=%5.1f i_term=%5.1f pwm=%3d' % (motor, float(feedforward_term), float(integral_term), pwm)
# update old values
_vcstate[motor]['ticks'] = ticks_new
_vcstate[motor]['time'] = time_old = time_new
_vcstate[motor]['iterm'] = integral_term
def _velocity(motor):
## vel_goal in mm/s
## motor either 'l' or 'r'
## iterm_old passed in from previous function runs (sum of the errors * ki)
## ticks_old is the old position of the given wheel
## time_old is the time of the last reading of the encoders
vel_goal = _vcstate[motor]["goalvel"]
iterm_old = _vcstate[motor]["iterm"]
ticks_old = _vcstate[motor]["ticks"]
time_old = _vcstate[motor]["time"]
# compute distance
ticks_new = rone.encoder_get_ticks(motor)
distance = _compute_distance(ticks_new, ticks_old)
# compute velocity_controller
time_new = sys.time()
time_delta = time_new - time_old
velocity = _compute_velocity(distance, time_delta)
_vcstate[motor]["vel"] = velocity
# some debug printing. Don't leave it in, it slows down the computer
# compute feedback terms
error = vel_goal - velocity
feedforward_term = _feedforward_compute(vel_goal)
integral_term = _integral_compute(error, iterm_old)
proportional_term = _proportional_compute(error)
pwm = feedforward_term + proportional_term + integral_term
pwm = int(pwm)
pwm = clamp(pwm, 100)
rone.motor_set_pwm(motor, pwm)
# Some example debugging output. Don't print this always, it will slow things down too much
# print 'motor=%s ff_term=%5.1f i_term=%5.1f pwm=%3d' % (motor, float(feedforward_term), float(integral_term), pwm)
# update old values
_vcstate[motor]["ticks"] = ticks_new
_vcstate[motor]["time"] = time_old = time_new
_vcstate[motor]["iterm"] = integral_term
def update():
global pose_state
current_time = sys.time()
update_time = pose_state['update_time']
if current_time > update_time:
update_time += POSE_UPDATE_PERIOD
# advance time if there have been delays in calling update
if update_time < current_time:
update_time = current_time + POSE_UPDATE_PERIOD
pose_state['update_time'] = update_time
# Compute the linear distance traveled by each wheel in millimeters
ticksL = rone.encoder_get_ticks('l')
ticksR = rone.encoder_get_ticks('r')
distL = float(velocity.encoder_delta_ticks(ticksL, pose_state['ticksL'])) * 0.0625
distR = float(velocity.encoder_delta_ticks(ticksR, pose_state['ticksR'])) * 0.0625
pose_state['ticksL'] = ticksL
pose_state['ticksR'] = ticksR
# Compute the distance traveled by the center of the robot in millimeters
dist = (distR + distL) / 2.0
pose_state['odometer'] += abs(dist)
# compute the arc angle in radians
# use the small angle approximation: arctan(theta) ~ theta
delta_theta = (distR - distL) / (WHEEL_BASE);
# (x, y, theta) = pose_state['pose']
# x = x + dist * math.cos(theta)
# y = y + dist * math.sin(theta)
# theta = math2.normalize_theta(theta + delta_theta)
# pose_state['pose'] = (x, y, theta)
theta = pose_state['theta']
pose_state['x'] += dist * math.cos(theta)
pose_state['y'] += dist * math.sin(theta)
pose_state['theta'] = math2.normalize_angle(theta + delta_theta)
import sys;
import list;
import dict;
import string;
import ipm;
def nulladd(x):
return(x+1);
def nulllib(y):
print ;
return(y - 355);
j=0;
### dir(__builtins__);
print sys.time(); # will return zero... not implemented yet X-D
a="hello world!";
print "=== check: string";
"hello world!";
a;
print "=== check: print string";
print "hello world!";
print a;
print len("hello world!");
print len(a);
##print length(a[2:7]); ## slicees are not implemented yet
##print a[2:7]; ## slicees are not implemented yet
print "=== check: print number";
def update():
if sys.time() > _vcstate["update_time"]:
_vcstate["update_time"] += _VEL_UPDATE_PERIOD
_velocity("l")
_velocity("r")
#mb = mb[0]
import flightplanstatus
#ma = sys.heap()
#ma = ma[0]
#print('import flightplanstatus')
#print(mb-ma)
#mb = sys.heap()
#mb = mb[0]
import mixersettings
#ma = sys.heap()
#ma = ma[0]
#print('import mixersettings')
#print(mb-ma)
n = 0
timenow = sys.time()
fpStatus = flightplanstatus.FlightPlanStatus()
while n < 120:
n = n+1
#openpilot.debug(n, timenow)
fpStatus.read()
fpStatus.Debug.value[0] = n
fpStatus.Debug.value[1] = timenow
fpStatus.write()
timenow = openpilot.delayUntil(timenow, 1000)
if openpilot.hasStopRequest():
sys.exit()
def fall():
found_flower = False
start_time = 0
target_theta = 0
my_color = -1
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
color = 'r' #for flowers only
def wander():
state = STATE_WANDER
def collect_pollen():
state = STATE_COLLECT_POLLEN
start_time = sys.time()
def align_with(target):
target_theta = target
pose.set_pose(0, 0, 0)
state = STATE_ALIGN
start_time = sys.time()
#motion_start_odo = pose.get_odometer()
while True:
beh.init(0.22, 40, 0.5, 0.1)
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
#FINITE STATE MACHINE
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_WANDER: #run forward, avoid direction of neighbors
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
if nav_tower == None:
state = STATE_RETURN_TO_BASE
else:
beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)
(flower, color) = detflower(nbrList)
if flower != None:
state = STATE_MOVE_TO_FLOWER
elif state == STATE_MOVE_TO_FLOWER:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
(flower, color) = detflower(nbrList)
if flower != None:
if (neighbors.get_nbr_range_bits(flower) >
6) or (beh.bump_angle_get() != None):
collect_pollen() #collect pollen if we bump or get close
else:
#otherwise keep following that flower
beh_out = beh.follow_nbr(flower, MOTION_TV)
elif state == STATE_COLLECT_POLLEN:
motion_start_odo = pose.get_odometer()
if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
state = STATE_RETURN_TO_BASE
found_flower = True
elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
tv = -MOTION_TV
rv = 0
beh_out = beh.tvrv(tv, rv)
turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
elif sys.time() < (turn_start_time + TURN_TIME):
tv = 40
rv = -MOTION_RV
beh_out = beh.tvrv(tv, rv)
else:
tv = MOTION_TV
rv = (MOTION_RV - 300)
beh_out = beh.tvrv(tv, rv)
elif state == STATE_RETURN_TO_BASE:
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = find_queen(nbrList)
if (nav_tower == None) and (queen == None):
beh_out = (-MOTION_TV, 0, True)
elif (nav_tower != None) and (queen == None):
beh_out = beh.follow_nbr(nav_tower)
elif neighbors.get_nbr_range_bits(queen) > 2:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif found_flower:
state = STATE_RECRUIT
start_time = sys.time()
else:
state = STATE_FOLLOW
start_time = sys.time()
elif state == STATE_FOLLOW:
recruiter = find_recruiter()
if recruiter == None:
beh_out = beh.BEH_INACTIVE
if sys.time() > (follow_start_time + FOLLOW_TIME):
wander()
else:
bearing = neighbors.get_nbr_bearing(recruiter)
orientation = neighbors.get_nbr_orientation(recruiter)
align_with(math.pi + bearing - orientation)
elif state == STATE_GO:
flower = detflower()
if not flower == None:
state = STATE_MOVE_TO_FLOWER
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_RECRUIT:
if sys.time() > (recruit_start_time + RECRUIT_TIME):
align_with(pose.get_theta() - math.pi)
elif state == STATE_ALIGN:
tv = 0
heading_error = math.normalize_angle(pose.get_theta() -
target_theta)
rv = ROTATE_RV_GAIN * heading_error
beh_out = beh.tvrv(tv, rv)
# you could actually do a running average in the list here
small_error = hba.average_error_check(heading_error, [],
HEADING_ERROR_LIMIT,
new_nbrs)
if new_nbrs:
print "error", error_list
if small_error:
state = STATE_GO
#END OF FINITE STATE MACHINE
bump_beh_out = beh.bump_beh(MOTION_TV)
if state not in [
STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT
]:
beh_out = beh.subsume([beh_out, bump_beh_out])
beh.motion_set(beh_out)
hba.set_msg(state, my_color, 0)
def collect_pollen():
state = STATE_COLLECT_POLLEN
start_time = sys.time()
# it under the terms of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1.
#
# Python-on-a-Chip is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# A copy of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1
# is seen in the file COPYING up one directory from this.
#
# Test for Issue #90: Create new lib function to return system time
# Ensure that time passes
#
import sys
t0 = sys.time()
print "t0 = ", t0
print "killing time..."
i=0
while i < 10000:
j=0
while j < 100:
j += 1
if i%100 == 0:
print i / 100, " ",
i += 1
print
t1 = sys.time()
def collect_pollen():
state = STATE_COLLECT_POLLEN
start_time = sys.time()
# it under the terms of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1.
#
# Python-on-a-Chip is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# A copy of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1
# is seen in the file COPYING up one directory from this.
#
# Test for Issue #90: Create new lib function to return system time
# Ensure that time passes
#
import sys
t0 = sys.time()
print "t0 = ", t0
print "killing time..."
i = 0
while i < 10000:
j = 0
while j < 100:
j += 1
if i % 100 == 0:
print i / 100, " ",
i += 1
print
t1 = sys.time()
def align_with(target):
target_theta = target
pose.set_pose(0, 0, 0)
state = STATE_ALIGN
start_time = sys.time()
#print 'radio msg:', radio_msg # debugging code - comment out when this is working
if 'g' in radio_msg:
# move forward
tv = FTL_TV
if 'r' in radio_msg:
# rotate left when red button pressed
rv = FTL_RV
elif 'b' in radio_msg:
# rotate right when blue button pressed
rv = -FTL_RV
return (tv, rv)
receive_msg_time = sys.time()
receive_msg_tv = 0
receive_msg_rv = 0
def receive_beh():
global receive_msg_time, receive_msg_tv, receive_msg_rv
radio_msg = rone.radio_get_message()
active = True
if radio_msg != None:
# we have a message! put lights in active mode
leds.set_pattern('g','blink_fast',LED_BRIGHTNESS)
receive_msg_time = sys.time()
(receive_msg_tv, receive_msg_rv) = remote_motion_controller(radio_msg)
else:
# no message. check for radio message timeout
if sys.time() > (receive_msg_time + REMOTE_XMIT_DELAY * 3):
else:
left_wheel.setCounts(left_wheel_forward)
right_wheel.setCounts(right_wheel_forward)
# Print out line following diags
if line_sensor_left.get():
print "X",
else:
print ".",
if line_sensor_middle.get():
print "X",
else:
print ".",
if line_sensor_right.get():
print "X"
else:
print "."
# Delay some
time = sys.time()
while time + 10 > sys.time():
pass
def flower_motion():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
if new_nbrs:
print "idle"
elif state == STATE_MOVE_TO_FLOWER:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
if new_nbrs:
print "move to flower"
# Move towards the flower until you bump into it
# for this demo, assume the first robot on the list is a flower
## flower = nbrList_getFirstRobot(nbrList)
(color,nbr) = detflower(nbrList)
flower = nbr
if flower != None:
# Stop if we get close or bump into the flower
#if neighbors.get_nbr_close_range(flower):
if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
state = STATE_COLLECT_POLLEN
collect_pollen_start_time = sys.time()
else:
# Move to the flower
beh_out = beh.follow_nbr(flower, MOTION_TV)
#print beh_out
elif state == STATE_COLLECT_POLLEN:
# this is where you will put your clever pollen collection code
# we will just wait for a second, then leave. (this will not collect very much pollen)
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
if new_nbrs:
print "collect"
# Timeout after 5 seconds
if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
state = STATE_MOVE_AWAY_FLOWER
elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
tv = -MOTION_TV
rv = 0
beh_out = beh.tvrv(tv,rv)
turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
elif sys.time() < (turn_start_time + TURN_TIME):
tv = 0
rv = -MOTION_RV
else:
tv = MOTION_TV
rv = MOTION_RV
beh_out = beh.tvrv(tv,rv)
elif state == STATE_MOVE_AWAY_FLOWER:
if new_nbrs:
print "avoid flower"
leds.set_pattern('r', 'blink_slow', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
# Move away from the flower until it is out of range
flower = nbrList_getFirstRobot(nbrList)
if flower != None:
# Point away the flower
beh_out = beh.avoid_nbr(flower, MOTION_TV)
else:
state = STATE_IDLE
# end of the FSM
bump_beh_out = beh.bump_beh(MOTION_TV)
if state != STATE_COLLECT_POLLEN:
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
hba.set_msg(0, 0, 0)
def update():
global old_pattern
current_time = sys.time()
update_time = _leds_state['update_time']
if current_time > update_time:
update_time += LED_UPDATE_PERIOD
# advance time if there have been delays in calling update
if update_time < current_time:
update_time = current_time + LED_UPDATE_PERIOD
_leds_state['update_time'] = update_time
counter = _leds_state['counter'] + 1
_leds_state['counter'] = counter
color = _leds_state['color']
pattern = _leds_state['pattern']
brightness = _leds_state['brightness']
if _leds_state['pattern'] != 'manual':
for c in 'rgb':
if c not in color:
_led_set_group(c, 0)
if pattern == 'group':
_led_set_group(color, brightness)
elif pattern == 'ramp_slow':
idx = counter % BLINK_SLOW
if idx < (BLINK_SLOW / 2):
# ramp up
b = brightness * idx / (BLINK_SLOW / 2)
else:
b = brightness * (BLINK_SLOW - idx) / (BLINK_SLOW / 2)
_led_set_group(color, b)
elif pattern == 'blink_slow':
idx = counter % BLINK_SLOW
if idx < (BLINK_SLOW / 2):
_led_set_group(color, brightness)
else:
_led_set_group(color, 0)
elif pattern == 'blink_fast':
idx = counter % BLINK_FAST
if idx < (BLINK_FAST / 2):
_led_set_group(color, brightness)
else:
_led_set_group(color, 0)
elif pattern == 'circle':
idx = counter % 5
led_map = rone._led_map[color]
for i in range(5):
if i == idx:
rone._led_set(led_map[i], brightness)
else:
rone._led_set(led_map[i], 0)
elif pattern == 'count':
# display ints as counting on the lights
idx = 0
for c in 'rgb':
if color[idx] == 0:
_led_set_group(c, 0)
else:
led_map = rone._led_map[c]
for i in range(5):
if i < color[idx]:
rone._led_set(led_map[i], brightness)
else:
rone._led_set(led_map[i], 0)
idx += 1
elif pattern == 'manual':
pass
old_pattern = pattern
def main(): sys.time(h,m,d)
def align_with(target):
target_theta = target
pose.set_pose(0,0,0)
state = STATE_ALIGN
start_time = sys.time()
return ((bump_bits & 7) > 0)
def bump_front_get_value(bump_bits):
return (bump_bits == 129)
def bump_right_get_value(bump_bits):
return ((bump_bits & 224) > 0)
BUMP_TIME_SIDE = 300
BUMP_TIME_ROTATE = 500
BUMP_STATE_IDLE = 0
BUMP_STATE_TURN_RIGHT = 1
BUMP_STATE_TURN_LEFT = 2
BUMP_STATE_ROTATE = 3
bump_time = sys.time()
bump_state = BUMP_STATE_IDLE
def bump_beh(tv):
# act on the information from the message. Note that this might be
# information stored from the last message we received, because message
# information remains active for a while
global bump_time
global bump_state
bump_bits = rone.bump_sensors_get_value()
# check bump sensors
if bump_left_get_value(bump_bits):
bump_state = BUMP_STATE_TURN_RIGHT
bump_time = sys.time() + BUMP_TIME_SIDE
elif bump_right_get_value(bump_bits):
def winter_time_keeper(initial_time):
score_time = sys.time()-initial_time
return score_time
def update():
if sys.time() > _vcstate['update_time']:
_vcstate['update_time'] += _VEL_UPDATE_PERIOD
_velocity('l')
_velocity('r')
def fall():
found_flower = False
start_time = 0
target_theta = 0
my_color = -1
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
color = 'r' #for flowers only
def wander():
state = STATE_WANDER
def collect_pollen():
state = STATE_COLLECT_POLLEN
start_time = sys.time()
def align_with(target):
target_theta = target
pose.set_pose(0,0,0)
state = STATE_ALIGN
start_time = sys.time()
#motion_start_odo = pose.get_odometer()
while True:
beh.init(0.22, 40, 0.5, 0.1)
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
#FINITE STATE MACHINE
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_WANDER: #run forward, avoid direction of neighbors
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
if nav_tower == None:
state = STATE_RETURN_TO_BASE
else:
beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)
(flower, color) = detflower(nbrList)
if flower != None:
state = STATE_MOVE_TO_FLOWER
elif state == STATE_MOVE_TO_FLOWER:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
(flower, color) = detflower(nbrList)
if flower != None:
if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
collect_pollen() #collect pollen if we bump or get close
else:
#otherwise keep following that flower
beh_out = beh.follow_nbr(flower, MOTION_TV)
elif state == STATE_COLLECT_POLLEN:
motion_start_odo = pose.get_odometer()
if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
state = STATE_RETURN_TO_BASE
found_flower = True
elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
tv = -MOTION_TV
rv = 0
beh_out = beh.tvrv(tv,rv)
turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
elif sys.time() < (turn_start_time + TURN_TIME):
tv = 40
rv = -MOTION_RV
beh_out = beh.tvrv(tv,rv)
else:
tv = MOTION_TV
rv = (MOTION_RV - 300)
beh_out = beh.tvrv(tv,rv)
elif state == STATE_RETURN_TO_BASE:
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = find_queen(nbrList)
if (nav_tower == None) and (queen == None):
beh_out = (-MOTION_TV, 0, True)
elif (nav_tower != None) and (queen == None):
beh_out = beh.follow_nbr(nav_tower)
elif neighbors.get_nbr_range_bits(queen) > 2:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif found_flower:
state = STATE_RECRUIT
start_time = sys.time()
else:
state = STATE_FOLLOW
start_time = sys.time()
elif state == STATE_FOLLOW:
recruiter = find_recruiter()
if recruiter == None:
beh_out = beh.BEH_INACTIVE
if sys.time() > (follow_start_time + FOLLOW_TIME):
wander()
else:
bearing = neighbors.get_nbr_bearing(recruiter)
orientation = neighbors.get_nbr_orientation(recruiter)
align_with(math.pi + bearing - orientation)
elif state == STATE_GO:
flower = detflower()
if not flower == None:
state = STATE_MOVE_TO_FLOWER
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_RECRUIT:
if sys.time() > (recruit_start_time + RECRUIT_TIME):
align_with(pose.get_theta() - math.pi)
elif state == STATE_ALIGN:
tv = 0
heading_error = math.normalize_angle(pose.get_theta() - target_theta)
rv = ROTATE_RV_GAIN * heading_error
beh_out = beh.tvrv(tv, rv)
# you could actually do a running average in the list here
small_error = hba.average_error_check(heading_error, [], HEADING_ERROR_LIMIT, new_nbrs)
if new_nbrs:
print "error", error_list
if small_error:
state = STATE_GO
#END OF FINITE STATE MACHINE
bump_beh_out = beh.bump_beh(MOTION_TV)
if state not in [STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT]:
beh_out = beh.subsume([beh_out, bump_beh_out])
beh.motion_set(beh_out)
hba.set_msg(state, my_color, 0)
def summer():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_FIND_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
beh_out = beh.tvrv(MOTION_TV, 0)
queen = get_queen()
if not queen == None:
state = STATE_BUMP_QUEEN
else:
#go straight and hope for the best
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_BUMP_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
queen = get_queen()
if queen == None:
state = STATE_RETURN
else:
if (neighbors.get_nbr_range_bits(queen) > 6) or (beh.bump_angle_get() != None):
state = STATE_BACK_UP
start_time = sys.time()
else:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif state == STATE_BACK_UP:
if sys.time() > start_time + BACK_UP_TIME:
state = STATE_RETURN
else:
beh_out = beh.tvrv(-MOTION_TV, 0)
elif state == STATE_RETURN:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
queen = get_queen()
if queen == None:
state = STATE_IDLE
else:
beh_out = beh.avoid_nbr(queen, MOTION_TV)
elif state == STATE_QUEEN:
if new_nbrs:
print 'Ich bin die Koenigin der welt!'
# end of the FSM
bump_beh_out = beh.bump_beh(MOTION_TV)
if not state == STATE_QUEEN:
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
hba.set_msg(state, 0, 0)
def spring():
at_tree_odo = None
tree_pose = None
followers = 0
have_seen_leader = False
beh.init(0.22, 40, 0.5, 0.1)
motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE, MOTION_RELEASE_DISTANCE
, MOTION_CAPTURE_ANGLE, MOTION_RELEASE_ANGLE)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
if new_nbrs:
print state
beh_out = beh.BEH_INACTIVE
# set colors, because why not do it at the top
color_counts = [0, 0, 0]
for i in range(3):
color_counts[i] = min([5, (state - 5 * i)])
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
elif state == STATE_SUCCESS:
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
else:
leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)
if rone.button_get_value('g'):
tree_pose = None
followers = 0
have_seen_leader = False
state = STATE_IDLE
# this is the main finite-state machine
if not state in [STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW]:
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
start_time = sys.time()
state = STATE_FOLLOW
if state == STATE_IDLE:
if rone.button_get_value('r'):
pose.set_pose(0, 0, 0)
state = STATE_WANDER
elif rone.button_get_value('b'):
state = STATE_QUEEN
elif state == STATE_QUEEN:
pass
elif state == STATE_WANDER:
## leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
nav = hba.find_nav_tower_nbr(NAV_ID)
beh_out = beh.avoid_nbr(nav, MOTION_TV)
queen = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
if bump_front():
if queen != None:
state = STATE_SUCCESS
else:
tree_pose = pose.get_pose()
motion.set_goal((0.0, 0.0), MOTION_TV)
at_tree_odo = pose.get_odometer()
state = STATE_RETURN
elif nav == None:
motion.set_goal((0.0, 0.0), MOTION_TV)
state = STATE_RETURN
elif state == STATE_RETURN:
## nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = None
recruiter = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
elif nbr_state == STATE_RECRUIT:
recruiter = nbr
if queen != None:
if (recruiter == None) and (tree_pose != None) and \
close_to_nbr(queen):
start_time = sys.time()
dist_traveled = pose.get_odometer() - at_tree_odo
at_queen_odo = pose.get_odometer()
state = STATE_RECRUIT
elif not closer_to_nbr(queen):
beh_out = beh.follow_nbr(queen, MOTION_TV)
else:
start_time = sys.time()
state = STATE_FOLLOW
else:
(tv, rv) = motion.update()
beh_out = beh.tvrv(tv, rv)
elif state == STATE_RECRUIT:
new_followers = 0
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
if neighbors.get_nbr_id(nbr) > rone.get_id():
state = STATE_FOLLOW
elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
new_followers += 1
if new_followers > followers:
print 'reset timer'
start_time = sys.time()
followers = max([followers, new_followers])
if followers == 4 or sys.time() > start_time + WAIT_TIME:
tree_pos = (tree_pose[0], tree_pose[1])
motion.set_goal(tree_pos, MOTION_TV)
state = STATE_LEADER
elif state == STATE_FOLLOW:
recruiter = None
leader = None
success = False
new_followers = 1
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
recruiter = nbr
elif nbr_state == STATE_LEADER:
leader = nbr
elif nbr_state == STATE_SUCCESS:
leader = nbr
success = True
elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
new_followers += 1
if success:
have_seen_leader = True
if new_followers > followers:
start_time = sys.time()
followers = max([followers, new_followers])
if recruiter == None:
if leader == None:
if have_seen_leader:
beh_out = beh.tvrv(MOTION_TV, 0)
elif followers == 5 or sys.time() > start_time + WAIT_TIME:
followers = 0
state = STATE_WANDER
else:
if success and bump() and close_to_nbr(leader):
state = STATE_SUCCESS
beh_out = beh.follow_nbr(leader, MOTION_TV)
elif state == STATE_LEADER:
## (tv, rv) = motion.update()
## beh_out = beh.tvrv(tv, rv)
## if motion.is_done():
## state = STATE_SUCCESS
beh_out = beh.tvrv(-MOTION_TV, 0)
if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
state = STATE_SUCCESS
elif state == STATE_SUCCESS:
pass
# end of the FSM
if state not in [STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS, STATE_QUEEN]:
bump_beh_out = beh.bump_beh(MOTION_TV)
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
msg = [0, 0, 0]
msg[MSG_IDX_STATE] = state
hba.set_msg(msg[0], msg[1], msg[2])
#print(mb-ma)
#mb = sys.heap()
#mb = mb[0]
import flightplanstatus
#ma = sys.heap()
#ma = ma[0]
#print('import flightplanstatus')
#print(mb-ma)
#mb = sys.heap()
#mb = mb[0]
import mixersettings
#ma = sys.heap()
#ma = ma[0]
#print('import mixersettings')
#print(mb-ma)
n = 0
timenow = sys.time()
fpStatus = flightplanstatus.FlightPlanStatus()
while n < 120:
n = n + 1
#openpilot.debug(n, timenow)
fpStatus.read()
fpStatus.Debug.value[0] = n
fpStatus.Debug.value[1] = timenow
fpStatus.write()
timenow = openpilot.delayUntil(timenow, 1000)
if openpilot.hasStopRequest():
sys.exit()
def update():
if sys.time() > _vcstate['update_time']:
_vcstate['update_time'] += _VEL_UPDATE_PERIOD
_velocity('l')
_velocity('r')
def init():
_leds_state['color'] = 'r'
_leds_state['pattern'] = 'circle'
_leds_state['brightness'] = 10
_leds_state['counter'] = 0
_leds_state['update_time'] = sys.time()
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 spring():
at_tree_odo = None
tree_pose = None
followers = 0
have_seen_leader = False
beh.init(0.22, 40, 0.5, 0.1)
motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE,
MOTION_RELEASE_DISTANCE, MOTION_CAPTURE_ANGLE,
MOTION_RELEASE_ANGLE)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
if new_nbrs:
print state
beh_out = beh.BEH_INACTIVE
# set colors, because why not do it at the top
color_counts = [0, 0, 0]
for i in range(3):
color_counts[i] = min([5, (state - 5 * i)])
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
elif state == STATE_SUCCESS:
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
else:
leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)
if rone.button_get_value('g'):
tree_pose = None
followers = 0
have_seen_leader = False
state = STATE_IDLE
# this is the main finite-state machine
if not state in [
STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW
]:
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
start_time = sys.time()
state = STATE_FOLLOW
if state == STATE_IDLE:
if rone.button_get_value('r'):
pose.set_pose(0, 0, 0)
state = STATE_WANDER
elif rone.button_get_value('b'):
state = STATE_QUEEN
elif state == STATE_QUEEN:
pass
elif state == STATE_WANDER:
## leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
nav = hba.find_nav_tower_nbr(NAV_ID)
beh_out = beh.avoid_nbr(nav, MOTION_TV)
queen = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
if bump_front():
if queen != None:
state = STATE_SUCCESS
else:
tree_pose = pose.get_pose()
motion.set_goal((0.0, 0.0), MOTION_TV)
at_tree_odo = pose.get_odometer()
state = STATE_RETURN
elif nav == None:
motion.set_goal((0.0, 0.0), MOTION_TV)
state = STATE_RETURN
elif state == STATE_RETURN:
## nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = None
recruiter = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
elif nbr_state == STATE_RECRUIT:
recruiter = nbr
if queen != None:
if (recruiter == None) and (tree_pose != None) and \
close_to_nbr(queen):
start_time = sys.time()
dist_traveled = pose.get_odometer() - at_tree_odo
at_queen_odo = pose.get_odometer()
state = STATE_RECRUIT
elif not closer_to_nbr(queen):
beh_out = beh.follow_nbr(queen, MOTION_TV)
else:
start_time = sys.time()
state = STATE_FOLLOW
else:
(tv, rv) = motion.update()
beh_out = beh.tvrv(tv, rv)
elif state == STATE_RECRUIT:
new_followers = 0
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
if neighbors.get_nbr_id(nbr) > rone.get_id():
state = STATE_FOLLOW
elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
new_followers += 1
if new_followers > followers:
print 'reset timer'
start_time = sys.time()
followers = max([followers, new_followers])
if followers == 4 or sys.time() > start_time + WAIT_TIME:
tree_pos = (tree_pose[0], tree_pose[1])
motion.set_goal(tree_pos, MOTION_TV)
state = STATE_LEADER
elif state == STATE_FOLLOW:
recruiter = None
leader = None
success = False
new_followers = 1
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
recruiter = nbr
elif nbr_state == STATE_LEADER:
leader = nbr
elif nbr_state == STATE_SUCCESS:
leader = nbr
success = True
elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
new_followers += 1
if success:
have_seen_leader = True
if new_followers > followers:
start_time = sys.time()
followers = max([followers, new_followers])
if recruiter == None:
if leader == None:
if have_seen_leader:
beh_out = beh.tvrv(MOTION_TV, 0)
elif followers == 5 or sys.time() > start_time + WAIT_TIME:
followers = 0
state = STATE_WANDER
else:
if success and bump() and close_to_nbr(leader):
state = STATE_SUCCESS
beh_out = beh.follow_nbr(leader, MOTION_TV)
elif state == STATE_LEADER:
## (tv, rv) = motion.update()
## beh_out = beh.tvrv(tv, rv)
## if motion.is_done():
## state = STATE_SUCCESS
beh_out = beh.tvrv(-MOTION_TV, 0)
if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
state = STATE_SUCCESS
elif state == STATE_SUCCESS:
pass
# end of the FSM
if state not in [
STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS,
STATE_QUEEN
]:
bump_beh_out = beh.bump_beh(MOTION_TV)
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
msg = [0, 0, 0]
msg[MSG_IDX_STATE] = state
hba.set_msg(msg[0], msg[1], msg[2])
def summer():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_FIND_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
beh_out = beh.tvrv(MOTION_TV, 0)
queen = get_queen()
if not queen == None:
state = STATE_BUMP_QUEEN
else:
#go straight and hope for the best
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_BUMP_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
queen = get_queen()
if queen == None:
state = STATE_RETURN
else:
if (neighbors.get_nbr_range_bits(queen) >
6) or (beh.bump_angle_get() != None):
state = STATE_BACK_UP
start_time = sys.time()
else:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif state == STATE_BACK_UP:
if sys.time() > start_time + BACK_UP_TIME:
state = STATE_RETURN
else:
beh_out = beh.tvrv(-MOTION_TV, 0)
elif state == STATE_RETURN:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
queen = get_queen()
if queen == None:
state = STATE_IDLE
else:
beh_out = beh.avoid_nbr(queen, MOTION_TV)
elif state == STATE_QUEEN:
if new_nbrs:
print 'Ich bin die Koenigin der welt!'
# end of the FSM
bump_beh_out = beh.bump_beh(MOTION_TV)
if not state == STATE_QUEEN:
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
hba.set_msg(state, 0, 0)
def winter_time_keeper(initial_time):
score_time = sys.time() - initial_time
return score_time
def time(): return sys.time()
