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 perform(self, robot: Robot, state: State, view: View) -> None:
heading_error = self.relative_bearing(state, view)
heading_error = (math.pi + heading_error) % math.tau - math.pi
print(f" RB is {math.degrees(heading_error)}°", end='')
# Add some pseudo heading error if the proximity sensors are going off
left_distance = view.left_distance
right_distance = view.right_distance
# Detect towers
if not NERF_MODE:
for station in StationCode:
station_position = STATION_CODE_LOCATIONS[station]
distance = math.hypot(
station_position.x - state.kalman.location.x,
station_position.y - state.kalman.location.y,
)
distance = max(0, distance - TOWER_RADIUS)
if distance > STEERING_THRESHOLD_METRES:
continue
absolute_bearing = math.atan2(
station_position.x - state.kalman.location.x,
station_position.y - state.kalman.location.y,
) % math.tau
relative_bearing = absolute_bearing - state.kalman.heading
#print(f"Proximity to {station.value}, range is {distance:.03f}m, relative {math.degrees(relative_bearing):.0f}°")
if 0 < relative_bearing < TOWER_ANGLE:
right_distance = min(right_distance, distance)
elif -TOWER_ANGLE < relative_bearing <= 0:
left_distance = min(left_distance, distance)
turn_back = False
if view.left_distance < STEERING_THRESHOLD_METRES:
heading_error += math.tan(TOWER_CLEARANCE_DISTANCE /
view.left_distance)
turn_back = True
if view.right_distance < STEERING_THRESHOLD_METRES:
heading_error -= math.tan(TOWER_CLEARANCE_DISTANCE /
view.right_distance)
turn_back = True
if turn_back:
print(f", steering {math.degrees(heading_error)}°", end='')
if -IN_PLACE_THRESHOLD < heading_error < IN_PLACE_THRESHOLD:
print("... moving ahead")
deflection = state.heading_pid.step(heading_error)
drive(robot, FORWARD_POWER,
FULL_DEFLECTION_TURN_RATE_PER_SECOND * deflection)
elif heading_error > 0:
print("... turning right")
drive(robot, -0.2 if turn_back else 0.2,
IN_PLACE_TURN_RATE_PER_SECOND)
elif heading_error < 0:
print("... turning left")
drive(robot, -0.2 if turn_back else 0.2,
-IN_PLACE_TURN_RATE_PER_SECOND)
robot.sleep(1 / 100)
def run(robot: Robot) -> None:
state = initial_state(robot)
last_action = ''
last_zone = None
while True:
view = get_world_view(robot)
#print(view)
update_state_from_view(robot, state, view)
action = choose_action(robot, state, view)
action_desc = str(action)
if action_desc != last_action:
print("Action: ", action_desc)
last_action = action_desc
if state.current_zone != last_zone:
print("Zone: ", state.current_zone, state.kalman.location)
last_zone = state.current_zone
action.perform(robot, state, view)
robot.sleep(0.01)
def setup():
global R
R = Robot.setup()
R.init()
set_time(R.usbkey)
logger = setup_logger(R.usbkey)
logger.info('Battery Voltage: %.2f' % R.power.battery.voltage)
R.wait_start()
try:
io = IOInterface(R)
except:
logger.exception("IOInterface could not initialize")
raise
return logger, io, R.zone
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 setup():
# A bad way to detect whether running in simulator
if sys.platform.startswith('win'):
### SIMULATOR ONLY ###
SRBot.zone, SRBot.sim = getInfo() # I made this to make simlator work
srBot = SRBot.setup()
srBot.init()
set_time(srBot.usbkey)
logger = setup_logger(srBot.usbkey)
logger.info('Battery Voltage: %.2f' % (srBot.power.battery.voltage))
srBot.wait_start()
try:
from main import Robot
robot = Robot(srBot)
except:
logger.exception("Robot could not initialize")
raise
return robot, srBot.zone
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,
)
def setup():
# A bad way to detect whether running in simulator
if sys.platform.startswith('win'):
### SIMULATOR ONLY ###
SRBot.zone, SRBot.sim = getInfo() # I made this to make simlator work
srBot = SRBot.setup()
srBot.init()
set_time(srBot.usbkey)
logger = setup_logger(srBot.usbkey)
logger.info('Battery Voltage: %.2f' % (srBot.power.battery.voltage))
srBot.wait_start()
try:
from main import Robot
robot = Robot(srBot)
except:
logger.exception("Robot could not initialize")
raise
return robot, srBot.zone
def perform(self, robot: Robot, state: State, view: View) -> None:
drive(robot, 0)
robot.radio.claim_territory()
new_targets = robot.radio.sweep()
successful = False
for target in new_targets:
if target.target_info.station_code == self.station:
if target.target_info.owned_by == state.zone:
successful = True
break
if successful:
# We claimed this one, mark all downstreams as now capturable
new_uncapturable = set(state.uncapturable)
new_uncapturable.discard(self.station)
for successor, predecessors in PREDECESSORS[Claimant(
robot.zone)].items():
if predecessors is None:
continue
if self.station in predecessors:
new_uncapturable.discard(successor)
new_cap_count = dict(state.num_captures)
new_cap_count[self.station] += 1
state.uncapturable = frozenset(new_uncapturable)
state.num_captures = new_cap_count
else:
# We failed to claim this one, assume that all predecessors became unowned
predecessors = PREDECESSORS[Claimant(robot.zone)][self.station]
if predecessors is None:
new_owned = state.captured
else:
new_owned = frozenset(state.captured - set(predecessors))
state.uncapturable = state.uncapturable | {self.station}
state.captured = new_owned
drive(robot, -0.5)
robot.sleep(0.2)
if NERF_MODE:
if random.random() < 0.5:
drive(robot, 0, math.radians(90) / 3)
else:
drive(robot, 0, -math.radians(90) / 3)
robot.sleep(3)
drive(robot, 0)
robot.sleep(1)
from sr.robot import Robot
import time
multiplier_left = 1
multiplier_right = 0.92
distance_mps = 1.0 / 6.4
angle_dps = 360.0 / 8.5
R = Robot()
def move(distance):
multiplier = 1
if distance < 0:
multiplier = -1
R.motors[0].m0.power = multiplier_left * 30 * multiplier
R.motors[0].m1.power = multiplier_right * -30 * multiplier
time.sleep(abs(distance / distance_mps))
R.motors[0].m0.power = 0
R.motors[0].m1.power = 0
def turn(degrees):
multiplier = 1
if degrees < 0:
multiplier = -1
R.motors[0].m0.power = multiplier_left * 30 * multiplier
R.motors[0].m1.power = multiplier_right * 30 * multiplier
time.sleep(abs(degrees / angle_dps))
R.motors[0].m0.power = 0
R.motors[0].m1.power = 0
def choose_action(robot: Robot, state: State, view: View) -> Action:
if NERF_MODE:
# Artificial stupidity
robot.sleep(0.2)
# Consider immediate claim targets, where we're already within the territory
for target in view.targets:
if 1.0 / target.signal_strength > 0.22:
continue
if target.target_info.owned_by == robot.zone:
#print(f"> Considered {target.target_info.station_code} but we already own it")
continue
if not is_capturable(state.zone, target.target_info.station_code,
state.captured):
print(
f"> Considered {target.target_info.station_code} but we believe we're missing a predecessor"
)
continue
if target.target_info.station_code in state.uncapturable:
print(
f"> Considered {target.target_info.station_code} but skipping due to previous difficulties"
)
continue
return ClaimImmediate(target.target_info.station_code)
# Back off if we're in proximity
if view.proximity:
if random.random() < 0.95:
return BackOff()
else:
return MoveRandomly()
if (state.current_target is not None
and state.current_target not in state.captured
and state.current_target not in state.uncapturable and
is_capturable(state.zone, state.current_target, state.captured)):
target = state.current_target
else:
target = choose_next_target(
state.zone,
state.captured,
state.uncapturable,
from_location=state.kalman.location,
dropped=view.dropped,
pseudo_distances={
x: 0.7 * y
for x, y in state.num_captures.items()
},
)
state.current_target = target
next_hop, route_direct = get_next_hop(get_zone(state.kalman.location),
get_station_location(target),
view.dropped)
if route_direct:
return GotoStation(target)
else:
print(f"Routing to {target} via {next_hop}")
return GotoLocation(ZONE_CENTRES[next_hop])
from albot.main import run from sr.robot import Robot robot = Robot() run(robot)
def perform(self, robot: Robot, state: State, view: View) -> None:
drive(robot, random.random() - 0.25, 0.25 * (random.random() - 0.5))
robot.sleep(0.2 + random.random() * 1.3)
def perform(self, robot: Robot, state: State, view: View) -> State:
drive(robot, -0.6,
IN_PLACE_TURN_RATE_PER_SECOND * (0.15 * random.random() + 0.4))
robot.sleep(0.4)