def BallFound():
global phase
global goalPosKnown
# FIND A BALL #
seen = False
ball = visThread.findBall()
if ball:
seen = True
# print 'RealPosition', ball
else:
ball = mHandler.getKalmanBallPos()
if firstCall["BallFound"]:
mot.setFME(True)
firstCall["BallFound"] = False
firstCall["BallNotFound"] = True
# initialize mean mu for kalman filter
if seen:
mHandler.setBallLoc(ball)
mHandler.setBallLoc(ball)
(x, y) = ball
memProxy.insertData("dntPhase", "BallFound")
if seen and x < 0.17 and -0.016 < y < 0.016:
print "Kick"
# BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE CORRECTION
mHandler.killWalk()
phase = "Kick"
else:
if seen:
print "Kalman position", x, y
try:
theta = math.atan(y / x) # TypeError: unsupported operand type(s) for /: 'list' and 'list'??
except:
theta = float(1)
y = y[0]
x = x[0]
print "error in theta calculation"
# hacked influencing of perception, causing walking forward to have priority
theta *= 0.4
x = 3.0 * (x - 0.17)
y *= 0.6
mHandler.sWTV(x, y, theta, max(1 - x, 0.85))
time.sleep(0.15)
goalPosKnown = False, None
else:
print "Not seen ball", x, y
try:
theta = math.atan(y / x) # TypeError: unsupported operand type(s) for /: 'list' and 'list'??
except:
y = y[0]
theta = float(1)
print "error in theta calculation"
mHandler.postWalkTo(0, 0, math.copysign(theta, y) / 2.0)
# TODO determine where ball is in headposition and look there instead of random places
if not vis.scanCircle(visThread):
phase = "BallNotFound"
def BallNotFoundKeep():
""" Keeper lost track of ball
"""
global phase
global ball_loc
mot.setFME(False)
visThread.startScan()
# if a ball is found
if vis.scanCircle(visThread):
# reinitialize
ball_loc = list()
phase = "BallFoundKeep"
firstCall["BallNotFoundKeep"] = True
elif firstCall["BallNotFoundKeep"]:
memProxy.insertData("dntBallDist", 0)
mot.keepNormalPose()
firstCall["BallNotFoundKeep"] = False
def Kick():
global phase
global goalPosKnown
mot.setFME(False)
memProxy.insertData("dntPhase", "Kick")
visThread.stopScan()
# scan for a goal
goal = None
if goalPosKnown[0]:
goal = goalPosKnown[1]
else:
goal = vis.scanCircleGoal()
if goal:
goalPosKnown = (True, goal)
print "Kick phase: ", goal
mot.setHead(0, 0.45)
visThread.startScan()
ball = vis.scanCircle(visThread)
# Case 0 : Ball stolen/lost.
# Check if the ball is still there, wait until ball is found has passed
# Something of a mean, ...
if not ball:
print "Ball gone"
phase = "BallNotFound"
elif ball[0] > 0.25 or ball[1] > 0.1 or ball[1] < -0.1:
print "Ball too far"
phase = "BallFound"
elif ball and not goal:
mHandler.kick(0)
phase = "BallNotFound"
else:
# else a goal is found, together with it's color
(color, kickangles) = goal
if type(kickangles[0]) == tuple:
(first, second) = kickangles
kickangle = (3 * first[0] + second[0]) / 4.0 # kick slightly more towards left pole
else:
kickangle = kickangles[0]
if color == "Blue":
goalColor = 0
else:
goalColor = 1
# use kalman filter to position ball in 10 measurements
c = 0
while c < 10:
c += 1
ball = visThread.findBall()
mHandler.setBallLoc(ball)
ball = mHandler.getKalmanBallPos()
# Cases 1-3, if you see your own goal, kick to the other side OR if the keeper saw the ball
print "keep saw ball: " + str(memProxy.getData("dntKeepSawBall"))
if memProxy.getData("dntKeepSawBall"):
# Case 1, goal is left, kick to the right.
if kickangle >= 0.7:
mHandler.kick(-1.1)
# Case 2, goal is right, kick to the left.
if kickangle <= -0.7:
mHandler.kick(1.1)
else:
# Case 3, goal is straight forward, HAK, but only in the first 2 minutes of each half.
if gsc.getSecondsRemaining() > 8:
mHandler.kick(1.1)
else:
mHandler.kick(1.1)
else:
# Case 4, other player's goal is found.
# Kick towards it.
# general kick interface distributes kick
if abs(kickangle < 0.6):
mHandler.walkTo(0, math.copysign(0.03, kickangle), 0)
while c < 10:
c += 1
ball = visThread.findBall()
mHandler.setBallLoc(ball)
ball = mHandler.getKalmanBallPos()
print "Goalcolor", goalColor, "KickAngle", kickangle
print "TeamColor", teamColor, "Locatie", ball
mHandler.kick(kickangle)
phase = "BallNotFound"
ledProxy.fadeRGB("LeftFaceLeds", 0x00000000, 0) # no goal yet, left led turns black
def BallFoundKeep():
global phase
global ball_loc
global firstCall
if firstCall["BallFoundKeep"]:
mot.stance()
firstCall["BallFoundKeep"] = False
mot.setFME(False)
maxlength = 6
halfmaxlength = 3
# FIND A BALL #
ball = visThread.findBall()
if ball:
# if ball[0] < 0.3 and (ball[1] < 0.5 or ball[1] > -0.5):
# phase = 'InGoalArea'
# mot.move('normalPose')
# visThread.findBall()
# return True
(x, y) = ball
length = len(ball_loc)
if length == maxlength:
ball_loc = ball_loc[1:]
ball_loc.append(ball)
else:
ball_loc.append(ball)
if length == maxlength:
xold = 0
yold = 0
xnew = 0
ynew = 0
# shift elements sidewards, newest ballloc becomes nr <maxlength>, oldest is thrown away
for number in range(maxlength):
(x, y) = ball_loc[number]
# add to xold/new and yold/new variables, number 0 is oldest, number <maxlength> most recent ballloc
if 0 <= number < halfmaxlength:
xold += x
yold += y
else:
xnew += x
ynew += y
# calculate the mean of measurements
xold /= halfmaxlength
yold /= halfmaxlength
xnew /= halfmaxlength
ynew /= halfmaxlength
# calc diff in distance
distold = math.sqrt(xold ** 2 + yold ** 2)
distnew = math.sqrt(xnew ** 2 + ynew ** 2)
speed = distold - distnew
print "Ball moving from ", xold, yold, "to", xnew, ynew, "(mean). Speed", speed, "Distnew: ", distnew
# calculate direction if speed is high enough
if speed > 0.23 or (distnew < 0.8 and speed > 0.05):
# This is all triangular magic using similarity between two triangles formed by balllocations and Nao.
# Keep in mind that y-axis is inverted and that x is forward, y is sideways!
#
# x
# (yold,xold) |
# . |
# | \ |
# B | \ |
# | \ |
# |_______. (ynew,xnew ) |
# A | \ |
# | \ |
# C | \ |
# | \ |
# y _____________|_________._______________.___________ -y
# (dir,0) Nao = (0,0)
#
# [--- D ---]
# Mathematical proof:
# A = yold - ynew
# B = xold - xnew
# C = xnew
# D = ynew - dir
# Similar triangles -> C / B = D / A
# D = A*C/B
# D = ynew - dir
# A*C/B = ynew - dir
# dir = A*C/B - ynew
dir = (yold - ynew) * xnew / (xold - xnew) - ynew
print "Direction", dir
# if a direction has been found, clear all variables
mHandler.dive(dir)
phase = "BallNotFoundKeep"
ball_loc = list()
visThread.clearCache()
firstCall["BallFoundKeep"] = True
print "Direction", dir
else:
phase = "BallNotFoundKeep"
firstCall["BallFoundKeep"] = True
