def input(self):
controller_input = SimpleControllerState()
theta = self.current_state.rot.yaw
correction_vector = self.target_state.pos - self.current_state.pos
facing_vector = Vec3(cos(theta), sin(theta), 0)
car_to_target = (self.target_state.pos - self.current_state.pos).normalize()
#Rotated to the car's reference frame on the ground.
rel_correction_vector = Vec3((correction_vector.x*cos(theta)) + (correction_vector.y * sin(theta)),
(-(correction_vector.x*sin(theta))) + (correction_vector.y * cos(theta)),
0)
if self.can_reverse and facing_vector.dot(car_to_target) < - 0.5:
correction_angle = atan2(rel_correction_vector.y, rel_correction_vector.x)
controller_input.throttle = - 1.0
if abs(correction_angle) > 1.25:
controller_input.handbrake = 1
controller_input.steer = cap_magnitude(-5*correction_angle, 1)
else:
correction_angle = atan2(rel_correction_vector.y, rel_correction_vector.x)
controller_input.throttle = 1.0
if abs(correction_angle) > 1.25:
controller_input.handbrake = 1
controller_input.steer = cap_magnitude(5*correction_angle, 1)
return controller_input
def input(self):
controller_input = SimpleControllerState()
current_angle_vec = Vec3(cos(self.current_state.rot.yaw), sin(self.current_state.rot.yaw), 0)
goal_angle_vec = Vec3(cos(self.goal_state.rot.yaw), sin(self.goal_state.rot.yaw), 0)
vel_2d = Vec3(self.current_state.vel.x, self.current_state.vel.y, 0)
if (self.current_state.pos - self.goal_state.pos).magnitude() > 400:
#Turn towards target. Hold throttle until we're close enough to start stopping.
controller_input = GroundTurn(self.current_state, self.goal_state).input()
elif vel_2d.magnitude() < 50 and current_angle_vec.dot(goal_angle_vec) < 0:
#If we're moving slowly, but not facing the right way, jump to turn in the air.
#Decide which way to turn. Make sure we don't have wraparound issues.
goal_x = goal_angle_vec.x
goal_y = goal_angle_vec.y
car_theta = self.current_state.rot.yaw
#Rotated to the car's reference frame on the ground.
rel_vector = Vec3((goal_x*cos(car_theta)) + (goal_y * sin(car_theta)),
(-(goal_x*sin(car_theta))) + (goal_y * cos(car_theta)),
0)
correction_angle = atan2(rel_vector.y, rel_vector.x)
#Jump and turn to reach goal yaw.
if self.current_state.wheel_contact:
controller_input.jump = 1
else:
controller_input.yaw = cap_magnitude(correction_angle, 1)
elif self.current_state.vel.magnitude() > 400:
#TODO: Proportional controller to stop in the right place
controller_input.throttle = -1
else:
#Wiggle to face ball
#Check if the goal is ahead of or behind us, and throttle in that direction
goal_angle = atan2((self.goal_state.pos - self.current_state.pos).y, (self.goal_state.pos - self.current_state.pos).x)
if abs(angle_difference(goal_angle,self.current_state.rot.yaw)) > pi/2:
correction_sign = -1
else:
correction_sign = 1
controller_input.throttle = correction_sign
#Correct as we wiggle so that we face goal_yaw.
if angle_difference(self.goal_state.rot.yaw, self.current_state.rot.yaw) > 0:
angle_sign = 1
else:
angle_sign = -1
controller_input.steer = correction_sign*angle_sign
return controller_input
def follow_arc(self, current_state):
controller_input = SimpleControllerState()
controller_input.steer = self.piece.direction
try:
center = self.center1
radius = self.radius1
except AttributeError:
center = self.center
radius = self.radius
controller_input.throttle = 1
if (current_state.pos - center).magnitude() > radius:
controller_input.throttle -= (
(current_state.pos - center).magnitude() - radius) / radius
elif controller_input.steer > 0:
controller_input.steer -= 2 * (
radius - (current_state.pos - center).magnitude()) / radius
else:
controller_input.steer += 2 * (
radius - (current_state.pos - center).magnitude()) / radius
controller_input.steer = cap_magnitude(controller_input.steer)
controller_input.throttle = cap_magnitude(controller_input.throttle)
return controller_input
def input(self):
'''
The final call to get the controller_input for the maneuver.
'''
controller_input = SimpleControllerState()
if self.current_state.wheel_contact:
#Speed up on the ground, turn as needed, then jump
if self.current_state.vel.magnitude() <= self.boost_threshold:
#Boost if we're slower than boost_threshold.
controller_input.boost = 1
#Accelerate if we're below accel_threshold
elif self.current_state.vel.magnitude() <= self.accel_threshold:
controller_input.throttle = 1
elif ( abs(self.current_state.rot.yaw - self.dodge_angle) < 0.2 ):
#Once we turn partway, jump and turn the rest of the way
if self.dodge_angle - self.current_state.rot.yaw > 0:
controller_input = JumpTurn(self.current_state, 0, 1).input()
else:
controller_input = JumpTurn(self.current_state, 0, -1).input()
else:
#Once we're up to speed, and not turned enough, turn away from the dodge
controller_input = QuickTurn(self.turn_direction, 1).input()
elif self.current_state.double_jumped:
#Once we dodge, rotate back around to land properly
controller_input.yaw = cap_magnitude(self.movement_angle - self.current_state.rot.yaw, 1)
elif not ( angles_are_close(self.current_state.rot.yaw, self.dodge_angle, 0.2) ):
#Turn a bit more while in the air before dodging
controller_input = JumpTurn(self.current_state, 0, self.turn_direction).input()
elif self.current_state.pos.z > 50:
#Once we're finally turned enough, dodge
controller_input = AirDodge(self.dodge_direction, self.current_state.jumped_last_frame).input()
controller_input.throttle = 1
return controller_input
def Cowculate(plan, game_info, ball_prediction, persistent):
'''
The main control function for BAC, Cowculate() returns the final input.
It takes a GameState object, a plan, and returns a controller_input object.
Cowculate will be the framework of all decision making, and will be the highest level of abstraction.
'''
controller_input = SimpleControllerState()
current_state = game_info.me
#############################################################################
if plan.layers[0] == "Boost":
'''
Decide how to get the boost we want to go for.
'''
#TODO: Optimize these, or phase them out for RLU or similar.
wobble = Vec3(current_state.omega.x, current_state.omega.y,
0).magnitude()
epsilon = 0.3
target_boost = None
if type(plan.layers[1]) == int:
target_boost = game_info.boosts[plan.layers[1]]
elif plan.layers[1] == "Pads":
#This will skip the target boost loop and follow the path
pass
if target_boost != None:
angle_to_boost = atan2((target_boost.pos - current_state.pos).y,
(target_boost.pos - current_state.pos).x)
facing_boost = angles_are_close(angle_to_boost,
current_state.rot.yaw, pi / 12)
grounded_facing_boost = facing_boost and current_state.wheel_contact
#Turn towards boostI
controller_input = GroundTurn(
current_state,
current_state.copy_state(pos=target_boost.pos)).input()
if 1000 < current_state.vel.magnitude(
) < 2250 and facing_boost and wobble < epsilon and (
current_state.pos - target_boost.pos
).magnitude() > 1000 and abs(current_state.omega.z) < epsilon:
#If slow, not wobbling from a previous dodge, facing towards the boost,
#and not already at the boost, dodge for speed
controller_input = FrontDodge(current_state).input()
elif current_state.vel.magnitude() < 2300 and (
grounded_facing_boost
or current_state.rot.pitch < -pi / 12):
controller_input.boost = 1
elif plan.path == None:
#Copied the "go to net" code because I don't plan on really improving this for now.
#This section will be greatly improved once I have ground recovery code in place.
#TODO: Reocvery code into picking a path more intelligently.
center_of_net = Vec3(0, -5120, 0)
#Turn towards the center of our net
controller_input = GroundTurn(
current_state,
current_state.copy_state(pos=center_of_net)).input()
#Variables to check if we want to flip for speed.
displacement_from_net = center_of_net - current_state.pos
distance_to_net = displacement_from_net.magnitude()
angle_to_net = atan2(displacement_from_net.y,
displacement_from_net.x)
facing_net = angles_are_close(angle_to_net, current_state.rot.yaw,
pi / 12)
speed = current_state.vel.magnitude()
if distance_to_net > 1500 * (
(speed + 500) / 1410) and 1000 < speed < 2000 and facing_net:
controller_input = FrontDodge(current_state).input()
elif current_state.boost > 60 and facing_net and current_state.wheel_contact and speed < 2300:
controller_input.boost = 1
#If we start to go up the wall on the way, turn back down.
if current_state.wheel_contact:
if current_state.rot.roll > 0.15:
controller_input.steer = 1
elif current_state.rot.roll < -0.15:
controller_input.steer = -1
else:
controller_input = plan.path.input()
#############################################################################
elif plan.layers[0] == "Goal":
'''
Decide how to go to or wait in net
'''
#Useful locations
center_of_net = Vec3(0, -5120, 0)
if game_info.ball.pos.x > 0:
far_post = Vec3(-1150, -5120 + 300, 0)
far_boost = game_info.boosts[3].pos
else:
far_post = Vec3(1150, -5120 + 300, 0)
far_boost = game_info.boosts[4].pos
if plan.layers[1] == "Go to net":
#Turn towards the center of our net
controller_input = GroundTurn(
current_state,
current_state.copy_state(pos=center_of_net)).input()
#Variables to check if we want to flip for speed.
displacement_from_net = center_of_net - current_state.pos
distance_to_net = displacement_from_net.magnitude()
angle_to_net = atan2(displacement_from_net.y,
displacement_from_net.x)
facing_net = angles_are_close(angle_to_net, current_state.rot.yaw,
pi / 12)
speed = current_state.vel.magnitude()
if distance_to_net > 1500 and 1000 < speed < 2000 and facing_net:
controller_input = FrontDodge(current_state).input()
elif current_state.boost > 60 and facing_net and current_state.wheel_contact and speed < 2300:
controller_input.boost = 1
#If we start to go up the wall on the way, turn back down.
if current_state.wheel_contact:
if current_state.rot.roll > 0.15:
controller_input.steer = 1
elif current_state.rot.roll < -0.15:
controller_input.steer = -1
elif plan.layers[1] == "Wait in net":
if plan.layers[2] == "Prep for Aerial":
controller_input = SimpleControllerState()
else:
ball_angle = atan2((game_info.ball.pos - current_state.pos).y,
(game_info.ball.pos - current_state.pos).x)
#Go to net, stop in the middle, then turn in place to face the ball.
#TODO: Improve NavigateTo or replace completely
rot = Orientation(pyr=[
current_state.rot.pitch, ball_angle, current_state.rot.roll
])
target_state = current_state.copy_state(pos=center_of_net,
rot=rot)
controller_input = NavigateTo(current_state,
target_state).input()
#############################################################################
elif plan.layers[0] == "Ball":
'''
Calculate how to go for the ball as decided in planning.
'''
if plan.layers[1] == "Challenge":
#TODO: Intelligent challenges.
if current_state.wheel_contact:
#If still on ground, jump
controller_input.jump = 1
else:
#Once we've jumped, dodge towards the ball
controller_input = AirDodge(
car_coordinates_2d(current_state,
game_info.ball.pos - current_state.pos),
current_state.jumped_last_frame).input()
'''
elif plan.layers[1] == "Save" and plan.layers[2] == "Backwards":
controller_input = GroundTurn(current_state,
current_state.copy_state(pos = game_info.ball.pos),
can_reverse = True).input()
'''
elif plan.layers[2] == "Aerial":
controller_input, persistent = aerial(game_info.dt,
game_info.team_sign,
persistent)
else:
#TODO: Replace all of this with shooting/clearing/better code.
#Need pathing to get to a reasonable spot, and intelligent dodges to place the ball properly.
for i in range(100):
#Adjust for Ball/Car radii
ball_vel = ball_prediction.slices[i].vel
try:
#Evil magic numbers. TODO: Actual timing and arrival prediction.
target_pos = ball_prediction.slices[
i].pos + ball_vel.normalize().scalar_multiply(150)
except ZeroDivisionError:
target_pos = ball_prediction.slices[i].pos
car_target_vector = target_pos - current_state.pos
turn_angle = abs(
current_state.rot.yaw -
atan2(car_target_vector.y, car_target_vector.x))
time_estimate = linear_time_to_reach(
game_info,
target_pos) - (ball_vel.magnitude() / 50) * (turn_angle)
if ball_prediction.slices[i].time < time_estimate + 1 / 30:
break
if plan.layers[1] == "Shot":
#If the opponent isn't close to the ball, reposition to shoot towards net
#Find the center of the opponent's net
center_of_net = Vec3(0, 5120, game_info.ball.pos.z)
#If the ball is left, go right, and vice versa.
if game_info.ball.pos.x > 0:
shooting_correction = (
60 * (5120 - abs(game_info.ball.pos.y))) / (
(game_info.ball.pos - center_of_net).magnitude())
else:
shooting_correction = -(
60 * (5120 - abs(game_info.ball.pos.y))) / (
(game_info.ball.pos - center_of_net).magnitude())
target_pos = Vec3(target_pos.x + shooting_correction,
target_pos.y, target_pos.z)
#Make sure we don't try to go to a point outside the map
target_pos = Vec3(cap_magnitude(target_pos.x, 4096),
cap_magnitude(target_pos.y, 5120), target_pos.z)
#Turn towards the target
controller_input = GroundTurn(
current_state,
current_state.copy_state(pos=target_pos)).input()
#If we're not supersonic, and we're facing roughly towards the ball, boost.
ball_angle = atan2((game_info.ball.pos - current_state.pos).y,
(game_info.ball.pos - current_state.pos).x)
if current_state.vel.magnitude(
) < 2250 and current_state.wheel_contact and angles_are_close(
current_state.rot.yaw, ball_angle, pi / 4):
controller_input.boost = 1
#############################################################################
elif plan.layers[0] == "Recover":
if plan.layers[1] == "Air":
#If we're in the air, and not trying to hit the ball, recover.
controller_input, persistent = aerial_rotation(
game_info.dt, persistent)
elif plan.layers[1] == "Ground":
controller_input = GroundTurn(
current_state,
current_state.copy_state(pos=Vec3(0, -5120, 0))).input()
#TODO: Work on have_steering_control, powersliding, etc.
return controller_input, persistent