def run(duration = 180):
import navigation
print("ready to go: waiting for switch")
arduino.set_led(True)
initial_switch = arduino.get_switch()
while arduino.get_switch() == initial_switch:
time.sleep(.02) # check every 20 ms
arduino.set_led(False)
stop_time = time.time() + duration
state = navigation.LookAround()
timeout_time = time.time() + state.timeout
variables.helix_enabled = True
helix_on = True
next_helix_twiddle = duration - constants.helix_twiddle_period[not helix_on]
arduino.set_helix(True)
arduino.set_sucker(True)
time.sleep(.3)
while time.time() < stop_time:
kinect.process_frame()
time_left = stop_time - time.time()
try: # sometimes this throws a NoneType exception, so let's catch it to be safe
new_balls = arduino.get_new_ball_count()
except Exception, ex:
new_balls = 0
print("{0} while attempting to get new ball count".format(ex))
variables.number_possessed_balls += new_balls
if new_balls:
print("{0} NEW BALLS, now {1} balls total with {2} seconds to go".format(new_balls, variables.number_possessed_balls, time_left))
variables.ball_attempts = 0
if not variables.ignore_balls and variables.ball_attempts >= constants.max_ball_attempts:
variables.ignore_balls = True
end_ignore_balls = time_left - random.uniform(.5, 1)*constants.ignore_balls_length
variables.ball_attempts = 0
if variables.ignore_balls and time_left < end_ignore_balls:
variables.ignore_balls = False
if kinect.yellow_walls:
variables.ignore_balls_until_yellow = False
if variables.number_possessed_balls >= constants.max_balls_to_possess:
variables.helix_enabled = False
arduino.set_helix(False) # possess future balls in the lower level
if variables.helix_enabled and time_left < next_helix_twiddle:
helix_on = not helix_on
arduino.set_helix(helix_on)
next_helix_twiddle = time_left - constants.helix_twiddle_period[not helix_on]
variables.yellow_stalk_period = (time_left < constants.yellow_stalk_time or
variables.number_possessed_balls >= constants.min_balls_to_stalk_yellow)
if kinect.yellow_walls and variables.yellow_stalk_period:
variables.can_follow_walls = False
try:
new_state = (state.on_timeout() if time.time() > timeout_time
else state.next(time_left))
if new_state is not None: # if the state has changed
state = new_state
timeout_time = time.time() + state.timeout
# TODO remove the {2} attempts
print("{0} with {1} seconds to go, ({2}, {3}, {4})".format(state, time_left, variables.ball_attempts, variables.can_follow_walls, variables.ignore_balls))
except Exception, ex:
print("{0} while attempting to change states".format(ex))
def kill(*args):
arduino.set_led(False)
arduino.drive(0, 0)
arduino.set_sucker(False)
arduino.set_helix(False)
arduino.set_door(False)
exit()
def run():
global want_change
print("starting state_tester.py")
state = navigation.LookAround()
arduino.set_helix(True)
arduino.set_sucker(True)
stop_time = time.time() + 180
timeout_time = time.time() + state.timeout
while time.time() < stop_time:
if want_change:
want_change = False
arduino.drive(0, 0)
print "Enter a state constructor (with no spaces) and, optionally, a time left (separated by a space), or enter nothing to quit"
s = raw_input("> ")
if s == "":
kill()
s = s.split(" ")
if len(s) > 1:
stop_time = time.time() + int(s[1])
new_state = None
for class_name in ["navigation", "maneuvering"]:
try:
new_state = eval(class_name + "." + s[0])
break
except AttributeError:
continue
if new_state is None:
print("{0} was not found in any of the classes".format(s[0]))
else:
state = new_state
timeout_time = time.time() + state.timeout
print("State manually changed to {0}".format(state))
kinect.process_frame()
try:
new_state = (state.on_timeout() if time.time() > timeout_time
else state.next(stop_time - time.time()))
if new_state is not None: # if the state has changed
state = new_state
timeout_time = time.time() + state.timeout
print("{0} with {1} seconds to go".format(state, stop_time - time.time()))
except Exception, ex:
print("{0} while attempting to change states".format(ex))
traceback.print_exc(file=sys.stdout)
def __init__(self):
arduino.set_helix(False)
arduino.set_sucker(False)
variables.helix_enabled = False
#!/usr/bin/python2.7 import arduino, time time.sleep(1) arduino.drive(0, 0) arduino.set_sucker(False) arduino.set_helix(False) arduino.set_door(False)