def InGoalArea():
global phase
mot.normalPose()
ball = visThread.findBall()
if ball:
(x,y) = ball
print x,y
if x < 0.17 and -0.06 < y < -0.02:
print 'Kick'
# BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE CORRECTION
mot.killWalk()
mot.rKickAngled(0)
phase = 'ReturnToGoal'
else:
# hacked influencing of perception, causing walking forward to have priority
theta = math.atan(y/x) / 2.5
x = 3 * (x-0.17)
if -0.1 < y < 0.1 :
y = 0.5 * y
else:
y = 2.0 * y
mot.SWTV(x , y, theta, 1.0)
else:
print 'No Ball'
mot.killWalk()
phase = 'ReturnToGoal'
def run(samples = 100):
motions.stiff()
motions.normalPose()
timeStamp = time.time()
checkLeft = True
checkRight = True
speech.say("Hello, I am overmind")
while(True):
(left, right) = getData(samples)
print "Right:", right, "Left:", left
if (wall(left, right)):
print "Wall detected"
if (checkLeft):
print "Checking right"
motions.walkTo((0, 0, -1.815))
checkLeft = False
elif (checkRight):
print "Checking left"
motions.walkTo((0, 0, 2.78))
checkRight = False
speech.say("I have escaped this maze like a boss")
else:
print "Checking back"
motions.walkTo((0, 0, 1.39))
else:
print "Walking straight"
motions.SWTV((0.5, 0, -0.0525, 0.8))
checkLeft = True
checkRight = True
def Ready():
"""Ready state: possible positioning
ledProxy: Chest Blue
"""
global firstCall
# Reinitialize phases, stand ready for match
if firstCall["Ready"]:
# stop ballfinding, there is no ball anyway
visThread.stopScan()
print "In ready state"
mHandler.killWalk()
# Initial pose
mot.stiff()
if playerType == 0:
mot.normalPose()
else:
mot.keepNormalPose()
firstCall["Initial"] = True
firstCall["Ready"] = False
firstCall["Set"] = True
firstCall["Playing"] = True
firstCall["Penalized"] = True
firstCall["FirstPress"] = True
def Set():
global control
global teamColor
global kickOff
global phase
global firstCall
control = [0,0,0]
# if the first iteration
if firstCall['Set']:
# update info about team, it is possible that game is started in set phase
(teamColor, kickOff, penalty) = sc.getMatchInfo()
print 'In set state'
visThread.startScan() # start ballscan
print 'TeamColor: ' , teamColor # print teamcolor as a check
# Initial pose, if not already in it
mot.stiff()
if playerType == 0:
mot.normalPose()
else:
mot.keepNormalPose()
audProxy.setOutputVolume(0) # set volume to zero
ledProxy.fadeRGB('ChestLeds', 0x00ffff00, 0) # set chestledcolor to green
# Team color must be displayed on left foot (Reference to rules)
if (teamColor == 0):
ledProxy.fadeRGB('LeftFootLeds', 0x000000ff, 0)
else:
ledProxy.fadeRGB('LeftFootLeds', 0x00ff0000, 0)
firstCall['Initial'] = True
firstCall['Ready'] = True
firstCall['Set'] = False
firstCall['Playing'] = True
firstCall['Penalized'] = True
# Head movements allowed in the set state
# FIND A BALL #
if phase != 'BallFound' and phase != 'BallFoundKeep':
if vis.scanCircle(visThread, 0.2):
if playerType == 0:
phase = 'BallFound'
else:
phase = 'BallFoundKeep'
# Keep finding the ball, it might move (or does it?)
if not(visThread.findBall()):
if playerType == 0:
phase = 'BallNotFound'
else:
phase = 'BallNotFoundKeep'
def Playing():
"""Playing state: play game according to phase transitions
ledProxy: Chest Green
"""
global phase
global firstCall
if firstCall["Set"]:
mot.stiff() # stiffness on (if it wasnt already)
mot.setHead(0, 0) # head in normal position
# if a regular player
if playerType == 0:
phase = "BallNotFound"
mot.normalPose(True) # normalPose, forcing call
# if a keeper
else:
phase = "BallNotFoundKeep"
mot.keepNormalPose()
firstCall["Set"] = False
if firstCall["Playing"]:
mHandler.killWalk()
visThread.startScan()
print "In playing state"
firstCall["Initial"] = True
firstCall["Ready"] = True
firstCall["Set"] = True
firstCall["Playing"] = False
firstCall["Penalized"] = True
if mot.standUp():
print "Fallen"
try:
if memProxy.getData("dntAction"):
phase = memProxy.getData("dntAction")
print "coach says: " + phase
else:
print "coach says: nothing"
except:
pass
# Execute the phase as specified by phase variable
phases.get(phase)()
def Set():
"""Set state: start searching for ball. CAUTION: Game is started in Set phase
instead of Initial in penalty shootout!
ledProxy: Chest Yellow
"""
global teamColor
global kickOff
global phase
global firstCall
# if the first iteration
if firstCall["Set"]:
mHandler.killWalk()
# update info about team, it is possible that game is started in set phase
(teamColor, kickOff, penalty) = gsc.getMatchInfo()
print "In set state"
visThread.startScan() # start ballscan
print "TeamColor: ", teamColor # print teamcolor as a check
# Initial pose, if not already in it
mot.stiff()
if playerType == 0:
mot.normalPose()
else:
mot.keepNormalPose()
# audProxy.setOutputVolume(0) # set volume to zero
firstCall["Initial"] = True
firstCall["Ready"] = True
firstCall["Set"] = False
firstCall["Playing"] = True
firstCall["Penalized"] = True
# Head movements allowed in the set state
# FIND A BALL #
ball = vis.scanCircle(visThread)
if ball:
mHandler.setBallLoc(ball)
if playerType == 0:
phase = "BallFound"
else:
phase = "BallFoundKeep"
def Initial():
global phase # to change, call global, else not possible
global ball_loc
global firstCall
if firstCall['Initial']:
print 'In initial state'
visThread.stopScan() # do not find the ball when in Initial
print 'TeamColor: ' , teamColor # print the teamcolor as a check
# Empty variables
ball_loc = dict()
# Team color must be displayed on left foot (Reference to rules)
ledProxy.off('AllLeds')
if (teamColor == 0):
ledProxy.fadeRGB('LeftFootLeds', 0x000000ff, 0)
else:
ledProxy.fadeRGB('LeftFootLeds', 0x00ff0000, 0)
mot.stiff() # stiffness on (if it wasnt already)
mot.setHead(0,0) # head in normal position
# if a regular player
if playerType == 0:
phase = 'BallNotFound'
mot.normalPose(True) # normalPose, forcing call
# if a keeper
else:
phase = 'BallNotFoundKeep'
mot.keepNormalPose()
firstCall['Initial'] = False
firstCall['Ready'] = True
firstCall['Set'] = True
firstCall['Playing'] = True
firstCall['Penalized'] = True
def run(samples=100):
motions.stiff()
motions.normalPose()
timeStamp = time.time()
check = True
while (time.time() - timeStamp < 1000):
(left, right) = getData(samples)
print "Right:", right, "Left:", left
deltaL = left - right
deltaR = right - left
print "DeltaR:", deltaR, "DeltaL:", deltaL
if ((left < 40) and (right < 40)):
print "WALL DETECTED"
if (check):
print "CHECK RIGHT"
motions.walkTo((0, 0, -1.33))
check = False
else:
print "CHECK LEFT"
motions.SWTV((0, 0, 0.25, 0.7))
elif ((right >= 40) and (left >= 40)):
check = True
print "WALKING STRAIGHT"
motions.SWTV((0.5, 0, 0, 0.5))
elif ((deltaR < 0 and deltaR > -5)):
print "ADJUST ANGLE TO LEFT"
motions.SWTV((0.5, 0, 0.05, 0.5))
elif ((deltaL < 0 and deltaL > -5)):
print "ADJUST ANGLE TO RIGHT"
motions.SWTV((0.5, 0, -0.05, 0.5))
else:
print "STOP"
motions.walkTo((0.01, 0, 0))
print "STOP"
motions.walkTo((0.01, 0, 0))
def Kick():
global phase
memProxy.insertData('dntPhase', 'Kick')
visThread.stopScan()
ledProxy.fadeRGB('LeftFaceLeds',0x00000000, 0) # no goal yet, left led turns black
ledProxy.fadeRGB('RightFaceLeds', 0x00ff0000, 0) # no ball anymore, right led turns red
# scan for a goal
goal = vis.scanCircleGoal()
motProxy.post.angleInterpolation(['HeadPitch', 'HeadYaw'], [[0.5], [0]], [[0.15], [0.15]], True)
visThread.startScan()
# Case 0 : Ball stolen/lost.
# Check if the ball is still there, wait until ball is found or 1 second has passed
now = time.time()
while time.time() - now < 1 and not(visThread.findBall()):
pass
ball = visThread.findBall()
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:
mot.walkTo(0,0.04 + ball[1],0)
mot.rKickAngled(0)
phase = 'BallNotFound'
else:
print "Kick phase:", goal
ledProxy.fadeRGB('RightFaceLeds', 0x0000ff00, 0) # no ball anymore, right led turns red
# else a goal is found, together with it's color
(color, kickangles ) = goal
if len(kickangles) == 2:
(first, second) = kickangles
kickangle = (3 * first + second) / 4.0 # kick slightly more towards left pole
else:
kickangle = kickangles[0]
if color == 'Blue':
goalColor = 0
ledProxy.fadeRGB('LeftFaceLeds',0x000000ff, 0) # blue goal
else:
goalColor = 1
ledProxy.fadeRGB('LeftFaceLeds',0x00ff3000, 0) # yellow goal , anyone got a better value?
# Cases 1-3, if you see your own goal, kick to the other side
if goalColor == teamColor:
# Case 1, goal is left, kick to the right.
if kickangle >= 0.7:
kickangle = -1
# Case 2, goal is right, kick to the left.
if kickangle <= -0.7:
kickangle = 1
else:
# Case 3, goal is straight forward, HAK
mot.walkTo(0,0.04 + ball[1],0)
mot.normalPose()
time.sleep(0.5)
ledProxy.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
mot.hakje(kickangle * -0.1)
phase = 'BallNotFound'
else:
# Case 4, other player's goal is found.
# Kick towards it.
if kickangle > 1.1:
kickangle = 1.1
if kickangle < -1.1:
kickangle = -1.1
# conversion to real kickangle
convert = 1.1 # (60 degrees rad)
# kick either left or right
if kickangle <= 0:
mot.walkTo(0, -0.04 + ball[1], 0)
mot.lKickAngled(kickangle/ -convert)
elif kickangle > 0:
mot.walkTo(0, 0.04 + ball[1], 0)
mot.rKickAngled(kickangle/ convert)
ledProxy.fadeRGB('LeftFaceLeds',0x00000000, 0)
phase = 'BallNotFound'
