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()
