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 run(): global want_change print ("starting wall_follow_test.py") state = FollowWallTest() fake_time_left = 180 while True: if want_change: want_change = False arduino.drive(0, 0) print "Enter kp, kd, and kdd, separated by spaces" s = raw_input("> ") if s == "": exit() s = s.split(" ") constants.wall_follow_kp = float(s[0]) constants.wall_follow_kd = float(s[1]) constants.wall_follow_kdd = float(s[2]) kinect.process_frame() try: new_state = state.next(fake_time_left) if new_state is not None: # if the state has changed state = new_state print ("{0} with {1} seconds to go".format(state, fake_time_left)) except Exception, ex: print ("{0} while attempting to change states".format(ex)) traceback.print_exc(file=sys.stdout) if not isinstance(state, FollowWallTest): print ("Back to FollowWallTest") state = FollowWallTest()
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 show_video(): kinect.process_frame() color.colorize(kinect.image, kinect.constants, kinect.colors) for ball in kinect.balls: cv.Circle( cv.fromarray(kinect.image), (int(ball["col"][0] + 0.5), int(ball["row"][0] + 0.5)), int(1.5 + np.sqrt(ball["size"] / 3.14)), (0, 0, 255), ) for wall in kinect.yellow_walls: cx, wx = wall["col"][0], width(wall["col"][1]) / 2 cy, wy = wall["row"][0], width(wall["row"][1]) / 2 cv.Rectangle( cv.fromarray(kinect.image), (int(cx - wx + 0.5), int(cy - wy + 0.5)), (int(cx + wx + 0.5), int(cy + wy + 0.5)), (0, 0, 255), ) cv.CvtColor(cv.fromarray(kinect.image), cv.fromarray(kinect.image), cv.CV_HSV2BGR) cv.ShowImage("Video", cv.fromarray(kinect.image)) cv.ShowImage("Depth", cv.fromarray(kinect.depth << 5))
def spin(): for i in range(10): t = time.time() for i in range(20): kinect.process_frame() print(20 / (time.time() - t))