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 __init__(self):
if arduino.get_bump()[1]:
self.sign = -1
elif arduino.get_bump()[0]:
self.sign = 1
else:
self.sign = np.sign(arduino.get_ir()[2] - arduino.get_ir()[1])
self.midtime = time.time() + constants.herp_derp_first_time
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 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 __init__(self):
variables.ball_attempts += 1
triggered_bump = np.where(arduino.get_bump())[0]
triggered_ir = np.where(np.array(arduino.get_ir()) > constants.ir_stuck_threshold)[0]
if (np.random.rand() < constants.probability_to_use_bump
or not len(triggered_ir)) and len(triggered_bump):
# only bump sensors are triggered, or both types are triggered and we randomly chose to look at bump
choice = random.choice(triggered_bump)
self.trigger_released = lambda: (not arduino.get_bump()[choice])
self.escape_angle = constants.bump_sensor_angles[choice] # randomly pick a triggered bump sensor
elif len(triggered_ir):
# only IR senors are triggered, or both types are triggered and we randomly chose to look at IR
choice = random.choice(triggered_ir)
self.trigger_released = lambda: (arduino.get_ir()[choice] < constants.ir_unstuck_threshold)
self.escape_angle = constants.ir_sensor_angles[choice] # randomly pick a triggered IR sensor
else:
self.escape_angle = None # oops, not good style
if self.escape_angle is not None:
self.escape_angle += random.triangular(-constants.unstick_angle_offset_range, constants.unstick_angle_offset_range) # add some randomness
self.unstick_complete = False
self.stop_time = time.time() + constants.unstick_wiggle_period
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 next(self, time_left):
"""Superclass method to execute appropriate event handlers and actions."""
irs = arduino.get_ir()
if any(arduino.get_bump()[0:2]) or irs[1] > 1 or irs[2] > 1:
return self.on_block()
elif any(arduino.get_bump()[2:4]) or irs[0] > 1 or irs[3] > 1:
return self.on_stuck()
elif time_left < constants.dump_time and kinect.yellow_walls:
return self.on_yellow()
elif (time_left >= constants.dump_time and kinect.balls
and not variables.ignore_balls
and not variables.ignore_balls_until_yellow):
return self.on_ball()
return self.default_action()
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)
