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)