def calculateTimeToReachBall(): interceptPoint = getBallIntersectionWithRobot(maintainCanSeeBall=False) interceptToGoal = ENEMY_GOAL_CENTER.minus(interceptPoint) interceptToGoalHeading = normalisedTheta( math.atan2(interceptToGoal.y, interceptToGoal.x)) return calculateTimeToReachPose(Global.myPos(), Global.myHeading(), interceptPoint, interceptToGoalHeading)
def getBallIntersectionWithRobot(maintainCanSeeBall=True): intervalInSeconds = 1 numSecondsForward = 1.0 # Estimate the ball position up to 1 second away numIterations = int(round(numSecondsForward / intervalInSeconds)) FRICTION = 0.9 # friction per second FRICTION_PER_ITERATION = FRICTION**intervalInSeconds ballVel = Global.ballWorldVelHighConfidence() ballPos = Global.ballWorldPos() myHeading = Global.myHeading() # If he ball is moving slowly, just chase the ball directly if ballVel.isShorterThan(10.0): return ballPos # Dont bother chasing a moving ball if its quite close. if Global.ballDistance() < 600.0: return ballPos ballVel.scale(intervalInSeconds) robotPos = Global.myPos() interceptPoint = ballPos bestChasePoint = ballPos.clone() seconds = 0.0 for i in xrange(0, numIterations): # noqa seconds += intervalInSeconds interceptPoint.add(ballVel) ballVel.scale(FRICTION_PER_ITERATION) toIntercept = interceptPoint.minus(robotPos) toInterceptHeading = math.atan2(toIntercept.y, toIntercept.x) # How far we need to turn to point at the interceptPoint toInterceptTurn = abs(normalisedTheta(toInterceptHeading - myHeading)) timeToTurn = toInterceptTurn / TURN_RATE timeToWalk = toIntercept.length() / WALK_RATE canReach = (timeToTurn + timeToWalk) <= seconds # Calculate difference in heading to the current ball position and # the intersect position, to make sure we don't turn too far and # lose sight of the ball v1 = interceptPoint.minus(robotPos).normalised() v2 = ballPos.minus(robotPos).normalised() heading = v1.absThetaTo(v2) if maintainCanSeeBall and heading > math.radians(75): return bestChasePoint if canReach: return bestChasePoint else: bestChasePoint = Vector2D.makeVector2DCopy(interceptPoint) return bestChasePoint
def calculateTimeToReachPose(myPos, myHeading, targetPos, targetHeading=None): toTarget = targetPos.minus(myPos) toTargetHeading = math.atan2(toTarget.y, toTarget.x) # How far we need to turn to point at the targetPos toTargetTurn = abs(normalisedTheta(toTargetHeading - myHeading)) # The straightline distance to walk to the targetPos toTargetDistance = toTarget.length() # How far we need to turn once we get to the targetPos so # that we are facing the targetHeading if targetHeading is None: toTargetHeadingTurn = 0.0 else: toTargetHeadingTurn = abs( normalisedTheta(toTargetHeading - targetHeading)) # approximate time it takes to avoid robots on the way avoidTime = 0 if toTargetDistance > 400: robots = Global.robotObstaclesList() for _robot in robots: robotPos = Vector2D.makeVector2DCopy(_robot.pos) toRobot = robotPos.minus(myPos) dist_squared = toRobot.length2() heading = abs(normalisedTheta(toRobot.heading() - toTargetHeading)) # further away robots are less relevant distValue = min(1, dist_squared / toTargetDistance**2) # robots aren't in the way are less relevant headingValue = min(1, heading / (math.pi / 2)) # heading is more relevant than distance, # has enough weighting to revert striker bonus time combinedValue = (1 - distValue**2) * (1 - headingValue**2) * 3 if combinedValue > avoidTime: avoidTime = combinedValue return (toTargetTurn / TURN_RATE + toTargetDistance / WALK_RATE + toTargetHeadingTurn / CIRCLE_STRAFE_RATE + avoidTime)
def angleToBallToGoal(absCoord): ball = blackboard.localisation.ballPos ballRR = blackboard.localisation.ballPosRR goalDir = angleToGoal(ball) return normalisedTheta(goalDir - (absCoord.theta + ballRR.heading))
def angleToGoal(absCoord): phi = Vector2D.angleBetween(ENEMY_GOAL_BEHIND_CENTER, Vector2D.makeVector2DCopy(absCoord)) return normalisedTheta(phi)
def angleToPoint(point, absCoord): phi = Vector2D.angleBetween(point, Vector2D.makeVector2DCopy(absCoord)) return normalisedTheta(phi)