def Finished():
visThread.stopScan() # stop looking for the ball
mHandler.killWalk() # stop walking
mot.stance() # sit down
mot.setHead(0, 0) # set head straight
time.sleep(1) # wait until seated etc
mot.killKnees() # stiffness off to prevent heating
if not gsc.getSecondaryState(): # if not penalty shootout
gsc.close()
audProxy.setOutputVolume(85) # volume on
def Finished():
visThread.stopScan() # stop looking for the ball
ledProxy.off('AllLeds') # turn off ledProxy
mot.killWalk() # stop walking
mot.stance() # sit down
mot.setHead(0,0) # set head straight
time.sleep(1) # wait until seated etc
mot.killKnees() # stiffness off to prevent heating
if not sc.getSecondaryState():
sc.close()
audProxy.setOutputVolume(85) # volume on
def BallFoundKeep():
global phase
global ball_loc
global firstCall
if firstCall['BallFoundKeep']:
mot.stance()
firstCall['BallFoundKeep'] = False
maxlength = 6
halfmaxlength = (maxlength/2.0)
# 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
memProxy.insertData('dntBallDist', math.sqrt(x**2 + y**2))
if len(ball_loc) == 0:
ball_loc[ 0 ] = ball
elif len(ball_loc) < maxlength:
# add to dict, key being a number from maxlength to 0. Last position is position 8 (key 7)
# but only if ball_loc is already filled and ball is a new location
if ball != ball_loc[ len(ball_loc) - 1 ]:
ball_loc[ len(ball_loc) ] = ball
elif ball != ball_loc[ maxlength - 1 ]:
xold = 0
yold = 0
xnew = 0
ynew = 0
# shift elements sidewards, newest ballloc becomes nr <maxlength>, oldest is thrown away
for number in ball_loc:
(x,y) = ball_loc[number]
if number != 0:
ball_loc[number-1] = (x,y)
# add to xold/new and yold/new variables, number 0 is oldest, number <maxlength> most recent ballloc
if number == 0:
pass
elif 0 < number < halfmaxlength + 1:
xold += x / halfmaxlength
yold += y / halfmaxlength
else:
xnew += x / halfmaxlength
ynew += y / halfmaxlength
xnew += ball[0] / halfmaxlength
ynew += ball[1] / halfmaxlength
ball_loc[maxlength-1] = ball
# 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
# calculate direction if speed is high enough
if speed > 0.225:
#mot.stiffKnees()
# 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
# dir = A*C/B - ynew
dir = (yold - ynew )* xnew / (xold - xnew) - ynew
# if a direction has been found, clear all variables
ball_loc = dict()
visThread.clearCache()
ball_loc['Direction'] = dir
phase = 'BallApproaching'
firstCall['BallFoundKeep'] = True
print 'Direction', dir
else:
#mot.stiffKnees()
phase = 'BallNotFoundKeep'
firstCall['BallFoundKeep'] = True
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
