def tolerance_angle(self, object_name):
if object_name == "ball":
return Util.get_smallest_angle(self.heading, self.direction['ball']['angle']) < self.TOLERANCE_GRAB_ANGLE
elif object_name == "ally":
return Util.get_smallest_angle(self.heading, self.direction['ally']['angle']) < self.TOLERANCE_ANGLE
elif object_name == "point":
return Util.get_smallest_angle(self.heading, self.direction['desired position']['angle']) \
< self.TOLERANCE_ANGLE
def heading_closer_to_attacking_goal_or_ally(robot):
try:
angle_to_score = Util.get_turn_angle(robot.heading, robot.direction["attacking goal"]['angle'])
angle_to_ally = Util.get_turn_angle(robot.heading, robot.direction["ally"]['angle'])
except:
return "goal"
if min(abs(angle_to_score), abs(angle_to_ally)) == abs(angle_to_score):
return "goal"
else:
return "ally"
def ball_goal_intercept_point(self, goal):
ball_line = self.ball_line()
width, height = self.vision.get_frame_size()
width = self.goal_coords[2][0] - self.goal_coords[0][0]
left_wall = ((self.goal_coords[0][0], 0), (self.goal_coords[0][0], height))
right_wall = ((width, 0), (width, height))
if goal == "left":
return Util.line_intersection(ball_line, left_wall)
else:
return Util.line_intersection(ball_line, right_wall)
def calculate_properties(self, *args):
if self.visible:
ball_position = args[0]
BaseRobot.calculate_properties(self, ball_position)
if self.desired_position is not None:
self.direction["desired_position"] = Util.get_direction(self.position, self.desired_position)
if self.enemy1.visible:
self.direction["enemy1"] = Util.get_direction(self.position, self.enemy1.position)
if self.enemy2.visible:
self.direction["enemy2"] = Util.get_direction(self.position, self.enemy2.position)
if self.enemy1.visible and self.enemy2.visible:
self.intercept_line = [self.enemy1.position, self.enemy2.position]
def set_desired_position(self, desired_position):
self.desired_position = desired_position
try:
if Util.point_in_defense_zone(self.zones, self.desired_position, self.attacking_goal) or (
Util.point_in_defense_zone(self.zones, self.desired_position, self.defending_goal)
and Util.point_in_defense_zone(self.zones, self.ally.position, self.defending_goal)
):
# print("[PLANNER GROUP 9] desired position in a naughty place, retreating to centre")
self.desired_position = self.centre
except:
pass
self.direction["desired position"] = Util.get_direction(self.position, self.desired_position)
def subtask_complete(self, subtask):
if subtask == "turn to ally":
angle_to_ally = Util.get_turn_angle(self.robot.heading, self.robot.direction["ally"]["angle"])
return self.robot.within_tolerance_angle(angle_to_ally)
elif subtask == "move to receive pass":
# if self.robot.desired_position == self.receive_point:
angle_to_ally = Util.get_turn_angle(self.robot.heading, self.robot.direction["ally"]["angle"])
distance_to_centre = Util.get_direction(self.robot.position, self.robot.centre)["distance"]
return self.robot.within_tolerance_angle(angle_to_ally) and distance_to_centre <= 200
# else:
# return False
elif subtask == "has ball":
return self.has_ball
elif subtask == "kicked":
return self.kicked
def subtask_complete(self, subtask):
"""
Returns True if subtask is completed successfully
"""
if subtask == "turn to ally":
angle_to_ally = Util.get_turn_angle(self.robot.heading, self.robot.direction["ally"]['angle'])
return self.robot.tolerance_angle("ally")
elif subtask == "move to receive pass":
# if self.robot.desired_position == self.receive_point:
angle_to_ally = Util.get_turn_angle(self.robot.heading, self.robot.direction["ally"]['angle'])
distance_to_centre = Util.get_direction(self.robot.position, self.robot.centre)['distance']
return self.robot.tolerance_angle("ally") and distance_to_centre <= self.robot.TOLERANCE_DISTANCE
elif subtask == "has ball":
return self.has_ball
elif subtask == "kicked":
return self.kicked
def has_ball_moved(self, old_ball_position):
# check to see if the current ball position is sufficiently far enough away from the old_ball_position
distance = Util.get_direction(self.ball_position, old_ball_position)['distance']
if distance >= 5:
return True
else:
return False
def ball_moving_to_defending_goal(self, goal):
# project the direction the ball is currently heading onto a side of the pitch and check if this point
ball_line = self.ball_line()
# Return False if the ball is stationary (ball line length withing threshold)
(x0, y0), (x1, y1) = ball_line
if (x0 - x1)**2 + (y0 - y1)**2 < 5**2:
return False
width, height = self.vision.get_frame_size()
width = self.goal_coords[2][0] - self.goal_coords[0][0]
left_wall = ((self.goal_coords[0][0], 0), (self.goal_coords[0][0], height))
right_wall = ((width, 0), (width, height))
if goal == "left":
return (Util.do_they_intersect(ball_line, left_wall) and ball_line[0][0] > ball_line[1][0])
else:
return (Util.do_they_intersect(ball_line, right_wall) and ball_line[0][0] < ball_line[1][0])
def calculate_properties(self, *args):
if self.visible:
ball_position = args[0]
BaseRobot.calculate_properties(self, ball_position)
if self.desired_position is not None:
self.direction["desired_position"] = Util.get_direction(self.position, self.desired_position)
if self.enemy1.visible:
self.direction["enemy1"] = Util.get_direction(self.position, self.enemy1.position)
if self.enemy2.visible:
self.direction["enemy2"] = Util.get_direction(self.position, self.enemy2.position)
if self.enemy1.visible and self.enemy2.visible:
self.intercept_line = [self.enemy1.position, self.enemy2.position]
if self.facing_attacking_goal:
if self.enemy1.visible and self.enemy2.visible and self.ally.visible:
list_of_robots = [self.ally, self.enemy1, self.enemy2]
self.can_score = Util.can_score(self.attacking_goal, self, list_of_robots)
else:
return True
else:
self.can_score = False
self.direction['attacking goal'] = Util.get_direction(self.position,
Util.get_midpoint(self.attacking_goal_line))
def execution_step(self, subtask):
# If we've never seen the robot, don't execute
# until seen
if self.robot.position is None: return
self.set_ui_var("Facing goal", self.robot.facing_attacking_goal)
if self.subtask != subtask:
self.subtask_started = True
self.robot.desired_position = None
self.robot.desired_heading = None
self.subtask_time = time.time()
else:
self.subtask_started = False
self.subtask = subtask
try:
self.has_ball = self.world_state.who_has_ball().role == 'group10'
except AttributeError:
self.has_ball = False
self.set_ui_var("Subtask", subtask)
self.set_ui_var("Has Ball", self.has_ball)
self.set_ui_var("Ball in Range", self.robot.ball_in_range)
# done
if subtask == 'acquire ball' and not self.has_ball:
if self.acquire_ball_grab_now:
if DEBUG: print "[INFO] Acquire Ball Grab Now"
if time.time() - self.acquire_ball_grab_timeout_local < 2:
self.grab()
else:
self.acquire_ball_grab_now = False
elif self.robot.ball_in_range and self.robot.required_grabbing == 1:
if DEBUG: print "[INFO] Ball in range: grabbing ball"
# Grab the ball
self.acquire_ball_grab_timeout_local = time.time()
self.acquire_ball_grab_now = True
self.grab()
# Back off a bit
elif self.robot.ball_in_range or self.robot.ball_in_bottom or \
self.robot.ball_in_left or self.robot.ball_in_right:
if DEBUG: print "[INFO] Backing off..."
get_angle_of_ball = self.robot.get_absolute_angle(self.world_state.ball_position)
angle_opposite = (get_angle_of_ball + 180) % 360
self.move(angle_opposite)
# Ball close - JAWS OPEN
elif self.robot.tolerance_distance('ball') and not self.robot.required_grabbing == 1:
if DEBUG: print "[INFO] Preparing to halt & ungrab"
self.halt()
self.ungrab()
# Ball far away - move closer to it
else:
angle_to_ball = self.robot.get_absolute_angle(self.world_state.ball_position)
self.move(angle_to_ball)
if not (self.robot.tolerance_angle("ball")):
self.turn(angle_to_ball)
#TODO - DONE - RECHECKED - THINK ABOUT TIMEOUT
elif subtask == "shoot" and self.has_ball:
if time.time() - self.subtask_time > 10:
self.kick()
else:
if self.robot.can_score:
self.kick()
# sweep to find an empty spot
elif self.robot.desired_heading is None:
left_post = self.robot.attacking_goal_line[0][1]
right_post = self.robot.attacking_goal_line[1][1]
random_y = random.uniform(left_post, right_post)
random_x = self.robot.attacking_goal_line[0][0]
angle_to_turn = self.robot.get_absolute_angle((random_x, random_y))
self.turn(angle_to_turn)
# TODO: DONE - RECHECKED
elif subtask == "defend penalty":
robot_with_ball = self.world_state.who_has_ball()
# one robot has ball
if robot_with_ball is not None:
required_angle = robot_with_ball.heading - math.copysign(180, robot_with_ball.heading)
# robot is not facing the goal - get to the closest post
if Util.intercept_goal(robot_with_ball) is None:
if self.robot.defending_goal == 'right':
if robot_with_ball.heading < 0:
self.move(self.robot.get_absolute_angle(self.robot.defending_goal_line[1]))
else:
self.move(self.robot.get_absolute_angle(self.robot.defending_goal_line[0]))
else:
# facing down
if self.robot.heading < 0:
self.move(self.robot.get_absolute_angle(self.robot.defending_goal_line[1]))
else:
self.move(self.robot.get_absolute_angle(self.robot.defending_goal_line[0]))
#robot is facing the goal
# TODO - test TOLERANCE ANGLE
else:
point_on_goal_line = Util.intercept_goal(robot_with_ball)
if point_on_goal_line is None:
self.halt()
else:
if not abs(required_angle - self.robot.heading) < 15:
# get absolute angle
required_angle = 360 - (required_angle+360) % 360
self.turn(required_angle)
# if not within grabbing distance
if not abs(self.robot.position[1] - point_on_goal_line[1]) <= 15:
# goal is supposed to be kicked to the left of my robot
if self.robot.position[1] - point_on_goal_line[1] > 0:
self.move(270)
# goal is supposed to be kicked to the right of my robot
else:
self.move(90)
# NOBODY HAS THE BALL
else:
self.halt()
# TODO: DONE - RECHECKED - NEEDS TESTING
elif subtask == "kick off" and self.has_ball:
if self.subtask_started:
random_angle = round(random.uniform(23, 67))
random_choice = random.choice["bottom", "top"]
# kick off is to the right
if self.robot.defending_goal == 'left':
if random_choice == "bottom":
random_angle = -random_angle
# kick off to the left
else:
if random_choice == "bottom":
random_angle += -180
else:
random_angle = 180 - random_angle
self.kickoff_angle = random_angle
self.subtask_started = True
# not facing angle
if not Util.get_smallest_angle(self.robot.heading, self.kickoff_angle) < 15:
random_angle = 360 - (self.kickoff_angle+360) % 360
self.turn(random_angle)
else:
self.kick()
elif subtask == "defend goal from ball" or subtask == "align with ball":
#print "Position of robot is " + str(self.robot.position[1])
goal_midpoint = Util.get_midpoint(self.robot.defending_goal_line)
#print "Position of goal midpoint is " + str(goal_midpoint[1])
def euclid(p1, p2):
return ((p2[0]-p1[0])**2 + (p2[1]-p1[1])**2)**0.5
dist_robot_to_goal = euclid(self.robot.position, goal_midpoint)
dist_ball_to_goal = euclid(self.world_state.ball_position, goal_midpoint)
aligning_away_from_goal = False
def map_to_ball_to_goal(x):
"""
:return: The y value of ball line at point x
"""
(x0, y0), (x1, y1) = (goal_midpoint[0], goal_midpoint[1]), \
(self.world_state.ball_position[0], self.world_state.ball_position[1])
return (y1 - y0) / (x1 - x0 + 0.0001) * (x - x0) + y0
self.set_ui_var("Map Ball to Goal", map_to_ball_to_goal(self.robot.position[0]))
if not self.robot.tolerance_angle('ball'):
self.turn(self.robot.get_absolute_angle(self.world_state.ball_position))
if (self.robot.defending_goal == "left" and 32 < (self.robot.position[0] - goal_midpoint[0]) < 36) \
or (self.robot.defending_goal == "right" and 32 < (goal_midpoint[0] - self.robot.position[0]) < 36):
required_x_coordinate = -1
elif dist_robot_to_goal < 150 or dist_ball_to_goal < dist_robot_to_goal:
if self.robot.defending_goal == "left":
required_x_coordinate = goal_midpoint[0] + 32
else:
required_x_coordinate = goal_midpoint[0] - 32
else:
aligning_away_from_goal = True
required_x_coordinate = -1
if self.world_state.ball_moving_to_defending_goal(self.robot.defending_goal):
required_y_coordinate = self.world_state.ball_line_y_val_at_x(self.robot.position[0])
else:
if aligning_away_from_goal:
required_y_coordinate = map_to_ball_to_goal(self.robot.position[0])
else:
required_y_coordinate = self.world_state.ball_position[1]
# Goal width: 75-80
if required_y_coordinate < self.robot.defending_goal_line[0][1] and not aligning_away_from_goal:
required_y_coordinate = self.robot.defending_goal_line[0][1] + 40
elif required_y_coordinate > self.robot.defending_goal_line[1][1] and not aligning_away_from_goal:
required_y_coordinate = self.robot.defending_goal_line[1][1] - 40
if required_x_coordinate == -1:
if self.robot.position[1] - required_y_coordinate > 0:
self.move(270)
else:
self.move(90)
elif abs(required_y_coordinate - self.robot.position[1]) < 12:
if not self.robot.tolerance_angle('ball'):
self.turn(self.robot.get_absolute_angle(self.world_state.ball_position))
else:
self.halt()
else:
self.move(self.robot.get_absolute_angle([required_x_coordinate, required_y_coordinate]))
if dist_ball_to_goal < 185:
self.ungrab()
else:
self.robot.required_grabbing = 0
# done - TESTED
elif subtask == 'turn to ally':
if not self.robot.tolerance_angle('ally'):
angle = self.robot.get_absolute_angle(self.robot.ally.position)
self.turn(angle)
# done - TESTED
elif subtask == "pass" :
self.kick()
# TODO - DONE - RECHECKED - DONE
elif subtask == "panic shoot":
if self.robot.facing_attacking_goal:
self.kick()
else:
self.set_ui_var("Not facing", subtask)
angle_to_goal = self.robot.get_absolute_angle(Util.get_midpoint(self.robot.attacking_goal_line))
self.grab()
self.turn(angle_to_goal)
# TODO - test
elif subtask == "defend enemy shot":
x_thresh = 30
robot_with_ball = self.world_state.who_has_ball()
if robot_with_ball is not None:
# ball is moving to the goal, get to the closest point
if robot_with_ball.facing_attacking_goal:
desired_position = Util.intercept_goal(robot_with_ball)
self.ungrab()
self.robot.desired_position = desired_position
# robot is not far away from the goal
elif abs(self.robot.position[0] - self.robot.attacking_goal_line[0][0]) > x_thresh:
goal_center = Util.get_midpoint(self.robot.defending_goal_line)
angle_to_move = self.robot.get_absolute_angle(goal_center)
self.move(angle_to_move)
# ball not moving to the goal
else:
self.halt()
# TODO - needs testing; ask about allow rotations
# TODO - world state does not even have can_score function
elif subtask == "move to receive pass" or subtask == "intercept shot" or subtask == "intercept pass":
if subtask == "move to receive pass":
if self.subtask_started:
self.receive_point = Util.set_receive_point(self.robot.zones, self.robot.attacking_goal)
desired_position = self.receive_point
elif subtask == "intercept shot":
enemy = self.world_state.who_has_ball()
if self.world_state.can_score(self.robot.defending_side, enemy):
desired_position = Util.get_intercept_point(
self.robot.position, [enemy.position, Util.intercept_goal(enemy)])
else:
desired_position = Util.get_intercept_point(
self.robot.position, [enemy.position, Util.get_midpoint(self.robot.defending_goal_line)])
elif subtask == "intercept pass":
desired_position = Util.get_intercept_point(self.robot.position, self.robot.intercept_line)
# TODO: rethink if there are any edge cases (pycharm complains)
self.robot.set_desired_position(desired_position)
# TODO - allow rotation?!
if not self.robot.tolerance_distance("desired position"):
angle_to_position = self.robot.get_absolute_angle(self.robot.desired_position)
self.move(angle_to_position)
# TODO - Done - RECHECKED - needs testing
# ungrab and wait to receive the ball, grabbing when vision tells us it is in grabbing range
elif subtask == "receive pass":
if self.robot.ball_in_range and self.robot.required_grabbing == 1:
self.grab()
else:
self.ungrab()
# TODO: add some comments explaining what this does. Primarily because pycharm complains.
if self.robot.desired_heading is not None:
# print 'heading not none'
self.turn(self.robot.desired_heading)
if self.robot.desired_position is not None:
# print 'desired not none'
self.move(self.robot.get_absolute_angle(self.robot.desired_position))
self.com.configure_command(self.robot.required_bearing, self.robot.required_heading,
self.robot.required_grabbing, self.robot.required_kicking)
# Reset all the values to recalculate them
self.robot.reset_command_values()
# Timeouts
curr_time = time.time()
# Kicker timeout
#TODO: test!
if curr_time - self.kick_timeout >= 5:
self.kicked = False
self.robot.required_kicking = 0
# Ungrab timeout - self.robot.required_grabbing = 1 for ungrabbing
# TODO: test!
if not self.robot.required_grabbing == 1:
self.ungrab_timeout = curr_time
else:
if curr_time - self.ungrab_timeout >= 5:
self.robot.required_grabbing = 0
#
if not self.robot.required_grabbing == 2: #grabbers are either relaxed or ungrabbing
self.grab_timeout = curr_time
else:
if curr_time - self.grab_timeout >= 5:
self.robot.required_grabbing = 0
def set_desired_position(self, desired_position):
self.desired_position = desired_position
self.direction['desired position'] = Util.get_direction(self.position,
self.desired_position)
def execution_step(self, subtask):
self.subtask_started = self.subtask != subtask
self.subtask = subtask
try:
self.has_ball = self.world_state.who_has_ball().role == "group09"
except AttributeError:
self.has_ball = False
timestamp = time()
heading = self.robot.heading
position = self.robot.position
defending_goal_midpoint = Util.get_midpoint(self.robot.defending_goal_line)
self.go_to_desired_position = False
if timestamp - self.timestamps["kicked"] >= 1:
self.kicked = False
if not self.grabbed and timestamp - self.timestamps["ungrabbed"] >= 5:
self.grab_force()
try:
naughty = Util.point_in_defense_zone(self.robot.zones, self.robot.position, self.robot.attacking_goal) or (
Util.point_in_defense_zone(self.robot.zones, self.robot.position, self.robot.defending_goal)
and Util.point_in_defense_zone(self.robot.zones, self.robot.ally.position, self.robot.defending_goal)
)
except:
naughty = False
if naughty:
self.robot.set_desired_position(self.robot.centre)
self.go_to_desired_position = True
# always grab the ball if possible, no matter what the subtask is
elif self.robot.ball_in_range and not self.has_ball and not self.grabbed:
self.grab()
self.halt()
# don't bash into walls
elif self.collision is not None:
if self.collision == "forward":
self.moving = True
self.com.move_backward(50) # TODO - calibrate
elif self.collision == "left":
self.moving = True
self.com.turn_clockwise(50) # TODO - calibrate
elif self.collision == "right":
self.moving = True
self.com.turn_anticlockwise(50) # TODO - calibrate
self.collision = None
# turn towards the ball, then ungrab and move towards it, grabbing and halting when it is within range
elif self.subtask == "acquire ball":
try:
naughty = Util.point_in_defense_zone(
self.robot.zones, self.world_state.ball_position, self.robot.attacking_goal
) or (
Util.point_in_defense_zone(
self.robot.zones, self.world_state.ball_position, self.robot.defending_goal
)
and Util.point_in_defense_zone(
self.robot.zones, self.robot.ally.position, self.robot.defending_goal
)
)
except:
naughty = False
if naughty:
self.robot.set_desired_position(self.robot.centre)
self.go_to_desired_position = True
# print("[PLANNER GROUP 9] ball in a naughty place, retreating to centre")
else:
angle_to_ball = Util.get_turn_angle(heading, self.robot.direction["ball"]["angle"])
if self.robot.ball_in_left or self.robot.ball_in_right:
self.grab()
self.move(-50, position) # TODO - calibrate
elif self.robot.ball_in_bottom:
self.grab()
self.move(50, position) # TODO - calibrate
elif not self.robot.within_tolerance_grab_angle(angle_to_ball):
self.grab()
self.turn(angle_to_ball, heading)
else:
distance_to_ball = self.robot.direction["ball"]["distance"]
if self.grabbed and self.robot.ball_in_range:
self.move(-50, position) # TODO - calibrate
elif not self.robot.ball_in_range:
self.ungrab()
self.move(distance_to_ball, position)
elif self.subtask == "penalty shoot":
if self.has_ball:
if self.subtask_started or self.bearing is None:
if self.robot.defending_goal == "left":
self.bearing = round(uniform(5, 15))
if choice(["left", "right"]) == "left":
self.bearing = -self.bearing
else:
self.bearing = round(uniform(175, 165))
if choice(["left", "right"]) == "left":
self.bearing = -self.bearing
angle_to_bearing = Util.get_turn_angle(heading, self.bearing)
if not self.robot.within_tolerance_angle(angle_to_bearing):
self.turn(angle_to_bearing, heading)
else:
self.kick_power(100)
elif self.subtask == "defend penalty":
# our robot cannot implement this subtask
pass
elif self.subtask == "kick off":
if self.has_ball:
if self.subtask_started or self.bearing is None:
if self.robot.defending_goal == "left":
self.bearing = round(uniform(23, 67))
if choice(["left", "right"]) == "left":
self.bearing = -self.bearing
else:
self.bearing = round(uniform(113, 157))
if choice(["left", "right"]) == "left":
self.bearing = -self.bearing
angle_to_bearing = Util.get_turn_angle(heading, self.bearing)
if not self.robot.within_tolerance_angle(angle_to_bearing):
self.turn(angle_to_bearing, heading)
else:
self.kick_power(100)
# turn our robot to face our ally
elif self.subtask == "turn to ally":
angle_to_ally = Util.get_turn_angle(heading, self.robot.direction["ally"]["angle"])
if not self.robot.within_tolerance_angle(angle_to_ally):
self.turn(angle_to_ally, heading)
elif self.subtask == "pass" and self.has_ball:
self.kick_distance(self.robot.direction["ally"]["distance"])
elif self.subtask == "panic shoot" or self.subtask == "shoot":
if self.has_ball:
if self.robot.facing_attacking_goal:
self.kick_power(100)
else:
angle_to_goal = Util.get_turn_angle(heading, self.robot.direction["attacking goal"]["angle"])
self.turn(angle_to_goal, heading)
elif (
self.subtask == "move to receive pass"
or self.subtask == "intercept shot"
or self.subtask == "intercept pass"
or self.subtask == "align with ball"
or self.subtask == "defend goal from ball"
or self.subtask == "defend enemy shot"
):
if self.subtask == "move to receive pass":
# if self.subtask_started:
# self.receive_point = Util.set_receive_point(self.robot.zones, self.robot.attacking_goal)
# desired_position = self.receive_point
desired_position = self.robot.centre
elif self.subtask == "intercept shot":
if self.world_state.who_has_ball() is None:
enemy = self.robot.enemy1
else:
enemy = self.world_state.who_has_ball()
if enemy.facing_attacking_goal:
desired_position = Util.get_intercept_point_or_midpoint(
position, [enemy.position, Util.intercept_goal(enemy)]
)
# print("ENEMY CAN SCORE {}".format(desired_position))
else:
desired_position = Util.get_intercept_point_or_midpoint(
position, [enemy.position, defending_goal_midpoint]
)
# print("ENEMY MIDPOINT {}".format(desired_position))
elif self.subtask == "intercept pass":
desired_position = Util.get_intercept_point_or_midpoint(position, self.robot.intercept_line)
else:
desired_position = None
#
# elif self.subtask == "align with ball" or self.subtask == "defend goal from ball":
# if self.world_state.ball_moving_to_defending_goal(self.robot.defending_goal):
# desired_position = self.world_state.ball_intercept_point(self.robot.defending_goal)
# if self.robot.defending_goal == "left":
# desired_position[0] += self.robot.GOAL_X_OFFSET
# else:
# desired_position[0] -= self.robot.GOAL_X_OFFSET
# else:
# ball_y = self.world_state.ball_position[1]
# if self.robot.defending_goal == "left":
# top = self.robot.defending_goal_line[0][1]
# bottom = self.robot.defending_goal_line[1][1]
# x = defending_goal_midpoint[0] + self.robot.GOAL_X_OFFSET
# else:
# top = self.robot.defending_goal_line[2][1]
# bottom = self.robot.defending_goal_line[3][1]
# x = defending_goal_midpoint[0] - self.robot.GOAL_X_OFFSET
# if ball_y <= top:
# y = top
# elif ball_y >= bottom:
# y = bottom
# else:
# y = ball_y
# desired_position = (x, y)
#
# else: # elif self.subtask == "defend enemy shot":
# enemy = self.world_state.who_has_ball()
# intercept = Util.intercept_goal(enemy)
# if intercept is not None:
# if self.robot.defending_goal == "left":
# desired_position = (intercept[0] + self.robot.GOAL_X_OFFSET, intercept[1])
# else:
# desired_position = (intercept[0] - self.robot.GOAL_X_OFFSET, intercept[1])
# else:
# desired_position = defending_goal_midpoint
#
if desired_position is not None:
self.robot.set_desired_position(desired_position)
self.go_to_desired_position = True
# ungrab and wait to receive the ball
elif self.subtask == "receive pass":
self.ungrab()
if self.go_to_desired_position:
distance_to_desired_position = self.robot.direction["desired position"]["distance"]
if not self.robot.within_tolerance_distance(distance_to_desired_position):
# get to the desired position
forward_angle_to_desired_position = Util.get_turn_angle(
heading, self.robot.direction["desired position"]["angle"]
)
if abs(forward_angle_to_desired_position) <= 90:
if not self.robot.within_tolerance_angle(forward_angle_to_desired_position):
self.turn(forward_angle_to_desired_position, heading)
else:
self.move(distance_to_desired_position, position)
else:
backward_angle_to_desired_position = Util.get_turn_angle(
Util.get_opposite_heading(heading), self.robot.direction["desired position"]["angle"]
)
if not self.robot.within_tolerance_angle(backward_angle_to_desired_position):
self.turn(backward_angle_to_desired_position, heading)
else:
self.move(-distance_to_desired_position, position)
else:
if self.subtask == "move to receive pass":
# turn to ally
angle_to_ally = Util.get_turn_angle(heading, self.robot.direction["ally"]["angle"])
if not self.robot.within_tolerance_angle(angle_to_ally):
self.turn(angle_to_ally, heading)
else:
self.ungrab()
elif (
self.subtask == "intercept shot"
or self.subtask == "intercept pass"
or self.subtask == "defend enemy shot"
):
# turn to the enemy with the ball and ungrab
if self.world_state.who_has_ball() is None:
enemy_with_ball = "enemy1"
else:
enemy_with_ball = self.world_state.who_has_ball().role
angle_to_enemy = Util.get_turn_angle(heading, self.robot.direction[enemy_with_ball]["angle"])
if not self.robot.within_tolerance_angle(angle_to_enemy):
self.turn(angle_to_enemy, heading)
else:
self.ungrab()
else: # self.subtask == "align with ball" or self.subtask == "defend goal from ball":
# turn to ball and ungrab
angle_to_ball = Util.get_turn_angle(heading, self.robot.direction["ball"]["angle"])
if not self.robot.within_tolerance_grab_angle(angle_to_ball):
self.turn(angle_to_ball, heading)
else:
self.ungrab()
