def perform(dkd = 90, side = None, bx = None, by = None):
# This implementation is very similar to sGetBehindBall (based on 2003)
# but the ball is on the bottom edge of the circle, not the centre.
global onCircle
if side != None:
print "Warning: sGetBesideBall.perform: side is not yet implemented"
if bx == None or by == None:
(bx, by) = Global.gpsGlobalBall.getPos()
(myx, myy) = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
# Determine the centre of the circle, which is CIRCLE_RADIUS towards
# dkd from the ball. Global coords.
cx = bx + math.cos(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS
cy = by + math.sin(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS
# If we are backward of the ball or really close just run at it
ballRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
ballDSquared = hMath.getDistSquaredBetween(myx, myy, bx, by)
if (abs(ballRobotH) > 90 or ballDSquared < hMath.SQUARE(20)):
Indicator.showHeadColor(Indicator.RGB_PURPLE)
ballH = hMath.getHeadingBetween(myx, myy, bx, by)
hTrack.saGoToTargetFacingHeading(bx, by, ballH)
return
# Work out if we are left or right of the centre (relative to DKD as 0)
robotH = hMath.getHeadingToMe(cx, cy, dkd, myx, myy)
# FIXME: allow choice of direction
if (robotH > 0): # robot to the left
#print "robot to left of ball",
direction = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
direction = Constant.dCLOCKWISE
# The circling point can be calculated as looking from the centre
# towards the robot at CIRCLE_DEGREES to the left/right, and distance
# CIRCLE_RADIUS. CircleAng is from the centre facing the robot with
# positive x at zero degrees.
# There are two modes here. In the first we are well outside the and
# running to make a tangent with the circle. In the second we are close
# to or inside the circle and tracing the circumference
centreDSquared = hMath.getDistSquaredBetween(myx, myy, cx, cy)
if (centreDSquared > hMath.SQUARE(CIRCLE_RADIUS + 20)):
#print "Outside circle, running to tangent"
onCircle = False
Indicator.showHeadColor(Indicator.RGB_GREEN)
if direction == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
else:
#print "On circle, tracing circumference"
onCircle = True
Indicator.showHeadColor(Indicator.RGB_YELLOW)
if direction == Constant.dANTICLOCKWISE:
circleAng = 110
else:
circleAng = 70
# print "me", int(myx), int(myy), "ball", int(bx), int(by), \
# "centre", int(cx), int(cy), "robotH", int(robotH),
# relative to centre facing robot
circleRelX = math.cos(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS
circleRelY = math.sin(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS
#print "circleAng", circleAng, "rel circle pos", circleRelX, circleRelY
robotH = hMath.normalizeAngle_180(robotH + dkd) # now global
(circleX, circleY) = hMath.getGlobalCoordinate(cx, cy, robotH, \
circleRelX, circleRelY)
# print "gRobotH", robotH, "circle pos", int(circleX), int(circleY)
# circleX/Y now is the global coords of the circle point, so walk there.
# ballH = hMath.getHeadingBetween(myx, myy, bx, by) # global
if onCircle:
# Calls the walk directly to ensure smoothness: no stopping to turn
relX = circleX - myx
relY = circleY - myy
relD = hMath.getLength((relX, relY))
relTheta = hMath.RAD2DEG(hMath.getHeadingToRelative(relX, relY))
# Don't turn outwards much, even if we are inside the circle. Walking
# forward will put us back on it. Nobu can you fix this?
# print "relTheta", relTheta, "=>",
# if direction == Constant.dANTICLOCKWISE and relTheta < 0:
# #relTheta = hMath.CLIP(relTheta, 15)
# relTheta = 0
# elif direction == Constant.dCLOCKWISE and relTheta > 0:
# #relTheta = hMath.CLIP(relTheta, 15)
# relTheta = 0
# print relTheta
left = 0
if abs(relTheta) < 30:
Action.walk(Action.MAX_FORWARD, left, relTheta)
else:
Action.walk(Action.MAX_FORWARD, left, relTheta)
else:
hTrack.saGoToTarget(circleX, circleY)
def walkToStillBall(ballD, ballH, getBehind = GET_BEHIND_NONE):
global gLastTurnFrame
global gGoalie
if getBehind == GET_BEHIND_GOALIE:
getBehind = GET_BEHIND_PRIORITY
gGoalie = True
fwd = Action.MAX_FORWARD
# Lighting challenge and penalty shooter don't get behind. It makes the
# grab a bit harder and takes time
if Global.lightingChallenge or Global.penaltyShot:
getBehind = GET_BEHIND_NONE
# Don't get behind when close to our goal box since
# this moves us towards it (unless we are the goalie)
if not gGoalie and hWhere.selfInOwnGoalBox(20, sideoffset = 10):
getBehind = GET_BEHIND_NONE
# Calculate getBehind adjustment
leftAdj = 0
if getBehind > GET_BEHIND_NONE:
myx, myy = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
ballx, bally = hMath.getGlobalCoordinate(myx, myy, myh,
*hMath.polarToCart(ballD, ballH+90))
# In defense we get between our goal and the ball. In attack we line
# up the ball and target goal. In the (small) midfield we line up with
# upfield
reversePoints = False
if bally < Constant.FIELD_LENGTH/2 - 20 \
or hTeam.amIFurthestBack(ignoreRoles = []):
lineX, lineY = Constant.OWN_GOAL_X, Constant.OWN_GOAL_Y
elif bally > Constant.FIELD_LENGTH/2 + 20:
lineX, lineY = Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y
reversePoints = True
else:
lineX, lineY = ballx, 0
# Draw a line between ball and goal. Find self offset from the line
dist = ((ballx - lineX)*(lineY - myy)-(lineX - myx)*(bally - lineY)) /\
math.sqrt(hMath.SQUARE(ballx - lineX) \
+ hMath.SQUARE(bally - lineY))
# If dist > 0 we are to the right of the line, so adjustment left
# These adjustments will only be applied assuming we are facing
# the ball
if dist > 15:
leftAdj = 14
elif dist < -15:
leftAdj = -12
elif myy > bally: # Always sidestep from in front
if dist >= 0:
leftAdj = 14
elif dist < 0:
leftAdj = -12
else:
leftAdj = 0
if leftAdj != 0:
# At this point check that sidestepping in this direction is not
# going to put obstacles between us and the ball. If so it would be
# better to go direct (and let dodgy dog do its thing)
(lBallX, lBallY) = Global.gpsLocalBall.getPos()
nobs = VisionLink.getNoObstacleBetween(leftAdj, 0,
int(lBallX) + leftAdj,
int(lBallY) - 30,
abs(leftAdj), 0,
Constant.MIN_GPS_OBSTACLE_BOX,
Constant.OBS_USE_GPS)
if nobs > 2 * Constant.MIN_GPS_OBSTACLE_BOX:
#Indicator.showFacePattern([3, 2, 0, 2, 3])
leftAdj = 0
# Multiply getBehind in some cases. Only one of these can apply at
# at time so put them in order of importance.
leftAdjMult = 1
if getBehind == GET_BEHIND_DEFAULT:
# Last man attacker must be behind
if hTeam.amIFurthestBack(ignoreRoles = []):
if myy < bally:
getBehind = GET_BEHIND_PRIORITY
else:
getBehind = GET_BEHIND_LOTS
# In defense do lots of getBehind
elif bally < Constant.FIELD_LENGTH * 0.25:
getBehind = GET_BEHIND_LOTS
# From in front of the ball do a little more to get behind the
# ball
elif myy > bally:
getBehind = GET_BEHIND_MORE
# Lots of getBehind if requested
if getBehind >= GET_BEHIND_ONLY:
fwd = 10 # Walk in a circular arc
leftAdjMult = 2
elif getBehind >= GET_BEHIND_PRIORITY:
fwd = 30 # Slow down so getBehind has more effect
leftAdjMult = 2
elif getBehind >= GET_BEHIND_LOTS:
leftAdjMult = 2
elif getBehind >= GET_BEHIND_MORE:
leftAdjMult = 1.5
leftAdj *= leftAdjMult
# If the line was to a target to line up, reverse direction
if reversePoints:
leftAdj = -leftAdj
#Indicator.showFacePattern([0]*5)
# if leftAdj > 0:
# Indicator.showFacePattern([0, 0, 0, 2, 3])
# elif leftAdj < 0:
# Indicator.showFacePattern([3, 2, 0, 0, 0])
# if gGoalie:
# Indicator.setDefault()
# # DANGER CASE: Goalie Facing Backwards
# if gGoalie and (225 <= myh < 315):
# Indicator.showHeadColor(Indicator.RGB_ORANGE)
# #ball to goalie's right
# if ballx <= myx:
# print "Ball to my RIGHT"
# Action.walk(-5,Action.MAX_LEFT,-Action.MAX_TURN,
# "ddd",minorWalkType=Action.SkeFastForwardMWT)
# #ball to goalie's left
# else:
# print "Ball to my LEFT"
# Action.walk(-5,-Action.MAX_LEFT,Action.MAX_TURN,
# "ddd",minorWalkType=Action.SkeFastForwardMWT)
# Walk direct - with getBehind adjustment
if abs(ballH) < 30:
# if gGoalie:
# Indicator.showHeadColor(Indicator.RGB_GREEN)
Action.walk(fwd, leftAdj, ballH,
minorWalkType=Action.SkeFastForwardMWT)
# Walk and turn to face
elif abs(ballH) < 80:
gLastTurnFrame = Global.frame
fwd = max(ballD * math.cos(ballH),0)
left = ballD * math.sin(ballH)
# if gGoalie:
# left = leftAdj
# Indicator.showHeadColor(Indicator.RGB_BLUE)
Action.walk(fwd,left,ballH,"ddd",minorWalkType=Action.SkeFastForwardMWT)
# Turn on the spot
else:
gLastTurnFrame = Global.frame
fwd = 0
left = 0
turnccw = hMath.CLIP(ballH,60)
# if gGoalie:
# fwd = -Action.MAX_FORWARD
#
# if abs(ballx - myx) > Constant.DOG_WIDTH * 2:
# left = (myx - ballx) * 0.7
# turnccw = 0
# Indicator.showHeadColor(Indicator.RGB_PURPLE)
Action.walk(fwd,left,turnccw,"ddd",minorWalkType=Action.SkeFastForwardMWT)
def performBall(dkd=90, dist=20, direction=None, accuracy=20):
global gLastCalledFrame
global gDirection
global gLastCalledFrame
bx, by = Global.ballX, Global.ballY
myx, myy = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
# If this function wasn't called in previous frame, then reset the direction.
if Global.frame == gLastCalledFrame:
gDirection = None
gLastCalledFrame = Global.frame
# Work out if we are to the left or right of the ball (relative to DKD as 0)
lRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
if abs(lRobotH) > 70:
gDirection = None
if direction == None and gDirection == None:
if lRobotH < 0: # robot to the left
#print "robot to left of ball",
gDirection = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
gDirection = Constant.dCLOCKWISE
elif direction != None:
gDirection = direction
if gDirection == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
# This factor is used to adjust the dodgyness of the sidewards and
factor = max(min(abs(180 - lRobotH) / 90.0, 1), 0)
lcx = math.cos(hMath.DEG2RAD(circleAng)) * dist * factor
lcy = math.sin(hMath.DEG2RAD(circleAng)) * dist * factor
robotH = hMath.normalizeAngle_0_360(lRobotH + dkd) # now global
cx, cy = hMath.getGlobalCoordinate(bx, by, robotH, lcx, lcy)
bh = Global.ballH
lcx, lcy = hMath.getLocalCoordinate(myx, myy, myh, cx, cy)
if abs(bh) > 30 and Global.ballD > dist:
Action.walk(0, 0, bh, minorWalkType=Action.SkeFastForwardMWT)
else:
Action.walk(lcy,
lcx,
bh * 0.75,
"ddd",
minorWalkType=Action.SkeFastForwardMWT)
if abs(hMath.normalizeAngle_180(myh + bh -
dkd)) < accuracy and abs(bh) < accuracy:
resetPerform()
return Constant.STATE_SUCCESS
gLastCalledFrame = Global.frame
return Constant.STATE_EXECUTING
def perform(dkd=90, dist=20, direction=None, bx=None, by=None, accuracy=20):
global gDirection
#if side != None:
# print "Warning: sGetBehindBall.perform: side is not yet implemented"
if bx == None or by == None:
(bx, by) = Global.gpsGlobalBall.getPos()
(myx, myy) = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
# Work out if we are to the left or right of the ball (relative to DKD as 0)
lRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
if direction == None and gDirection == None:
if lRobotH < 0: # robot to the left
#print "robot to left of ball",
gDirection = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
gDirection = Constant.dCLOCKWISE
elif direction != None:
gDirection = direction
# The circling point can be calculated as looking from the ball
# towards the robot at CIRCLE_DEGREES to the left/right, and distance
# dist. CircleAng is from the ball facing the robot with positive x
# at zero degrees
# if robotH > 180 - CIRCLE_DEGREES:
# # If we are within CIRCLE_DEGREES of the target point don't overshoot
# circleAng = 90 + (180 - robotH)
# elif robotH < -180 + CIRCLE_DEGREES:
# circleAng = 90 - (-180 + robotH)
# else:
if gDirection == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
#print ""
#print "local robot H ", lRobotH
# relative to target facing robot
# This factor is used to adjust the dodgyness of the sidewards and
# backwards ability of fast skelliptical walk.
factor = 1 #max(min(abs(180-lRobotH)/60.0,2),1)
lcx = math.cos(hMath.DEG2RAD(circleAng)) * dist * factor
lcy = math.sin(hMath.DEG2RAD(circleAng)) * dist * factor
#print "circleAng", circleAng, "rel circle pos", circleRelX, circleRelY
robotH = hMath.normalizeAngle_0_360(lRobotH + dkd) # now global
cx, cy = hMath.getGlobalCoordinate(bx, by, robotH, lcx, lcy)
#print " local circle pos : (", lcx ,",",lcy,")"
#print "my pos : (", myx, ",", myy, ",",myh,")"
#print "global robotH ", robotH
#print "global ball :(", bx, ",", by, ") global circle pos : (", cx ,",",cy,")"
# circleX/Y now is the global coords of the circle point, so walk there.
bh = hMath.getHeadingBetween(myx, myy, bx, by) # global
lbh = hMath.normalizeAngle_180(bh - myh)
lcx, lcy = hMath.getLocalCoordinate(myx, myy, myh, cx, cy)
#lcdSquared = hMath.getDistSquaredBetween(0,0,lcx,lcy)
Action.walk(lcy, lcx, lbh, "ddd", minorWalkType=Action.SkeFastForwardMWT)
if abs(hMath.normalizeAngle_180(bh -
dkd)) < accuracy and abs(lbh) < accuracy:
resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def walkToStillBall(ballD, ballH, getBehind=False):
# Calculate getBehind adjustment
leftAdj = 0
if getBehind:
myx, myy = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
ballx, bally = hMath.getGlobalCoordinate(
myx, myy, myh, *hMath.polarToCart(ballD, ballH + 90))
# In defense we get between our goal and the ball. In attack we line
# up the ball and target goal. In the (small) midfield we line up with
# upfield
reversePoints = False
if bally < Constant.FIELD_LENGTH / 2 - 20:
lineX, lineY = Constant.OWN_GOAL_X, Constant.OWN_GOAL_Y
elif bally > Constant.FIELD_LENGTH / 2 + 20:
lineX, lineY = Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y
reversePoints = True
else:
lineX, lineY = ballx, 0
# Draw a line between ball and own goal. Find self offset from the line
dist = ((ballx - lineX)*(lineY - myy)-(lineX - myx)*(bally - lineY)) /\
math.sqrt(hMath.SQUARE(ballx - lineX) \
+ hMath.SQUARE(bally - lineY))
# If dist > 0 we are to the right of the line, so adjustment left
# These adjustments will only be applied assuming we are facing
# the ball
if dist > 15:
leftAdj = 14
elif dist < -15:
leftAdj = -12
else:
leftAdj = 0
# From in front of the ball adjust even more to get more behind
if myy > bally:
leftAdj *= 1.5
# If the line was to a target to line up, reverse direction
if reversePoints:
leftAdj = -leftAdj
Indicator.showFacePattern([0] * 5)
if ballD < MID_BALL_DIST and abs(ballH) > 30:
# Turn on the spot
ballH = hMath.CLIP(ballH, 70)
Action.walk(0, 0, ballH, minorWalkType=Action.SkeFastForwardMWT)
else:
# Walk direct - with getBehind adjustment
if leftAdj > 0:
Indicator.showFacePattern([0, 0, 0, 2, 3])
elif leftAdj < 0:
Indicator.showFacePattern([3, 2, 0, 0, 0])
if abs(ballH) < 30:
Action.walk(Action.MAX_FORWARD,
leftAdj,
ballH,
minorWalkType=Action.SkeFastForwardMWT)
else:
ballH = hMath.CLIP(ballH, 70)
Action.walk(0, 0, ballH, minorWalkType=Action.SkeFastForwardMWT)
"""