def __init__(self, world):
self._world = world
self._communicate = Controller()
self._kick_off = False
class Task(object):
def __init__(self, world):
self._world = world
self._communicate = Controller()
self._kick_off = False
@property
def world(self):
return self._world
@world.setter
def world(self, world):
self._world = world
"""
Strategies
"""
def task_defender_kick_off(self):
print ("Please kick off")
if self.task_kick_off():
print ("kick off completed")
return self.task_defender()
else:
return False
def task_defender(self):
"""
Robot will strive to stay in the defending region, avoid other robots, and intercept ball.
Once it has the ball, it shall pass to teammate.
If the ball is in the attacking region, it shall just protect the goal and wait.
"""
# while the ball is with us, just go to it. When you've got it, pass to teammate
if self.ball_in_defender_region():
print ("Ball in defender region")
# if the ball is in the attacker region as well, we need to check who's closer - attacker or us
if self.ball_in_attacker_region() and not self.are_we_closer_than_teammate():
print ("Ball is in the attacker region as well, and teammate is closer: sit and wait for the ball")
self.task_sit_between_goal_and_ball()
# we're good to go, get ball and kick to teammate
self.task_grab_rotate_kick()
# the ball is in just the attacker region, our task is to be half way between the ball and the goal
else:
print "Ball is not in defender region, sit and wait for ball"
self.task_sit_between_goal_and_ball()
# always return false, this means this task will keep running
return False
def task_kick_off(self):
"""
Robot will kick the ball to kick off position
"""
# assume we have the ball already in grabber and grabbers close
if not self._kick_off:
# This is if we're shooting right to left
# kick_off_x = self.world.defender_region.left - 15
# kick_off_y = self.world.pitch_boundary_top - 10
# this is if we're shooting left to right
if self._world.our_side == "right":
kick_off_x = self.world.defender_region.right + 15
kick_off_y = self.world.pitch_boundary_bottom - 10
else:
kick_off_x = self.world.defender_region.left - 15
kick_off_y = self.world.pitch_boundary_top - 10
if self.rotate_to_alignment(kick_off_x, kick_off_y):
if self.ungrab_ball():
# kick ball to teammate
distance = self._world.our_robot.get_displacement_to_point(kick_off_x, kick_off_y)
self._kick_off = True
return self.kick_ball(distance_to_kick=distance)
return False
else:
return True
def task_attacker(self):
"""
Robot will not enter defending region. It will stay in attacking region.
If the ball is in the defending region (which it cannot enter), it will wait to receive the ball from teammate.
If the ball is in the attacking region, it will strive to grab the ball.
Once it has the ball, it will look to shoot it in the goal.
"""
self.task_grab_rotate_kick_in_goal()
# always return false, this means this task will keep running
return False
def task_attacker_kick_off(self):
print ("Please kick off")
if self.task_kick_off():
print ("kick off completed")
return self.task_attacker()
else:
return False
def task_penalty(self):
"""
Robot will aim to take a penalty, playing by the penalty rules. Assumes robot is holding ball
"""
# wait a random amount of time
random_number = randint(1, 5)
time.sleep(random_number)
x = self.world.their_goal.x
y = self.world.their_goal.y - 30
if self.rotate_to_alignment(x, y):
return self.task_kick_ball_in_goal()
return False
def task_penalty_goalie(self):
"""
Robot will be goalie for penality. Robot will randomly open grabbers, close them, while facing the ball
"""
if self.rotate_to_ball():
if self.ungrab_ball():
time.sleep(20)
self.grab_ball()
return False
"""
Big tasks; these are made up of smaller tasks
"""
def task_vision(self):
pass
def task_rotate_and_grab(self):
# rotate to face the ball
if self.rotate_to_ball():
# wait till ball has stopped
if self._world.ball.speed < 5:
# move to the ball with grabbers open
if self.ungrab_ball():
if self.task_move_to_ball():
if self.grab_ball():
return self.ball_received()
return False
# Assuming we're facing the right direction
def task_grab_rotate_kick(self):
"""
Opens grabbers and moves to the ball, grabs it, rotates to teammate, kicks it to teammate.
Does not check if ball is received by teammate
"""
print ("Go grab the ball, kick it to teammate")
# if self.ungrab_ball():
if self.rotate_to_ball():
#
if self.move_to_ball():
# if self.ungrab_ball():
if self.ball_received():
# grab the ball we've just be given
if self.grab_ball():
# rotate to face the other robot
if self.rotate_to_alignment(self._world.teammate.x, self._world.teammate.y):
if self.ungrab_ball():
# kick ball to teammate
distance = self._world.our_robot.get_displacement_to_point(
self._world.teammate.x, self._world.teammate.y
)
return self.kick_ball(distance_to_kick=distance)
return False
def task_grab_rotate_kick_in_goal(self):
"""
Opens grabbers and moves to the ball, grabs it, rotates to goal, kicks it to goal.
Does not check if ball is received by teammate
"""
print ("Go grab the ball, kick it to goal")
# if self.ungrab_ball():
if not self.ball_received():
print ("ball not received, go to ball")
if self.rotate_to_ball():
#
self.move_to_ball()
else:
if self.grab_ball():
print ("ball received. kick in goal")
# rotate to face the other robot
if self.rotate_to_alignment(self.world.their_goal.x, self.world.their_goal.y):
if self.ungrab_ball():
# kick ball to teammate
distance = self._world.our_robot.get_displacement_to_point(
self.world.their_goal.x, self.world.their_goal.y
)
return self.kick_ball(distance_to_kick=distance)
return False
def task_move_to_ball(self):
# If the ball isn't moving, we can just move to it
if self._world.ball.speed < 5:
if self.rotate_to_ball():
return self.move_to_ball()
else:
return False
# If the ball IS moving, we need to predict where it's going and move there...
else:
# move to predicted stopping point, but only when the ball is deceleration
if self.world.ball.acceleration[0] < 0:
predicted_x = self.world.ball.predicted_stopping_coordinates_x
predicted_y = self.world.ball.predicted_stopping_coordinates_y
if self.rotate_to_alignment(predicted_x, predicted_y):
return self.move_to_coordinates(predicted_x, predicted_y)
return False
def task_move_and_grab_ball(self):
# If we're happy with rotation and movement, grab the ball
if self.rotate_to_ball():
if self.ungrab_ball():
if self.move_to_ball():
return self.grab_ball()
return False
def task_kick_ball_in_goal(self):
# If we're happy with rotation to face goal, ungarb and kick the ball
if self.rotate_to_alignment(self.world.their_goal.x, self.world.their_goal.y):
if self.ungrab_ball():
return self.kick_ball()
return False
def task_sit_between_goal_and_ball(self):
# work out co-ordinates half way between goal and ball
midpoint_x, midpoint_y = helper.calculate_midpoint(
self.world.our_goal.x, self.world.our_goal.y, self.world.ball.x, self.world.ball.y
)
print (
"our goal x, our goal y, ball x, ball y",
self.world.our_goal.x,
self.world.our_goal.y,
self.world.ball.x,
self.world.ball.y,
)
if self.rotate_to_alignment(midpoint_x, midpoint_y):
if self.move_to_coordinates(midpoint_x, midpoint_y):
return self.ungrab_ball()
return False
"""
Smaller tasks
"""
def move_to_coordinates(self, x, y):
"""
Given a specific robot, it will try and move this robot to a given co-ordinate, assuming it is facing the correct way
already
:param target_vector
"""
print ("Move to coordinates x, y", x, y)
# Calculate how long we need to run the motor for
distance = self._world.our_robot.get_displacement_to_point(x, y)
# are we gonna hit anyone in this time
safety_check = self.safety_check(x, y)
if safety_check:
if distance < 40:
return True
else:
calculated_duration = self.calculate_motor_duration(distance)
# Tell arduino to move for the duration we've calculated
self._communicate.move_duration(calculated_duration)
# Wait until this task has completed
if calculated_duration > 0:
time.sleep(calculated_duration / 1000)
# Returns false which means we'll get more data from vision first, run this function again, to verify ok
return False
else:
print ("Safety check failed -> routing to the nearest possible edge")
return self.move_to_coordinates(safety_check[0], safety_check[1])
def rotate_to_ball(self):
print ("Rotate to ball")
return self.rotate_to_alignment(self._world.ball.x, self._world.ball.y)
def move_to_ball(self):
return self.move_to_coordinates(self._world.ball.x, self._world.ball.y)
def rotate_to_alignment(self, x, y):
"""
Given a specific robot, it will rotate to face a specific angle
:param x:
:param y:
"""
angle_to_rotate = self._world.our_robot.get_rotation_to_point(x, y)
distance = self._world.our_robot.get_displacement_to_point(x, y)
# If the angle of rotation is less than 15 degrees, leave it how it is
if (
(15 >= angle_to_rotate >= -15 and distance > 40)
or (5 >= angle_to_rotate >= -5 and distance <= 40)
or (25 >= angle_to_rotate >= -25 and distance <= 30)
):
return True
else:
duration = self.calculate_motor_duration_turn(angle_to_rotate)
wait_time = self._communicate.turn(duration)
time.sleep(abs(wait_time))
# Returns false which means we'll get more data from vision first, run this function again, to verify ok
return False
def ungrab_ball(self):
# if the grabbers are already open, don't do anything
print ("Ungrab ball")
# if not self.world.our_robot.grabbers_open:
print "Grabbers aren't open, please open them"
wait_time = self._communicate.ungrab()
time.sleep(wait_time)
self.world.our_robot.grabbers_open = True
return True
def grab_ball(self):
# if the grabbers are already closed, don't do anything
print ("Grab ball")
# if self.world.our_robot.grabbers_open:
wait_time = self._communicate.grab()
time.sleep(wait_time)
self.world.our_robot.grabbers_open = False
return True
def kick_ball(self, distance_to_kick=None):
print ("Kick ball")
if distance_to_kick:
power = self.calculate_kick_power(distance_to_kick)
else:
power = 1
wait_time = self._communicate.kick(power)
time.sleep(wait_time)
return True
"""
Helper methods
"""
def are_we_closer_than_teammate(self):
distance_for_us = self.world.our_robot.get_displacement_to_point(self.world.ball.x, self.world.ball.y)
distance_for_teammate = self.world.teammate.get_displacement_to_point(self.world.ball.x, self.world.ball.y)
if distance_for_us > distance_for_teammate:
return False
else:
return True
def ball_received(self):
# calculate displacement from us to ball
distance = self._world.our_robot.get_displacement_to_point(self._world.ball.x, self._world.ball.y)
print (" we are ", distance, " away from ball")
if distance < 33:
return True
else:
return False
def ball_received_by_teammate(self):
# calculate displacement from us to ball
distance = self._world.teammate.get_displacement_to_point(self._world.ball.x, self._world.ball.y)
if distance < 50:
return True
else:
return False
def ball_in_defender_region(self):
if self._world.defender_region.contains(self._world.ball.x, self._world.ball.y):
return True
else:
return False
def ball_in_attacker_region(self):
if self._world.attacker_region.contains(self._world.ball.x, self._world.ball.y):
return True
else:
return False
def safety_check(self, resultant_x, resultant_y):
"""
Before any movement is called, this is called. This essentially checks if the movement we're about to do will hit someone
else (roughly).
Check we aren't gonna run into a wall either
:param resultant_y:
:param resultant_x:
:return: bool
"""
print ("Safety check. Our robot is at, x, y", self.world.our_robot.x, self.world.our_robot.y)
# we're aiming for these co-ordinates, but realistically we aren't gonna move in a straight line
# so let's see if there's any robots in our path (but not loop through every possibility, just every +padding
# certaintly not most efficient way but who cares anymore...
check_x = self.world.our_robot.x
check_y = self.world.our_robot.y
if self.world.safety_padding <= 0:
self.world.safety_padding = 1
while check_x <= resultant_x and check_y <= resultant_y:
# is this co-ordinate within (z) units of other robots? if so we need to stop and think
robots = [self.world.teammate, self.world.their_defender, self.world.their_attacker]
for robot in robots:
if (abs(resultant_x - robot.x) <= self.world.robot_safety_padding) and abs(
resultant_y - robot.y
) <= self.world.robot_safety_padding:
print ("TOo close to another robot")
# if this robot is moving, don't do anything
if robot.speed > 5:
return resultant_x, resultant_y
# robot is unlikely to move, let's re-route
else:
# this needs to be implemented
return resultant_x, resultant_y
check_x += self.world.safety_padding
check_y += self.world.safety_padding
# check if we're going to run into a wall
print ("Trying to move to resultant_x, resultant_y", resultant_x, resultant_y)
if self.world.pitch_boundary_bottom - self.world.safety_padding <= resultant_y:
print ("Trying to go somewhere greater than the greatest (bottom) boundary")
return resultant_x, self.world.pitch_boundary_bottom - self.world.safety_padding
if self.world.pitch_boundary_top + self.world.safety_padding >= resultant_y:
print ("Trying to go somewhere less than the lowest (top) boundary")
return resultant_x, self.world.pitch_boundary_top + self.world.safety_padding
if self.world.pitch_boundary_left + self.world.safety_padding >= resultant_x:
print ("Trying to go somewhere less than the left boundary")
return self.world.pitch_boundary_left + self.world.safety_padding, resultant_y
if self.world.pitch_boundary_right - self.world.safety_padding <= resultant_x:
print ("Trying to go somewhere greater than the right boundary")
return self.world.pitch_boundary_right - self.world.safety_padding, resultant_y
# we're good to move here
return True
@staticmethod
def calculate_kick_power(distance):
"""
Given a distance to kick, crudely calculates the power for the kicker; this is only used for ball kicking atm
:param distance:
"""
# power is between 0.0 and 1.0, assume distance given is between 0.0 and 2.0. this function needs improving
power = distance / 2
print ("calculated power is ", power)
if power > 1:
return 1
return power
@staticmethod
def calculate_motor_duration_turn(angle_to_rotate):
"""
:param angle_to_rotate: given in degrees
"""
# crude angle -> duration conversion
duration = 100 + (abs(angle_to_rotate) * 5.5)
if angle_to_rotate < 0:
duration = -duration
return -duration
@staticmethod
def calculate_motor_duration(distance):
"""
Given a distance to travel, we need to know how long to run the motor for
:param distance: provided in metres
"""
# some crude distance -> duration measure. assumes 10cm of movement equates to 100ms, past the initial 100ms
duration = 20 + (distance * 7.5)
return duration