def default_action(self): if (variables.can_follow_walls and variables.ignore_balls and not variables.yellow_stalk_period): return GoToWall() if kinect.yellow_walls: # safe to go here because default_action() will be called at least once before on_timeout() self.saw_yellow = True arduino.drive(0, self.turn)
def default_action(self): arduino.drive(0, self.turn * constants.look_around_speed) if arduino.get_ir()[self.ir] > constants.wall_follow_limit: # ok because of state C if self.turning_away: return LookAway(turning_away = False) else: return FollowWall()
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 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 on_yellow(self): wall = max(kinect.yellow_walls, key = lambda wall: wall['size']) offset = constants.yellow_follow_kp * (wall['col'][0] - 80) arduino.drive(max(0, constants.drive_speed - abs(offset)), offset) if (max(arduino.get_ir()[1:-1]) > constants.dump_ir_threshhold and wall['size'] > 3500): return maneuvering.DumpBalls(final = abs(wall['col'][0] - 80) < constants.wall_center_tolerance)
def on_ball(self): ball = max(kinect.balls, key = lambda ball: ball['size']) if ball['size'] > self.size * constants.ball_stuck_ratio: self.non_herp_time = time.time() if time.time() - self.non_herp_time > constants.herp_derp_timeout: return maneuvering.HerpDerp() self.size = .9 * self.size + .1 * ball['size'] offset = constants.ball_follow_kp * (ball['col'][0] - 80) arduino.drive(max(0, constants.drive_speed - abs(offset)), offset) if ball['row'][0] > constants.close_ball_row: return maneuvering.SnarfBall()
def drive_away(self): """ We're hitting an obstacle at angle (front of the robot is 0, positive is clockwise) and need to escape. Periodically reverse the direction (as determined by self.reverse). For angles 0 or pi, you want full forward and no turn. For angles pi/2 and 3pi/2, you want no forward and only turn. This suggests using trig functions, though maybe there's a better implementation. Note that escape_angle is the angle of the sensor, but the trig functions rely on having the angle you want to drive, so add pi. """ drive = np.cos(self.escape_angle + np.pi) turn = constants.unstick_max_turn * np.sin(self.escape_angle + np.pi) arduino.drive(drive, turn)
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 follow(self): side_ir = arduino.get_ir()[3] # hard-coded to follow on right p = constants.wall_follow_dist - side_ir d = (p - self.last_p) if self.last_p else 0 dd = (d - self.last_d) if self.last_d else 0 self.last_p, self.last_d = p, d if time.time() - self.time_wall_seen > constants.lost_wall_timeout: return GoToWall() elif ((max(arduino.get_ir()[1:-1]) > constants.wall_follow_limit and (time.time() - self.time_wall_absent) > constants.lost_wall_timeout) or self.turning_away): # too close in front self.turning_away = True self.time_wall_seen = time.time() drive = 0 turn = constants.wall_follow_turn * -1 arduino.drive(drive, turn) if (max(arduino.get_ir()[1:-1]) < constants.wall_follow_limit and side_ir > constants.wall_follow_limit): self.turning_away = False elif side_ir > constants.wall_follow_limit: # if we see a wall self.time_wall_seen = time.time() drive = constants.wall_follow_drive turn = (constants.wall_follow_kp * p + constants.wall_follow_kd * d + constants.wall_follow_kdd * dd) arduino.drive(drive, turn) else: # lost wall but not timed out, so turn into the wall self.time_wall_absent = time.time() drive = .2 # TODO kludge turn = constants.wall_follow_turn arduino.drive(drive, turn)
def follow(self): side_ir = arduino.get_ir()[3] # hard-coded to follow on right p = constants.wall_follow_dist - side_ir d = (p - self.last_p) if self.last_p else 0 dd = (d - self.last_d) if self.last_d else 0 self.last_p, self.last_d = p, d if time.time() - self.time_wall_seen > constants.lost_wall_timeout: return navigation.LookAround() elif ( max(arduino.get_ir()[1:-1]) > constants.wall_follow_limit and (time.time() - self.time_wall_absent) > constants.lost_wall_timeout ) or self.turning_away: # too close in front self.turning_away = True self.time_wall_seen = time.time() drive = 0 turn = constants.wall_follow_turn * -1 arduino.drive(drive, turn) # print("A {d: 4.2f} {t: 4.2f} {ir: 4.2f}".format(d=drive, t=turn, ir=side_ir)) if max(arduino.get_ir()[1:-1]) < constants.wall_follow_limit and side_ir > constants.wall_follow_limit: self.turning_away = False elif side_ir > constants.wall_follow_limit: # if we see a wall self.time_wall_seen = time.time() drive = constants.wall_follow_drive turn = constants.wall_follow_kp * p + constants.wall_follow_kd * d + constants.wall_follow_kdd * dd arduino.drive(drive, turn) # print("B {d: 4.2f} {t: 4.2f} {ir: 4.2f}".format(d=drive, t=turn, ir=side_ir)) else: # lost wall but not timed out, so turn into the wall self.time_wall_absent = time.time() drive = 0 turn = constants.wall_follow_turn arduino.drive(drive, turn)
def next(self, time_left): # override next so nothing can interrupt a dump fl, fr = arduino.get_ir()[1:-1] if min(fl, fr) > constants.dump_ir_final: arduino.drive(0, 0) if not self.final: return ConfirmLinedUp() else: return WaitInSilence() elif abs(fl - fr) > constants.dump_ir_turn_tol: arduino.drive(0, np.sign(fr - fl) * constants.dump_turn_speed) else: arduino.drive(constants.dump_fwd_speed, 0)
def default_action(self): if max(arduino.get_ir()) > constants.wall_follow_dist: arduino.drive(0, 0) return FollowWall() arduino.drive(constants.drive_speed, 0)
def next(self, time_left): # override next so nothing can interrupt a HappyDance arduino.set_door(True) if time.time() > self.next_shake: self.next_shake = time.time() + constants.dance_period self.shake_dir *= -1 arduino.drive(0, self.shake_dir * constants.dance_turn)
def next(self, time_left): # override next because we snarf no matter what arduino.drive(constants.snarf_speed, 0)
def next(self, time_left): arduino.drive(-constants.dump_fwd_speed, 0)
def next(self, time_left): arduino.drive(0, 0) if time_left < constants.eject_time: return HappyDance()
def next(self, time_left): # don't do anything else if time.time() < self.midtime: arduino.drive(constants.herp_derp_first_drive, self.sign * constants.herp_derp_first_turn) else: arduino.drive(0, -1 * self.sign * constants.herp_derp_second_turn)
def default_action(self): arduino.drive(constants.drive_speed, 0)
#!/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)