def test(argv):
global selfX, selfY, selfH, ballX, ballY, ballH
id(argv)
selfX = 30
selfY = 150
selfH = 270
ballX = 10
ballY = 80
ballH = hMath.getHeadingToMe(selfX, selfY, selfH, ballX, ballY)
print str(perform())
def timeToReachPoint(x, y, h, myX=None, myY=None, myH=None,\
doTurn=True, doDKD=True, doObs=True):
if myX == None:
myX = Global.selfLoc.getX()
myY = Global.selfLoc.getY()
myH = Global.selfLoc.getHeading()
relH = hMath.getHeadingToMe(myX, myY, myH, x, y)
absH = hMath.getHeadingBetween(myX, myY, x, y)
(lX, lY) = hMath.getLocalCoordinate(myX, myY, myH, x, y)
# 1. Estimate the straight line time to get there
# 2. Add time to turn and face (or slow walk due to turning on the run)
# 3. Add time to face desired heading when we arrive (or getBehind)
# 4. Add time for obstacles if there are any in the way
# if (Global.frame % 10 == 0):
# print "dist:", hMath.getDistanceBetween(myX, myY, x, y) /EST_WALK_SPEED
# print "turn:", abs(relH) /EST_TURN_SPEED
# print "dkd:", abs(hMath.normalizeAngle_180(absH - h)) /EST_TURN_SPEED
time = hMath.getDistanceBetween(myX, myY, x, y) / EST_WALK_SPEED
if doTurn:
time += abs(relH) / EST_TURN_SPEED
if doDKD:
# Time to turn to dkd when we get there is penalised more harshly
time += (abs(hMath.normalizeAngle_180(absH - h)) /
EST_TURN_SPEED) * 1.3
if doObs:
nobs = VisionLink.getNoObstacleBetween(0, 0, int(lX), int(lY), 30, 30,
0, Constant.OBS_USE_NONE)
if nobs > 100:
nobs = 50
#if x == Global.ballX:
# print "Nobs=", nobs
if nobs >= 5:
# 10ms per obstacle point. A typical half-obstructed path might
# count 60 obs -> 600ms. Max at nobs 100 -> 1 sec
time += nobs * 10
return int(time)
def selectKick(selType):
global gKickType
global gKickHead
global gKickCounter
global gBreakCounter
#gBreakCounter = BREAK_DURATION
#gKickCounter = 0
#return
gBreakCounter = 0
# Choosing ball releasing approach by dribbleType
# Selecting kick approach for offensive.
if selType == SEL_OFFENSIVE:
if Global.vTGoal.isVisible():
cx = Global.vTGoal.getCentroid()[0]
width = Global.vTGoal.getWidth()
minH, maxH = selectGap()
padding = abs(maxH - minH) / 5.0
# Within the gap
if minH < -padding and padding < maxH:
#gKickType = Action.NUFwdKickWT
#gKickCounter = NU_FWD_DURATION
gKickType = Action.HeadTapWT
gKickCounter = HEADTAP_FWD_DURATION
#print "within gap"
# Take side step and shoot
elif minH < 0 < maxH:
if abs(minH) > abs(maxH):
gKickType = SIDE_STEP_LEFT_SHOOT
else:
gKickType = SIDE_STEP_RIGHT_SHOOT
gKickCounter = SIDE_STEP_SHOOT_DURATION
#print "side step shoot"
# Check if the goal is in the view.
# If so have a crack.
elif cx - width / 2 <= Constant.IMAGE_WIDTH * 0.4\
and Constant.IMAGE_WIDTH * 0.6 <= cx + width / 2\
and Global.selfLoc.getY() < Constant.FIELD_LENGTH * 0.6:
#gKickType = Action.NUFwdKickWT
#gKickCounter = NU_FWD_DURATION
gKickType = Action.HeadTapWT
gKickCounter = HEADTAP_FWD_DURATION
#print "have a crack"
# gap is in right hand side, do Upenn left?
elif minH < maxH < -45:
gKickType = Action.UpennLeftWT
gKickCounter = UPENN_DURATION
# gap is in left hand side, do Upenn right?
elif 45 < minH < maxH:
gKickType = Action.UpennRightWT
gKickCounter = UPENN_DURATION
else:
gBreakCounter = BREAK_DURATION
#print "break out by visual"
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
cx = gLastVisTGoal.getCentroid()[0]
width = gLastVisTGoal.getWidth()
if cx - width / 2 <= Constant.IMAGE_WIDTH * 0.4\
and Constant.IMAGE_WIDTH * 0.6 <= cx + width / 2\
and Global.selfLoc.getY() < Constant.FIELD_LENGTH * 0.6:
#gKickType = Action.NUFwdKickWT
#gKickCounter = NU_FWD_DURATION
gKickType = Action.HeadTapWT
gKickCounter = HEADTAP_FWD_DURATION
#print "have a crack last visual goal"
else:
gBreakCounter = BREAK_DURATION
#print "break out by last visual"
else:
gBreakCounter = BREAK_DURATION
#print "break out by nothing!!!"
# Choose defensive kick strategy for grab dribble.
elif selType == SEL_DEFENSIVE:
# gap heading must be between the headings of the offensive field edges.
bestGap = VisionLink.getBestGap(0, 100, GAP_MAX_DIST, GAP_MIN_DIST,
GAP_MIN, GAP_MIN_INTENSITY)
bestH = 0
if bestH != None:
bestH = bestGap[2]
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
maxH = hMath.getHeadingToMe(selfX, selfY, selfH, 0,
Constant.FIELD_LENGTH * 0.6)
minH = hMath.getHeadingToMe(selfX, selfY, selfH, Constant.FIELD_WIDTH,
Constant.FIELD_LENGTH * 0.6)
obsFront = VisionLink.getNoObstacleBetween(0, 0, 0, 100)
#print "defensive: "
#print "minH: ",minH
#print "bestH: ", bestH
#print "maxH: ",maxH
#print "obsFront: ", obsFront
if Global.vOGoal.isVisible():
gKickType = AVOID_OWN_GOAL
gKickCounter = AVOID_OWN_GOAL_DURATION
gBreakCounter = BREAK_DURATION
elif obsFront > MIN_OBS_TO_GAP\
and bestGap != None and minH < bestH < maxH:
#print "gap : selected!!!!!!!!"
gKickHead = selfH + bestH
gKickType = GAP_KICK
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.NUFwdKickWT
gKickCounter = NU_FWD_DURATION
# Choose mid-field kick strategy for grab dribble.
elif selType == SEL_MIDFIELD:
# gap heading must be between the headings of the two goal box edges.
bestGap = VisionLink.getBestGap(0, 100, GAP_MAX_DIST, GAP_MIN_DIST,
GAP_MIN, GAP_MIN_INTENSITY)
bestH = 0
if bestGap != None:
bestH = bestGap[2]
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
maxH = hMath.getHeadingToMe(selfX, selfY, selfH,
Constant.MIN_GOALBOX_EDGE_X,
Constant.FIELD_LENGTH)
minH = hMath.getHeadingToMe(selfX, selfY, selfH,
Constant.MAX_GOALBOX_EDGE_X,
Constant.FIELD_LENGTH)
obsFront = VisionLink.getNoObstacleBetween(0, 0, 0, 100)
#print "midfield: "
#print "minH: ",minH
#print "bestH: ", bestH
#print "maxH: ",maxH
#print "obsFront: ", obsFront
if Global.vOGoal.isVisible():
gKickType = AVOID_OWN_GOAL
gKickCounter = AVOID_OWN_GOAL_DURATION
gBreakCounter = BREAK_DURATION
elif obsFront > MIN_OBS_TO_GAP\
and bestGap != None and minH < bestH < maxH:
#print "gap : selected!!!!!!!!!!!"
gKickHead = selfH + bestH
gKickType = GAP_KICK
gKickCounter = SOFT_TAP_DURATION
else:
gBreakCounter = BREAK_DURATION
elif selType == SEL_GPS:
selfH = Global.selfLoc.getHeading()
selfX, selfY = Global.selfLoc.getPos()
#hMath.getDistSquaredBetween(pos[0],pos[1],targetPos[0],targetPos[1])
gKickHead = selfH #+ hMath.getHeadingToMe(selfX, selfY, selfH,targetPosition[0],targetPosition[1])
gKickType = JUST_SHOOT
gKickCounter = SOFT_TAP_DURATION
# Otherwise, just release the ball.
else:
gBreakCounter = BREAK_DURATION
# If we were doing diagonal dribble recently,
# then step forward a little before we shoot.
if Global.frame - gLastDiagFrame < DIAG_DURATION\
and gKickType == Action.NUFwdKickWT:
gKickType = FWD_STEP_SHOOT
gKickCounter = FWD_STEP_SHOOT_DURATION
# If we are kicking, then hold the ball tight.
# Make sure you check for appropriate kick type to be called by Action.kick().
# ..
if gBreakCounter == 0\
and gKickType != AVOID_OWN_GOAL\
and gKickType != FWD_STEP_SHOOT\
and gKickType != SIDE_STEP_LEFT_SHOOT\
and gKickType != SIDE_STEP_RIGHT_SHOOT\
and gKickType != JUST_SHOOT\
and gKickType != GAP_KICK:
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
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 selectKick(selType):
global gNextKickType
global gKickType
global gKickHead
global gKickCounter
global gBreakCounter
#gBreakCounter = BREAK_DURATION
#gKickCounter = 0
#return
gBreakCounter = 0
# Choosing ball releasing approach by dribbleType
# Selecting kick approach for offensive.
if selType == SEL_OFFENSIVE:
#print "select SEL_OFFENSIVE"
if Global.vTGoal.isVisible():
#print "goal visible"
cx = Global.vTGoal.getCentroid()[0]
width = Global.vTGoal.getWidth()
minH, maxH = selectGap()
padding = abs(maxH - minH) / 5.0
# Within the gap
if minH < -padding and padding < maxH:
#gKickType = Action.NUFwdKickWT
#gKickCounter = NU_FWD_DURATION
gKickType = Action.HeadTapWT
gKickCounter = HEADTAP_FWD_DURATION
print "Shooting within gap"
# Take side step and shoot
elif minH < 0 < maxH:
if abs(minH) > abs(maxH):
gKickType = SIDE_STEP_LEFT_SHOOT
else:
gKickType = SIDE_STEP_RIGHT_SHOOT
gKickCounter = SIDE_STEP_SHOOT_DURATION
#print "side step shoot"
# Check if the goal is in the view.
# If so have a crack.
elif cx - width / 2 <= Constant.IMAGE_WIDTH * 0.4\
and Constant.IMAGE_WIDTH * 0.6 <= cx + width / 2\
and Global.selfLoc.getY() < Constant.FIELD_LENGTH * 0.6:
#gKickType = Action.NUFwdKickWT
#gKickCounter = NU_FWD_DURATION
gKickType = Action.HeadTapWT
gKickCounter = HEADTAP_FWD_DURATION
print "Shooting from far (not gap)"
# gap is in right hand side, do Upenn left?
elif not Global.penaltyShot and minH < maxH < -45:
gKickType = Action.UpennLeftWT
gKickCounter = UPENN_DURATION
print "Shooting with UPenn"
# gap is in left hand side, do Upenn right?
elif not Global.penaltyShot and 45 < minH < maxH:
gKickType = Action.UpennRightWT
gKickCounter = UPENN_DURATION
print "Shooting with UPenn"
# Penalty shot tries sidestep instead of fail
#elif Global.penaltyShot:
# if abs(minH) > abs(maxH):
# gKickType = SIDE_STEP_LEFT_SHOOT
# else:
# gKickType = SIDE_STEP_RIGHT_SHOOT
# gKickCounter = SIDE_STEP_SHOOT_DURATION
else:
gBreakCounter = BREAK_DURATION
print "vGoal but not shooting - no kick suitable"
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
#print "goal recently visible"
cx = gLastVisTGoal.getCentroid()[0]
width = gLastVisTGoal.getWidth()
if cx - width / 2 <= Constant.IMAGE_WIDTH * 0.4\
and Constant.IMAGE_WIDTH * 0.6 <= cx + width / 2\
and Global.selfLoc.getY() < Constant.FIELD_LENGTH * 0.6:
#gKickType = Action.NUFwdKickWT
#gKickCounter = NU_FWD_DURATION
gKickType = Action.HeadTapWT
gKickCounter = HEADTAP_FWD_DURATION
print "Shooting from far at last visual goal"
# Penalty shot tries sidestep instead of fail
#elif Global.penaltyShot:
# if abs(minH) > abs(maxH):
# gKickType = SIDE_STEP_LEFT_SHOOT
# else:
# gKickType = SIDE_STEP_RIGHT_SHOOT
# gKickCounter = SIDE_STEP_SHOOT_DURATION
else:
gBreakCounter = BREAK_DURATION
print "no vGoal and not shooting - no kick suitable"
# If the penalty player gets here we really want to try and line up
# the goal
#elif Global.penaltyShot:
# #turnCCW = turnToGPSGoal()
# #performDribble(Action.MAX_FORWARD,left,turnCCW)
# h = hMath.getHeadingToFaceAt(Global.selfLoc,\
# Constant.TARGET_GOAL_X,\
# Constant.TARGET_GOAL_Y)
# if h > 0:
# gKickType = SIDE_STEP_LEFT_SHOOT
# else:
# gKickType = SIDE_STEP_RIGHT_SHOOT
# gKickCounter = SIDE_STEP_SHOOT_DURATION
else:
print "selKick OFFENSIVE: goal not seen - break!"
gBreakCounter = BREAK_DURATION
#print "break out by nothing!!!"
# Choose defensive kick strategy for grab dribble.
elif selType == SEL_DEFENSIVE:
#print "select SEL_DEFENSIVE"
# gap heading must be between the headings of the offensive field edges.
bestGap = VisionLink.getBestGap(0, 100, GAP_MAX_DIST, GAP_MIN_DIST,
GAP_MIN, GAP_MIN_INTENSITY)
bestH = 0
if bestH != None:
bestH = bestGap[2]
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
maxH = hMath.getHeadingToMe(selfX, selfY, selfH, 0,
Constant.FIELD_LENGTH * 0.6)
minH = hMath.getHeadingToMe(selfX, selfY, selfH, Constant.FIELD_WIDTH,
Constant.FIELD_LENGTH * 0.6)
# Using same obstacle function as dodging.
#obsFront = VisionLink.getNoObstacleBetween(0,0,0,100)
obsFront = VisionLink.getNoObstacleInBox(-25, 100, 25, 20,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
#print "defensive: "
#print "minH: ",minH
#print "bestH: ", bestH
#print "maxH: ",maxH
#print "obsFront: ", obsFront
if (not Global.lightingChallenge) and Global.vOGoal.isVisible():
gKickType = AVOID_OWN_GOAL
gKickCounter = AVOID_OWN_GOAL_DURATION
gBreakCounter = BREAK_DURATION
elif obsFront > MIN_OBS_TO_GAP\
and bestGap != None and minH < bestH < maxH:
#print "gap : selected!!!!!!!!"
gKickHead = selfH + bestH
gKickType = GAP_KICK
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.NUFwdKickWT
gKickCounter = NU_FWD_DURATION
# Choose mid-field kick strategy for grab dribble.
elif selType == SEL_MIDFIELD:
#print "select SEL_MIDFIELD"
# gap heading must be between the headings of the two goal box edges.
bestGap = VisionLink.getBestGap(0, 100, GAP_MAX_DIST, GAP_MIN_DIST,
GAP_MIN, GAP_MIN_INTENSITY)
bestH = 0
if bestGap != None:
bestH = bestGap[2]
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
maxH = hMath.getHeadingToMe(selfX, selfY, selfH,
Constant.MIN_GOALBOX_EDGE_X,
Constant.FIELD_LENGTH)
minH = hMath.getHeadingToMe(selfX, selfY, selfH,
Constant.MAX_GOALBOX_EDGE_X,
Constant.FIELD_LENGTH)
obsFront = VisionLink.getNoObstacleBetween(0, 0, 0, 100)
#print "midfield: "
#print "minH: ",minH
#print "bestH: ", bestH
#print "maxH: ",maxH
#print "obsFront: ", obsFront
if (not Global.lightingChallenge) and Global.vOGoal.isVisible():
gKickType = AVOID_OWN_GOAL
gKickCounter = AVOID_OWN_GOAL_DURATION
gBreakCounter = BREAK_DURATION
elif obsFront > MIN_OBS_TO_GAP\
and bestGap != None and minH < bestH < maxH:
#print "gap : selected!!!!!!!!!!!"
gKickHead = selfH + bestH
gKickType = GAP_KICK
gKickCounter = SOFT_TAP_DURATION
else:
gBreakCounter = BREAK_DURATION
# Kick strategy for releasing and regrabbing.
elif selType == SEL_RELEASE:
#print "select SEL_RELEASE"
gKickType = RELEASE
gKickCounter = RELEASE_DURATION
# Otherwise, just release the ball.
else:
#print "no selType, breaking"
gBreakCounter = BREAK_DURATION
# If we were doing diagonal dribble recently,
# then step forward a little before we shoot.
if Global.frame - gLastDiagFrame < DIAG_DURATION\
and (gKickType == Action.NUFwdKickWT or gKickType == Action.HeadTapWT):
gNextKickType = gKickType
gKickType = FWD_STEP_SHOOT
gKickCounter = FWD_STEP_SHOOT_DURATION
# If doing sidestep shoot then set next kick to shoot
if gKickType == SIDE_STEP_LEFT_SHOOT or gKickType == SIDE_STEP_RIGHT_SHOOT:
gNextKickType = Action.HeadTapWT
# If we are kicking, then hold the ball tight.
# Make sure you check for appropriate kick type to be called by
# Action.kick()...
if gBreakCounter == 0\
and gKickType != AVOID_OWN_GOAL\
and gKickType != FWD_STEP_SHOOT\
and gKickType != SIDE_STEP_LEFT_SHOOT\
and gKickType != SIDE_STEP_RIGHT_SHOOT\
and gKickType != GAP_KICK\
and gKickType != RELEASE:
#print "selectKick kicking (", gKickType, ")"
perform(0, 0, 0)
Action.kick(gKickType)
moveHeadForwardTight()
def DecideNextAction():
global indCycle, grabObject, counter, shouldGrab, moveTo
global haveObstacle, isCorrectstandingPosition, distanceOK, framesLastSentYUV
global localiseTime, lastHeading, lostObstacle
global allBox, leftBox, rightBox, leftLongBox, rightLongBox
Indicator.superDebug()
point4 = [SIDE_MAX_DETECTION, 50]
point_leftbox_topleftlong = [-SIDE_MAX_DETECTION, 150]
shouldHeading = None
calculateObstacleBox()
if False:
print
print "isCorrectstandingPosition:", isCorrectstandingPosition
print "shouldGrab:", shouldGrab
print "haveObstacle:", haveObstacle
print "lostObstacle:", lostObstacle
print "Global.BallD:", Global.ballD
isCorrectstandingPosition = True
if (Global.vBall.isVisible() and Global.ballD > 30):
isCorrectstandingPosition = False
if shouldGrab or not isCorrectstandingPosition:
print "shouldGrab or find ball and stop"
if grabObject == None:
grabObject = GrabGenerator()
try:
grabObject.next()
except StopIteration:
reset()
else:
print "stand still"
Action.standStill()
result =\
VisionLink.getPointToNearestObstacleInBox(\
point_leftbox_topleftlong[0],\
point_leftbox_topleftlong[1],\
point4[0],point4[1],
5\
)
# if (leftlong > MIN_POINTS_CONSIDERED_AS_OBSTACLE and leftlong > rightlong):
# haveObstacle = haveObstacle + 1
# print "haveObstacle", haveObstacle
# print "checking left"
# if result == None:
# print "error it should be checking left but return None"
# shouldHeading = None
# localise()
# else:
# pointX, pointY = result
# shouldHeading = hMath.getHeadingToMe(0,0,0,pointX, pointY)
# elif (rightlong > MIN_POINTS_CONSIDERED_AS_OBSTACLE and rightlong > leftlong):
# haveObstacle = haveObstacle + 1
# print "haveObstacle", haveObstacle
# print "checking right"
# if result == None:
# print "error it should be checking right but return None"
# shouldHeading = None
# localise()
# else:
# pointX, pointY = result
# shouldHeading = hMath.getHeadingToMe(0,0,0,pointX, pointY)
# else :
# localise()
# if haveObstacle > 0:
# print "haveObstacle", haveObstacle
# haveObstacle = 0
if result == None:
if (lostObstacle > 3 and lastHeading != None):
shouldHeading = lastHeading
print "just lost"
else:
localise()
print "no nearest obstacle"
haveObstacle = 0
lostObstacle = lostObstacle + 1
lastHeading = None
else:
print "result is not none", result
pointX, pointY = result
shouldHeading = hMath.getHeadingToMe(0, 0, 0, pointX, pointY)
print "pointing", -pointX, ":", pointY
print "heading:", shouldHeading
lastHeading = shouldHeading
if (shouldHeading != None):
#localise()
calculate = int((shouldHeading - 90.0) / 90 * 80)
if (framesLastSentYUV > 30):
VisionLink.sendYUVPlane()
framesLastSentYUV = 0
hTrack.stationaryLocalise(2, calculate + 3, calculate - 3)
#Action.setHeadParams(-pointX, 0, pointY, Action.HTAbs_xyz)
print "calculate:", calculate
if result != None and (leftBox > MIN_POINTS_CONSIDERED_AS_OBSTACLE
and leftBox > rightBox):
print "move left:", haveObstacle
haveObstacle = haveObstacle + 1
lostObstacle = 0
if (framesLastSentYUV > 30):
VisionLink.sendYUVPlane()
framesLastSentYUV = 0
if (haveObstacle > 3):
shouldGrab = True
moveTo = LEFT
elif result != None and (rightBox > MIN_POINTS_CONSIDERED_AS_OBSTACLE
and rightBox > leftBox):
print "move right:", haveObstacle
haveObstacle = haveObstacle + 1
lostObstacle = 0
if (framesLastSentYUV > 30):
VisionLink.sendYUVPlane()
framesLastSentYUV = 0
if (haveObstacle > 3):
shouldGrab = True
moveTo = RIGHT
if False: #all > 0 :# and counter == 60:
print
print "all:", allBox, "endall"
print "left:", leftBox, "endleft"
print "right:", rightBox, "endright"
print "leftlong:", leftLongBox, "endleft"
print "rightlong:", rightLongBox, "endright"
counter = 0
counter = counter + 1
framesLastSentYUV = framesLastSentYUV + 1
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 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