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 move_to_point(robot, x, y, target_theta=None, smart=False): """ Given the robot's current tracked position, moves to point (x, y), where x and y are metres from the origin. """ if not valid_point(x, y): log(robot, 'Cant go there') return False log(robot, "Moving to point x=%.1f y=%.1f" % (x, y)) dist, angle = directions_for_point(robot, x, y) robot.sound.play('Valkyries') log(robot, "dist=%.1f angle=%.1f" % (dist, angle)) turn(robot, angle) # if smart and not avoid_obstacles(robot, x, y, target_theta): move_straight(robot, dist) if target_theta is not None: d_theta = target_theta - robot.position.theta if d_theta > pi: d_theta -= pi + pi elif d_theta < -pi: d_theta += pi + pi print(d_theta) turn(robot, d_theta) log(robot, "done.") robot.sound.stop() return True
def move_to_point(robot, x, y, target_theta=None, smart=False): """ Given the robot's current tracked position, moves to point (x, y), where x and y are metres from the origin. """ if not valid_point(x, y): log(robot, 'Cant go there') return False log(robot, "Moving to point x=%.1f y=%.1f" % (x, y)) dist, angle = directions_for_point(robot, x, y) robot.sound.play('Valkyries') log(robot, "dist=%.1f angle=%.1f" % (dist, angle)) turn(robot, angle) # if smart and not avoid_obstacles(robot, x, y, target_theta): move_straight(robot, dist) if target_theta is not None: d_theta = target_theta - robot.position.theta if d_theta > pi: d_theta -= pi+pi elif d_theta < -pi: d_theta += pi+pi print(d_theta) turn(robot, d_theta) log(robot, "done.") robot.sound.stop() return True
def line_up_to_marker(robot, marker, dist=0.4): """ Moves the robot 'dist' metres in front of a given marker. """ log(robot, "Lining up to marker:") robot.sound.play('Valkyries') dist, angle1, angle2 = directions_for_marker(marker, d=dist) log(robot, "dist=%.2f, angle1=%.2f, angle2=%.2f" % (dist, angle1, angle2)) turn(robot, angle1) move_straight(robot, dist) turn(robot, angle2) log(robot, "Lined up to Marker.") robot.sound.stop()
def approx(robot): z = robot.zone angles = [pi / 6, pi / 3, pi / 2] distances = [1, 0.5, 0.3, 0.1, 0.5] if z == 0: for phi in angles: for i in range(3): for j in [1, -1]: print 'turning %.2f' % phi turn(robot, phi * j) sleep(5) else: for dist in distances: for i in [1, -1]: print 'moving %.2f' % dist * i move_straight(robot, dist * i) sleep(5)
def roll_marker(robot, option): if option == 1: raise_arms(robot, pos=65) sleep(0.4) move_straight(robot, -0.1) elif option == 2: raise_arms(robot, pos=60) sleep(1) retract_arms(robot, time_limit=0.8) sleep(0.7) open_arms(robot, pos=45) sleep(0.7) raise_arms(robot, pos=50) sleep(0.2) extend_arms(robot) elif option == 3: pass
def approx(robot): z = robot.zone angles = [pi/6, pi/3, pi/2] distances = [1, 0.5, 0.3, 0.1, 0.5] if z == 0: for phi in angles: for i in range(3): for j in [1, -1]: print 'turning %.2f' % phi turn(robot, phi*j) sleep(5) else: for dist in distances: for i in [1, -1]: print 'moving %.2f' % dist*i move_straight(robot, dist*i) sleep(5)
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)
def avoid_obstacles(robot, x, y, theta): # This will need more arguments markers = robot.see() theta = robot.position.theta for marker in markers: if marker.info.code in range(28, 32): markers_ = robot.see() for m in markers_: # Leaves m being a robot marker if m.info.code in range(28, 32): break else: log(robot, 'Lost opponent\'s robot') return # Works out direction of movement of that robot x0, y0 = marker_pos(marker, robot.position) x1, y1 = marker_pos(m, robot.position) dx = x1 - x0 dy = y1 - y0 DX = marker.centre.world.x - m.centre.world.x X = robot.position.x Y = robot.position.y dist_to_target = hypot(x - X, y - Y) if dx < 0.1 and dy < 0.1: # The robot is still if m.dist <= dist_to_target: # It's too close # This works log(robot, 'Robot still, too close.') # Turn 45 deg away from opp. log(robot, radians(m.rot_y) - pi / 4) turn(robot, radians(m.rot_y) - pi / 4) # TO-DO: Turn towards the centre # Maybe check whether we can go to this point # Find the point to go to get past the opp. # move_straight(robot, .2) dist = hypot(0.5, m.dist) dx_ = sin(theta) * dist dy_ = cos(theta) * dist # move_straight(robot, hypot(0.5, m.dist)) # Use move_to_p. move_to_point(robot, X + dx_, Y + dy_, theta, False) # This gets us where we wanted return True else: return False else: print 'opponent moving' # opp_theta = (5*pi/2 - atan2(dy, dx)) % 2*pi opp_theta = pi / 6 - pi # Is it going towars us? if theta - pi / 9 <= ((opp_theta + pi) % (2 * pi)) <= theta + pi / 9: # Move in parallel turn(robot, opp_theta + pi - theta) #may result in head-on co. if m.dist <= dist_to_target: move_straight(robot, m.centre.world.y) # How long? return True else: print 'not avoiding' return False # May cause trouble # Get to target point elif hypot(X - x1, Y - y1) <= dist_to_target: # It's too close if m.centre.world.x >= 0.4: # How close we'll get to it # if we move forwards print 'we can move past it' move_straight(robot, m.dist) # How long? # This gets us where we wanted move_to_point(robot, x, y, theta, False) return True else: # Let it pass print 'waiting' camera_delay = 1.5 d_left = 0.4 - m.centre.world.x t_left = d_left / (DX / camera_delay) if t_left < 5: # Don't wait too long sleep(t_left) return False return True else: return False elif marker.info.code in range(40, 52): return False if our_token(marker, robot.zone): pass # This should happen quite often else: pass # Do something? else: log(robot, 'Seen nothing')
def avoid_obstacles(robot, x, y, theta): # This will need more arguments markers = robot.see() theta = robot.position.theta for marker in markers: if marker.info.code in range(28, 32): markers_ = robot.see() for m in markers_: # Leaves m being a robot marker if m.info.code in range(28, 32): break else: log(robot, 'Lost opponent\'s robot') return # Works out direction of movement of that robot x0, y0 = marker_pos(marker, robot.position) x1, y1 = marker_pos(m, robot.position) dx = x1 - x0 dy = y1 - y0 DX = marker.centre.world.x - m.centre.world.x X = robot.position.x Y = robot.position.y dist_to_target = hypot(x-X, y-Y) if dx < 0.1 and dy < 0.1: # The robot is still if m.dist <= dist_to_target: # It's too close # This works log(robot, 'Robot still, too close.') # Turn 45 deg away from opp. log(robot, radians(m.rot_y)-pi/4) turn(robot, radians(m.rot_y)-pi/4) # TO-DO: Turn towards the centre # Maybe check whether we can go to this point # Find the point to go to get past the opp. # move_straight(robot, .2) dist = hypot(0.5, m.dist) dx_ = sin(theta) * dist dy_ = cos(theta) * dist # move_straight(robot, hypot(0.5, m.dist)) # Use move_to_p. move_to_point(robot, X+dx_, Y+dy_, theta, False) # This gets us where we wanted return True else: return False else: print 'opponent moving' # opp_theta = (5*pi/2 - atan2(dy, dx)) % 2*pi opp_theta = pi/6 - pi # Is it going towars us? if theta - pi/9 <= ((opp_theta+pi) % (2*pi)) <= theta + pi/9: # Move in parallel turn(robot, opp_theta+pi-theta) #may result in head-on co. if m.dist <= dist_to_target: move_straight(robot, m.centre.world.y) # How long? return True else: print 'not avoiding' return False # May cause trouble # Get to target point elif hypot(X-x1, Y-y1) <= dist_to_target: # It's too close if m.centre.world.x >= 0.4: # How close we'll get to it # if we move forwards print 'we can move past it' move_straight(robot, m.dist) # How long? # This gets us where we wanted move_to_point(robot, x, y, theta, False) return True else: # Let it pass print 'waiting' camera_delay = 1.5 d_left = 0.4 - m.centre.world.x t_left = d_left / (DX/camera_delay) if t_left < 5: # Don't wait too long sleep(t_left) return False return True else: return False elif marker.info.code in range(40, 52): return False if our_token(marker, robot.zone): pass # This should happen quite often else: pass # Do something? else: log(robot, 'Seen nothing')