def go_to_point_behind_ball():
desiredPoint = MotionSkills.getPointBehindBall(ball, param.AWAY_GOAL)
while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.13) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.13)) or \
(team1_robot_state.pos_y_est > (desiredPoint.y + 0.13) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.13)):
robot_point = Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est)
desiredPoint = MotionSkills.getPointBehindBall(ball, param.AWAY_GOAL)
targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), param.AWAY_GOAL)
radian180 = param.degreeToRadian(180)
radian360 = param.degreeToRadian(360)
radian5 = param.degreeToRadian(5)
anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180
command = MotionSkills.go_to_point(team1_robot_state, desiredPoint)
angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL)
omega = angular_command.omega
if(anglediff <= radian5 and anglediff >= -radian5):
omega = 0
velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est)
time.sleep(robot_update_delay)
angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL)
velchange.goXYOmega(0,0,angular_command.omega)
time.sleep(angular_command.runTime)
velchange.goXYOmega(0,0,0)
def follow_behind_ball():
mutex.acquire()
while True:
desiredPoint = MotionSkills.getPointBehindBall(ball)
if (team1_robot_state.pos_x_est > (desiredPoint.x + 0.05) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.05)) or \
(team1_robot_state.pos_y_est > (desiredPoint.y + 0.05) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.05)):
go_to_point_behind_ball()
