def initial_state(robot: Robot) -> State:
return State(
zone=robot.zone,
heading_pid=PIDController(
full_deflection_error=math.radians(120),
prediction_time=0.3,
fine_tune_time=2.0,
time=lambda: robot.time(),
),
captured=frozenset(),
uncapturable=frozenset(),
current_target=None,
current_zone=None,
zone_history=[],
kalman=KalmanFilter(
initial_position=(
Location(x=-7, y=0)
if robot.zone == 0
else Location(x=7, y=0)
),
initial_heading=(
math.radians(90)
if robot.zone == 0
else math.radians(270)
),
),
kalman_time=robot.time(),
num_captures={
x: 0
for x in StationCode
},
)
def update_state_from_view(robot: Robot, state: State, view: View) -> None:
# Update captured
new_captured = set(state.captured)
for target in view.targets:
if target.target_info.owned_by == state.zone:
new_captured.add(target.target_info.station_code)
else:
if not NERF_MODE:
# In nerf mode we pretend we're unaware of zones taken away from us
new_captured.discard(target.target_info.station_code)
state.captured = new_captured
# Zone updating
zone_list = list(state.zone_history)
zone_list.append(get_zone(state.kalman.location))
if len(zone_list) > 4:
zone_list = zone_list[-4:]
state.zone_history = zone_list
zones = collections.Counter(zone_list)
zones.update([zone_list[-1]])
(element, num_instances), = zones.most_common(1)
if num_instances > 1:
state.current_zone = element
else:
state.current_zone = None
time = robot.time()
state.kalman.tick(
dt=time - state.kalman_time,
left_power=robot.motors[0].m0.power,
right_power=robot.motors[0].m1.power,
)
state.kalman.update_heading(view.compass)
for target in view.targets:
state.kalman.update_location(
location=single_target_position(state.kalman.heading, target),
stdev=0.08,
)
state.kalman_time = time
print(f"Position is {state.kalman.location.x:.3f}, {state.kalman.location.y:.3f} ±{state.kalman.location_error:.3f}m")
print(f"Heading is {math.degrees(state.kalman.heading):.0f}° ±{math.degrees(state.kalman.heading_error):.0f}°")
def get_world_view(R: Robot) -> View:
compass = R.compass.get_heading()
targets = R.radio.sweep()
left_distance = R.ruggeduinos[0].analogue_read(0)
right_distance = R.ruggeduinos[0].analogue_read(1)
proximity = (
left_distance < 0.05 or # Front left ultrasound
right_distance < 0.05 or # Front right ultrasound
R.ruggeduinos[0].digital_read(2) # Front bump switch
)
return View(
compass=compass,
targets=list(targets),
proximity=proximity,
left_distance=left_distance,
right_distance=right_distance,
dropped=R.time() > 60.0,
)