def main(): """ Robot will use two corners on one side of the wall in the arena. It will try to put all 4 tokens into slots. This gives us 9 points. """ option = 2 robot = Robot() reset_log(robot) robot.sound = Sound(robot, USING_SOUND) robot.sound.play('R2D2') robot.position = Tracker(robot.zone) set_pins(robot) slots_x = 2.91 if robot.zone in [0, 3] else 5.09 target_theta = pi / 2 if robot.zone in [0, 3] else 1.5 * pi if robot.zone in [0, 1]: dy = 0.9 slot_y_0 = 2.65 else: dy = -0.9 slot_y_0 = 5.65 while 1: try: if option == 1: put_down(robot) grab(robot) token_to_slot(robot, robot.zone) for i in range(4): zone = robot.zone if i < 2 else 3 - robot.zone has_token = get_token_from_corner(robot, zone) if has_token: token_to_slot(robot, zone) elif option == 2: put_down(robot) grab(robot) for i in range(4): zone = robot.zone if i < 2 else 3 - robot.zone if i == 0: move_to_point(robot, slots_x, slot_y_0 + 0.9, target_theta) token_to_slot_2(robot) elif i == 1: move_to_point(robot, 2, 2) get_token_from_corner(robot, zone) move_to_point(robot, slots_x, slot_y_0, target_theta) token_to_slot_2(robot) else: move_to_point(robot, 2, 2) get_token_from_corner(robot, zone) slot_y = slot_y_0 + dy * i move_to_point(robot, slots_x, slot_y, target_theta) token_to_slot_2(robot) except: print_exc() restart(robot)
def token_to_slot_2(robot): """ Assumes robot is near the slot with the token already. """ markers = robot.see(res=RESOLUTION) if not markers: for i in range(3): markers = robot.see(res=RESOLUTION) if markers: break for marker in markers: if marker.info.code in range(32, 40): line_up_to_marker(robot, marker, 0.4) sleep(0.2) put_down(robot) sleep(0.4) move_straight(robot, -0.6) break # Return True? elif marker.info.code in range(40, 52): # This is unlikely to happen at the beginning # of the competition/match. pass # Return False? # Check if it's in a slot. # If it's not our take it out? put_down(robot)
def main(): """ Robot will use two corners on one side of the wall in the arena. It will try to put all 4 tokens into slots. This gives us 9 points. """ option = 2 robot = Robot() reset_log(robot) robot.sound = Sound(robot, USING_SOUND) robot.sound.play('R2D2') robot.position = Tracker(robot.zone) set_pins(robot) slots_x = 2.91 if robot.zone in [0, 3] else 5.09 target_theta = pi/2 if robot.zone in [0, 3] else 1.5*pi if robot.zone in [0, 1]: dy = 0.9 slot_y_0 = 2.65 else: dy = -0.9 slot_y_0 = 5.65 while 1: try: if option == 1: put_down(robot) grab(robot) token_to_slot(robot, robot.zone) for i in range(4): zone = robot.zone if i < 2 else 3-robot.zone has_token = get_token_from_corner(robot, zone) if has_token: token_to_slot(robot, zone) elif option == 2: put_down(robot) grab(robot) for i in range(4): zone = robot.zone if i < 2 else 3-robot.zone if i == 0: move_to_point(robot, slots_x, slot_y_0+0.9, target_theta) token_to_slot_2(robot) elif i == 1: move_to_point(robot, 2, 2) get_token_from_corner(robot, zone) move_to_point(robot, slots_x, slot_y_0, target_theta) token_to_slot_2(robot) else: move_to_point(robot, 2, 2) get_token_from_corner(robot, zone) slot_y = slot_y_0 + dy*i move_to_point(robot, slots_x, slot_y, target_theta) token_to_slot_2(robot) except: print_exc() restart(robot)
def main_test(robot): while True: robot.power.led[0] = 1 grab(robot) robot.power.led[0] = 0 sleep(2) robot.power.led[1] = 1 put_down(robot) robot.power.led[1] = 0 sleep(2) print "Cycled!"
def get_token_from_corner(robot, zone): """ Moves to specified corner, finds a marker and picks it up. """ log(robot, "Attempting to get token from corner of zone %d..." % (zone)) token_marker = look_for_token(robot, zone) if token_marker: line_up_to_marker(robot, token_marker) robot.sound.play('Heart') put_down(robot) move_till_touch(robot) grab(robot) robot.sound.stop() return True else: log(robot, "No tokens found.") return False
def token_to_slot(robot, slot): """ Moves robot near the slot, and places token on shelf. """ log(robot, "Moving to zone start point...") slot_x = SLOT_POINTS[slot][0] slot_y = SLOT_POINTS[slot][1] slot_theta = 3 * pi / 2 if slot in [1, 2] else pi / 2 move_to_point(robot, slot_x, slot_y, slot_theta) log(robot, "Scanning for slot markers...") robot.sound.play('Radar') markers = robot.see(res=RESOLUTION) if not markers: for i in range(3): markers = robot.see(res=RESOLUTION) if markers: break found_Marker = False for marker in markers: if marker.info.code in range(32, 40): log(robot, "Found Token Marker:" + str(marker.info.code)) found_Marker = True robot.sound.stop() line_up_to_marker(robot, marker, 0.4) sleep(0.2) put_down(robot) sleep(0.4) break if not found_Marker: robot.sound.stop() log(robot, "Marker Not Detected.") move_straight(robot, 0.4) put_down(robot) sleep(0.4) log(robot, "Moving away from marker.") move_straight(robot, -0.5) grab(robot)
def token_to_slot(robot, slot): """ Moves robot near the slot, and places token on shelf. """ log(robot, "Moving to zone start point...") slot_x = SLOT_POINTS[slot][0] slot_y = SLOT_POINTS[slot][1] slot_theta = 3 * pi/2 if slot in [1, 2] else pi/2 move_to_point(robot, slot_x, slot_y, slot_theta) log(robot, "Scanning for slot markers...") robot.sound.play('Radar') markers = robot.see(res=RESOLUTION) if not markers: for i in range(3): markers = robot.see(res=RESOLUTION) if markers: break found_Marker = False for marker in markers: if marker.info.code in range(32, 40): log(robot, "Found Token Marker:" + str(marker.info.code)) found_Marker = True robot.sound.stop() line_up_to_marker(robot, marker, 0.4) sleep(0.2) put_down(robot) sleep(0.4) break if not found_Marker: robot.sound.stop() log(robot, "Marker Not Detected.") move_straight(robot, 0.4) put_down(robot) sleep(0.4) log(robot, "Moving away from marker.") move_straight(robot, -0.5) grab(robot)