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)
