def teammateMinWeightedDistToBall():
minDist = robot.FULL_FIELD_LENGTH
ballPosAbs = blackboard.localisation.ballPos
lostCount = blackboard.localisation.ballLostCount
robotPos = blackboard.localisation.robotPos
if lostCount < 100:
robotObs = blackboard.localisation.robotObstacles
for i in range(0,len(robotObs)):
isFriend = isTeamMate(robotObs[i])
# Find position of the robots in absolute terms and discard if near the end of field (false positive)
robotObsAbs = MathUtil.rrToAbs(robotPos, robotObs[i].rr)
if robotObsAbs[0] > 2800:
isFriend = False
if isFriend:
dist = math.hypot(ballPosAbs.x - robotObsAbs[0], ballPosAbs.y - robotObsAbs[1])
if dist < minDist:
minDist = dist
return minDist
def teammateMinWeightedDistToBall():
minDist = robot.FULL_FIELD_LENGTH
ballPosAbs = blackboard.localisation.ballPos
lostCount = blackboard.localisation.ballLostCount
robotPos = blackboard.localisation.robotPos
if lostCount < 100:
robotObs = blackboard.localisation.robotObstacles
for i in range(0, len(robotObs)):
isFriend = isTeamMate(robotObs[i])
# Find position of the robots in absolute terms and discard if near the end of field (false positive)
robotObsAbs = MathUtil.rrToAbs(robotPos, robotObs[i].rr)
if robotObsAbs[0] > 2800:
isFriend = False
if isFriend:
dist = math.hypot(ballPosAbs.x - robotObsAbs[0],
ballPosAbs.y - robotObsAbs[1])
if dist < minDist:
minDist = dist
return minDist
def angleToGoalShot(ballPosAbs, robotPos, robotObstacles, commitToKick):
goalRight = angleToGoalRight(ballPosAbs)
goalLeft = angleToGoalLeft(ballPosAbs)
pointsOfAim = [[angleToGoal(ballPosAbs), -1.0, Kick.MIDDLE],
[goalLeft - math.radians(10), -1.0, Kick.LEFT],
[goalRight + math.radians(10), -1.0, Kick.RIGHT]]
# If we've committed to a kick already
if Kick.MIDDLE <= commitToKick <= Kick.RIGHT:
return pointsOfAim[commitToKick]
# If goal angle is so fine that we can only kick for the middle anyway
if pointsOfAim[Kick.LEFT][0] < pointsOfAim[Kick.MIDDLE][0] or pointsOfAim[
Kick.RIGHT][0] > pointsOfAim[Kick.MIDDLE][0]:
return pointsOfAim[Kick.MIDDLE]
nearestDist = 10000
result = pointsOfAim[Kick.MIDDLE]
for i in range(0, len(robotObstacles)):
# Find position of the robots in ball relative terms
ball = (ballPosAbs.x, ballPosAbs.y, 0.0)
rr = MathUtil.absToRr(ball,
MathUtil.rrToAbs(robotPos, robotObstacles[i].rr))
dist = rr[0]
heading = rr[1]
if pointsOfAim[Kick.LEFT][0] > heading > pointsOfAim[Kick.RIGHT][0]:
if dist < nearestDist:
nearestDist = dist
if heading > pointsOfAim[Kick.MIDDLE][0]:
# Robot is to the left
#print "Aiming right"
result = pointsOfAim[Kick.RIGHT]
else:
#print "Aiming left"
# Robot is to the right
result = pointsOfAim[Kick.LEFT]
return result
def angleToGoalShot(ballPosAbs, robotPos, robotObstacles, commitToKick):
goalRight = angleToGoalRight(ballPosAbs)
goalLeft = angleToGoalLeft(ballPosAbs)
pointsOfAim = [[angleToGoal(ballPosAbs), -1.0, Kick.MIDDLE],
[goalLeft - math.radians(10), -1.0, Kick.LEFT],
[goalRight + math.radians(10), -1.0, Kick.RIGHT]]
# If we've committed to a kick already
if Kick.MIDDLE <= commitToKick <= Kick.RIGHT:
return pointsOfAim[commitToKick]
# If goal angle is so fine that we can only kick for the middle anyway
if pointsOfAim[Kick.LEFT][0] < pointsOfAim[Kick.MIDDLE][0] or pointsOfAim[Kick.RIGHT][0] > pointsOfAim[Kick.MIDDLE][0]:
return pointsOfAim[Kick.MIDDLE]
nearestDist = 10000
result = pointsOfAim[Kick.MIDDLE]
for i in range(0,len(robotObstacles)):
# Find position of the robots in ball relative terms
ball = (ballPosAbs.x, ballPosAbs.y, 0.0)
rr = MathUtil.absToRr(ball, MathUtil.rrToAbs(robotPos, robotObstacles[i].rr))
dist = rr[0]
heading = rr[1]
if pointsOfAim[Kick.LEFT][0] > heading > pointsOfAim[Kick.RIGHT][0]:
if dist < nearestDist:
nearestDist = dist
if heading > pointsOfAim[Kick.MIDDLE][0]:
# Robot is to the left
#print "Aiming right"
result = pointsOfAim[Kick.RIGHT]
else:
#print "Aiming left"
# Robot is to the right
result = pointsOfAim[Kick.LEFT]
return result
