def getOutOfTheCircle(centerX, centerY, targetH, doForce=False, radius=40): selfX, selfY = Global.selfLoc.getPos() dist = hMath.getDistanceBetween(centerX, centerY, selfX, selfY) heading = hMath.getHeadingBetween(centerX, centerY, selfX, selfY) if amIInTheCircle(centerX,centerY,radius)\ or doForce: if dist > radius: relX = dist * 2 * math.cos(hMath.DEG2RAD(heading)) relY = dist * 2 * math.sin(hMath.DEG2RAD(heading)) else: relX = radius * 2 * math.cos(hMath.DEG2RAD(heading)) relY = radius * 2 * math.sin(hMath.DEG2RAD(heading)) destX = centerX + relX destY = centerY + relY hTrack.saGoToTargetFacingHeading(destX, destY, targetH) Indicator.showFacePattern([0, 2, 0, 2, 0]) return Constant.STATE_EXECUTING return Constant.STATE_SUCCESS
def trackVisualBallByProj(adjX=0, adjY=0, adjZ=0, isTurning=False, adjPos=1.0): ballPos = VisionLink.getProjectedBall() # If projection returns infinity, then use gps. if abs(ballPos[0]) >= Constant.LARGE_VAL\ or abs(ballPos[1]) >= Constant.LARGE_VAL: # We should divide the velocity adjustment by half. trackVisualBallByTrig(adjX, adjY, adjZ, adjPos=adjPos) return ballPosX = ballPos[0] * adjPos ballPosY = ballPos[1] * adjPos #print " track by proj ", # Note that xyz in getPointProjection is different from xyz to headMotion if isTurning and abs(Global.ballH) > 80: d = hMath.getDistanceBetween(0, 0, ballPosX, ballPosY) x = d * math.cos(hMath.DEG2RAD(10)) y = d * math.sin(hMath.DEG2RAD(10)) if Global.ballH < 0: x = -x Action.setHeadParams(x + adjX, adjY, y + adjZ, Action.HTAbs_xyz) else: Action.setHeadParams(ballPosX + adjX, adjY, ballPosY + adjZ, Action.HTAbs_xyz)
def saGoToTargetFacingHeading(targetX, targetY, targetH, \ maxSpeed = Action.MAX_FORWARD, maxTurn = Action.MAX_TURN): # Variables relative to self localisation. selfPos = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() cosSelf = math.cos(math.radians(selfH)) sinSelf = math.sin(math.radians(selfH)) relX = targetX - selfPos[0] relY = targetY - selfPos[1] relH = hMath.normalizeAngle_180(targetH - selfH) # now take into account that as we turn, the position of our head will change relX += Constant.DOG_LENGTH / 2.0 * (cosSelf - math.cos(math.radians(targetH))) relY += Constant.DOG_LENGTH / 2.0 * (sinSelf - math.sin(math.radians(targetH))) relD = hMath.getLength((relX, relY)) # Calculate approximate coordinates of all the dog's legs legs = [] for legNum in range(4): legPos = list(selfPos) if legNum >= 2: legPos[0] = legPos[0] - DOG_RECT_LENGTH * cosSelf legPos[1] = legPos[1] - DOG_RECT_LENGTH * sinSelf if (legNum % 2) == 0: legPos[0] = legPos[0] - DOG_RECT_WIDTH * sinSelf legPos[1] = legPos[1] + DOG_RECT_WIDTH * cosSelf else: legPos[0] = legPos[0] + DOG_RECT_WIDTH * sinSelf legPos[1] = legPos[1] - DOG_RECT_WIDTH * cosSelf legs.append(tuple(legPos)) # If a leg coordinate lies within the rear wall, clip the Y movement clipMove = False for legPos in legs: if abs(legPos[0] - Constant.FIELD_WIDTH / 2) > Constant.GOAL_WIDTH and\ legPos[1] < 2: clipMove = True if clipMove and (relY < 0): relY = 0 if relX == 0 and relY == 0: #on the dog, math.atan2(0,0) give "Value Error: math domain error" relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos(hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnCCW = hMath.CLIP(relH, maxTurn) Action.walk(forward, left, turnCCW, minorWalkType=Action.SkeFastForwardMWT)
def isHeadRightOk(): if not Global.vBall.isVisible(): return False ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD return -2.0 < ballx < 3.2\ and bally < 3.4*Constant.BallRadius\ and Global.ballD < 12\ and abs(Global.pan) < 30
def isGoalieUPleftOk(): if hTrack.canSeeBall(): ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD if (ballx < 3.0 and ballx > -5.0 and bally < 4.4 * Constant.BallRadius and Global.ballD < 18 and Global.tilt < -9 and abs(Global.pan) < 30): return True return False
def isUpennRightOk(): if not Global.vBall.isVisible(): return False ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD maxBallRadiiAway = 4.5 return -5.0 < ballx < 3.0\ and bally < maxBallRadiiAway * Constant.BallRadius\ and abs(Global.ballH) < 30
def findByWeightedVisBall(): d = Global.weightedVisBallDist h = Global.weightedVisBallHead if abs(h) < 90: x = d * math.sin(hMath.DEG2RAD(h)) y = d * math.cos(hMath.DEG2RAD(h)) Action.setHeadParams(x,Constant.BallDiameter,y,Action.HTAbs_xyz) else: Action.setHeadToLastPoint() if not gHeadOnly: # Make sure we use gps's information, not wireless # FIXME: if we are going to dodgy dog then don't get behind walkToBall(d,h,getBehind = gGetBehind)
def perform(paw=AUTO_PAW_KICK): global pawKickType if Global.lostBall > Constant.LOST_BALL_GPS: return Constant.STATE_FAILED # Head ball tracking movement sFindBall.perform(True) PAW_KICK_Y_OFFSET = Constant.BallRadius PAW_KICK_X_OFFSET = 7.0 ballX = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD ballY = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD # The 2003 offset is 6 and it is not too bad. if paw == AUTO_PAW_KICK: if pawKickType == LEFT_PAW_KICK and ballX < -PAW_KICK_X_OFFSET / 2: pawKickType = RIGHT_PAW_KICK elif pawKickType == RIGHT_PAW_KICK and ballX > PAW_KICK_X_OFFSET / 2: pawKickType = LEFT_PAW_KICK else: pawKickType = paw if pawKickType == RIGHT_PAW_KICK: targetX = ballX + PAW_KICK_X_OFFSET else: targetX = ballX - PAW_KICK_X_OFFSET targetY = ballY - PAW_KICK_Y_OFFSET targetD = math.sqrt(hMath.SQUARE(targetX) + hMath.SQUARE(targetY)) targetH = hMath.RAD2DEG(math.asin(targetX / targetD)) # Once the ball is lined up and well within range for the forward, the # robot should commit to charge at the ball at full speed. if abs(targetX) <= 1.0 and targetD < Constant.BallRadius * 1.5: turnccw = hMath.CLIP(targetH, 10) Action.walk(Action.MAX_FORWARD, 0, turnccw, minorWalkType=Action.SkeFastForwardMWT) else: sFindBall.walkToBall(targetD, targetH, getBehind=sFindBall.GET_BEHIND_LOTS) return Constant.STATE_EXECUTING
def clipFireballPanHack(): global gLastClipFireballFrame if Global.vBall.isVisible(): ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD ##~ print "local x %.2f, y%.2f ballH %.2f" % (ballx, bally,Global.ballH) if abs(ballx) <= 2.5 and 0 <= bally <= 5.0 * Constant.BallRadius: Action.finalValues[Action.Panx] = 0 gLastClipFireballFrame = Global.frame elif False and gLastClipFireballFrame >= Global.frame - 1 \ and abs(ballx) <= 3.2 and 0 <= bally <= 5.0 * Constant.BallRadius: Action.finalValues[Action.Panx] = 0 gLastClipFireballFrame = Global.frame
def areWeAboveTheLine(offset, angle, includeYou, tx, ty): global targetX, targetY # Draw the line. ang = 90 - angle / 2.0 m = math.tan(hMath.DEG2RAD(ang)) bpos = (ty + offset) - m * tx bneg = (ty + offset) + m * tx selfX, selfY = Global.selfLoc.getX(), Global.selfLoc.getY() # If you are far back then you are ok (hysterisis). if (includeYou and selfY < Constant.GOALBOX_DEPTH)\ or selfY < 40: if Debug.defenderTriggerDebug: print "No problem because enough back!" return False # If I am behind the threshold line, no need to trigger. if includeYou and (selfY < m * selfX + bpos) and (selfY < -m * selfX + bneg): if Debug.defenderTriggerDebug: print "False because of the line!" return False return True
def saGoToTargetFacingHeading(targetX, targetY, targetH, \ maxSpeed = Action.MAX_FORWARD, maxTurn = Action.MAX_TURN): # ariables relative to self localisation. selfX = Global.selfLoc.getX() selfY = Global.selfLoc.getY() selfH = Global.selfLoc.getHeading() relX = targetX - selfX relY = targetY - selfY relH = hMath.normalizeAngle_180(targetH - selfH) relX += Constant.DOG_LENGTH/2.0/10.0*(math.cos(math.radians(selfH)) \ - math.cos(math.radians(targetH))) relY += Constant.DOG_LENGTH/2.0/10.0*(math.sin(math.radians(selfH)) \ - math.sin(math.radians(targetH))) relD = hMath.getLength((relX, relY)) distanceSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) inCircle = distanceSquared <= hMath.SQUARE(40) faceH = hMath.getHeadingToFaceAt(Global.selfLoc, targetX, targetY) ##~ print "faceH: ", faceH if not inCircle: if abs(faceH) >= 30: Action.walk(0, 0, faceH) else: #if sStealthDog.stealthDog(True): # hFWHead.compulsoryAction = hFWHead.mustSeeBall # hTrack.trackVisualBall() # return Action.walk(maxSpeed, 0, hMath.CLIP(faceH / 1.5, maxTurn)) else: if relX == 0 and relY == 0: # On the dog, math.atan2(0,0) give "Value Error: math domain error". relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnCCW = hMath.CLIP(relH, maxTurn) Action.walk(forward, left, turnCCW)
def trackVisualBallByTrig(adjX=0, adjY=0, adjZ=0, isTurning=False, adjPos=1.0): #print " track by trig ", ballD, ballH = Global.ballD, Global.ballH ballX = math.sin(hMath.DEG2RAD(ballH)) * ballD ballY = math.cos(hMath.DEG2RAD(ballH)) * ballD ballX = ballX * adjPos ballY = ballY * adjPos if isTurning and abs(Global.ballH) > 80: d = Global.ballD x = d * math.cos(hMath.DEG2RAD(10)) y = d * math.sin(hMath.DEG2RAD(10)) if Global.ballH < 0: x = -x Action.setHeadParams(x + adjX, Constant.BallDiameter + adjY, y + adjZ, Action.HTAbs_xyz) else: Action.setHeadParams(ballX + adjX, Constant.BallDiameter + adjY, ballY + adjZ, Action.HTAbs_xyz)
def saGoToTarget(targetX, targetY, maxSpeed=None, maxTurn=None): if maxSpeed == None: maxSpeed = Action.MAX_FORWARD if maxTurn == None: maxTurn = Action.MAX_TURN selfPos = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() relX = targetX - selfPos[0] relY = targetY - selfPos[1] relD = hMath.getLength((relX, relY)) if relX == 0 and relY == 0: relH = 0 else: relH = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = 0 left = 0 turnccw = 0 if relD < CLOSE_WALK_RANGE and abs(relH) > 50: forward = relY left = relX turnccw = relH * 0.8 # print "close walk and turn",relY,left,turnccw else: if abs(relH) > 80: # stop and turn turnccw = relH * 0.8 elif abs(relH) > 25: # walk and turn turnccw = relH * 0.8 forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relH)) # print "walk and turn",forward, 0, turnccw else: # ellipticalwalk and turn turnccw = hMath.CLIP(relH * 0.8, 25) forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relH)) # finally, we walk Action.walk(forward, left, turnccw, minorWalkType=Action.SkeFastForwardMWT)
def saGoToTarget(targetX, targetY, maxSpeed = Action.MAX_FORWARD, \ maxTurn = Action.MAX_TURN): selfX, selfY = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() relX = targetX - selfX relY = targetY - selfY relD = hMath.getLength((relX, relY)) distanceSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) inCircle = distanceSquared <= hMath.SQUARE(40) faceH = hMath.getHeadingToFaceAt(Global.selfLoc, targetX, targetY) if not inCircle and abs(faceH) >= 30: Action.walk(0, 0, hMath.CLIP(faceH, maxTurn)) elif not inCircle: #if sStealthDog.stealthDog(True): # hFWHead.compulsoryAction = hFWHead.mustSeeBall # hTrack.trackVisualBall() # return Action.walk(maxSpeed, 0, hMath.CLIP(faceH / 1.5, maxTurn)) else: if relX == 0 and relY == 0: relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnCCW = hMath.CLIP(relTheta, maxTurn) Action.walk(forward, left, turnCCW)
def saGoToTargetFacingHeading(targetX, targetY, targetH, \ maxSpeed = None, maxTurn = None): if maxSpeed == None: maxSpeed = Action.MAX_FORWARD if maxTurn == None: maxTurn = Action.MAX_TURN # Variables relative to self localisation. selfPos = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() relX = targetX - selfPos[0] relY = targetY - selfPos[1] relH = hMath.normalizeAngle_180(targetH - selfH) # take into account that as we turn, the position of our head will change relX += Constant.DOG_NECK_TO_BODY_CENTER_OFFSET * \ (math.cos(math.radians(selfH)) - math.cos(math.radians(targetH))) relY += Constant.DOG_NECK_TO_BODY_CENTER_OFFSET * \ (math.sin(math.radians(selfH)) - math.sin(math.radians(targetH))) relD = hMath.getLength((relX, relY)) if relX == 0 and relY == 0: relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos(hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnccw = hMath.CLIP(relH, maxTurn) Action.walk(forward, left, turnccw, "ssd", minorWalkType=Action.SkeFastForwardMWT)
def walk(target, selfPos, selfH): relX = target[0] - selfPos[0] relY = target[1] - selfPos[1] relD = hMath.getLength((relX, relY)) if relX == 0 and relY == 0: relThetaRad = 0 else: relThetaRad = hMath.DEG2RAD(hMath.normalizeAngle_180(hMath.RAD2DEG(math.atan2(relY, relX)) - selfH)) sinTheta = math.sin(relThetaRad) cosTheta = math.cos(relThetaRad) leftSign = relD * sinTheta leftAbs1 = abs(leftSign) #leftAbs1: the distance left component if cosTheta != 0: #leftAbs2: the max capping due to max forward leftAbs2 = abs(maxFwd * (sinTheta / cosTheta)) else: leftAbs2 = 100000 # huge number #print "left1:", leftAbs1, "left2:", leftAbs2 leftAbs = min(leftAbs1, leftAbs2, maxLeft) left = hMath.getSign(leftAbs, leftSign) if sinTheta != 0: forward = left * (cosTheta / sinTheta) else: fwdSign = relD fwdAbs = min(maxFwd, abs(relD)) forward = hMath.getSign(fwdAbs, fwdSign) if turningLeft: turnCCW = maxTurn else: turnCCW = -maxTurn print __name__, forward, left, turnCCW Action.walk(forward, left, turnCCW,walkType=Action.SkellipticalWalkWT)
def canDoGrab(safe): global firedType, fired1, fired2 global gLastGrabBallH global gLastGrabWeightedBallH id(safe) # Ball must be visible if Global.vBall.isVisible(): ballD = Global.vBall.getDistance() ballH = Global.vBall.getHeading() else: return False can = True # you are close to the ball can = can and 0 <= ballD <= GRAB_DIST # you are not turning right now! can = can and Global.frame - gLastTurnFrame > 6 # strictly speaking it's 11, because 23 * 8 * 2 / 1000 * 30 # Check if the ball is really close (i.e. under the robot's chin. # Copied from hTrack.clipFireballPan() ballX = math.sin(hMath.DEG2RAD(ballH)) * ballD ballY = math.cos(hMath.DEG2RAD(ballH)) * ballD ballX1 = math.sin(hMath.DEG2RAD( Global.weightedVisBallHead)) * Global.weightedVisBallDist #ballY1 = math.cos(hMath.DEG2RAD(Global.weightedVisBallHead)) * Global.weightedVisBallDist #can = can and abs(ballX) < 2.5 can = can and ballY < 15 #can = can and abs(ballX1) < 2.5 #can = can and ballY1 < 15 if abs(ballX) < 0.5 and ballY < 8: gLastGrabBallH = 0 gLastGrabWeightedBallH = 0 if can: firedType = 1 fired1 += 1 else: can = can and abs(Global.weightedVisBallHead) < 8 can = can and abs(Global.ballH) < 8 can = can and abs(Global.weightedVisBallHead - Global.ballH) < 6 #can = can and abs(Global.fstVisBallHead - Global.thdVisBallHead) < 6 gLastGrabBallH = Global.ballH gLastGrabWeightedBallH = Global.weightedVisBallHead if can: firedType = 2 fired2 += 1 # Force step complete, if all our grabbing criteria satisfies. # But, does this work? if can: Action.forceStepComplete() if False and can: vel = VisionLink.getGPSBallVInfo(Constant.CTLocal) velx, vely = vel[0], vel[1] print "" print "Camera Frame : ", Global.cameraFrame print "ballX : ", ballX, " ballY : ", math.cos( hMath.DEG2RAD(ballH)) * ballD print "gps ball d : ", Global.gpsLocalBall.getDistance() print "fst vis ball d : ", Global.fstVisBallDist, ", h : ", Global.fstVisBallHead print "snd vis ball d : ", Global.sndVisBallDist, ", h : ", Global.sndVisBallHead print "thd vis ball d : ", Global.thdVisBallDist, ", h : ", Global.thdVisBallHead print "Global.weightedVisBallHead : ", Global.weightedVisBallHead print "Global.ballH : ", Global.ballH print "vel : ", velx, ",", vely print "Since turned : ", Global.frame - gLastTurnFrame print "lostball : ", Global.lostBall print "Grab decision true : ", can print "fired Type : ", firedType print "" return can
def approachBall(sGrabDoGetBehind=None): global gLastApproachFrame global gLastReallyCloseFrame global gLastTurnFrame resetPerform() if sGrabDoGetBehind == None: sGrabDoGetBehind = sFindBall.GET_BEHIND_DEFAULT # Setting default action values with sFindBall. # Action values are overwritten by the approach ball code below. sFindBall.perform(doGetBehind=sGrabDoGetBehind) if Global.ballD < TIME_CRITICAL_DIST and Global.lostBall <= 3: VisionLink.setTimeCritical() else: VisionLink.clearTimeCritical() if Global.lostBall < Constant.LOST_BALL_GPS and Global.ballD < CLOSE_DIST: gLastApproachFrame = Global.frame if Global.frame - gLastApproachFrame < 5: VisionLink.setGrabbing() # Try to step just the right amount, but limit the minimum step # length. Reduce max to shorten step time and improve reaction time #vel = VisionLink.getGPSBallVInfo(Constant.CTLocal) #velX, velY = vel[0], vel[1] rate = Action.SKE_FF_PG * 2.0 * 8.0 / 1000.0 fwd = Global.ballD - BALL_AGAINST_CHEST_DIST fwd = hMath.EXTEND(fwd, Action.MAX_SKE_FF_FWD_STP * 0.7) # don't walk backward if fwd < 0: fwd = 0 turnCCW = Global.ballH left = Global.ballD * math.sin(hMath.DEG2RAD(Global.ballH)) #if left > 0: # left = hMath.CLIP(left,Action.MAX_SKE_FF_LEFT_STP * 0.8) #else: # left = hMath.CLIP(left,Action.MAX_SKE_FF_RIGHT_STP * 0.8) if Global.ballD < REALLY_CLOSE_DIST: gLastReallyCloseFrame = Global.frame mwt = Action.SkeNeckTurnWalkMWT if Global.frame - gLastReallyCloseFrame < 5: turnCCW = hMath.CLIP(turnCCW, 30) if abs(left) < 2: #print "branch 1" fwd = fwd / rate left = left / rate #turnCCW = hMath.CLIP(turnCCW * 0.4,30) / rate Action.walk(fwd, left, turnCCW, "sss", minorWalkType=mwt) elif abs(left) < 8: #print "branch 2" left = left * 1.2 / rate #turnCCW = hMath.CLIP(turnCCW * 0.4,30) / rate Action.walk(0, left, 0, "sss", minorWalkType=mwt) else: #print "branch 3" #turnCCW = hMath.CLIP(turnCCW * 0.5,30) / rate turnCCW = turnCCW * 0.7 Action.walk(0, 0, turnCCW, minorWalkType=mwt) gLastTurnFrame = Global.frame else: if abs(turnCCW) < 30: #print "branch 5" #left = left * 0.6 turnCCW = turnCCW * 1.1 Action.walk(fwd, 0, turnCCW, "ddd", minorWalkType=mwt) else: if Global.ballD < 30: #print "branch 6" turnCCW = turnCCW * 0.8 Action.walk(0, 0, turnCCW, minorWalkType=mwt) else: Action.walk(fwd, 0, turnCCW, minorWalkType=mwt) gLastTurnFrame = Global.frame # Use the hacked version of HTAbs_xyz if Action.finalValues[Action.HeadType] == Action.HTAbs_xyz: Action.finalValues[Action.HeadType] = Action.HTAbs_xyz_hack # Force high gain!! Global.forceHighGain = True # setting hTrack.panLow variable to true. hTrack.panLow = True
def grabBall(): global grabbingCount global isGrabbed global gLastGrabSelfLoc global gLastGrabTime debug("Grabbing!") # Hack lost ball here. Global.lostBall = 0 # Force high gain!! Global.forceHighGain = True turnCCW = gLastGrabBallH * 1.2 Action.walk(Action.MAX_FORWARD, 0, turnCCW, minorWalkType=Action.SkeNeckTurnWalkMWT) grabbingCount += 1 if grabbingCount < GRAB_FORWARD_TIME: # If we can see the ball still and is lined up, delay the grabbingCount. # Experimenting! if Global.vBall.isVisible(): ballX = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD ballY = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD if abs(ballX) < 0.5 and ballY < 8: grabbingCount = hMath.EXTEND(grabbingCount - 1, 1) Action.setHeadParams(0, -25, 30, Action.HTAbs_h) Action.closeMouth() debug("Going for the ball") elif grabbingCount < GRAB_NO_CHECK_TIME: moveHeadForward() Action.closeMouth() elif grabbingCount < GRAB_COMPLETE_TIME: moveHeadForwardTight() #Action.closeMouth() Action.openMouth() if Global.vBall.isVisible(): #print "wait i can see the ball still.." VisionLink.clearTimeCritical() resetPerform() sFindBall.setHint(25, 90) return Constant.STATE_FAILED else: moveHeadForward() Action.openMouth() isGrabbed = True grabbingCount = 0 gLastGrabSelfLoc = Global.selfLoc.getCopy() gLastGrabTime = VisionLink.getCurrentTime() frameReset() VisionLink.clearTimeCritical() 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 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, 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 getDKDRange(): nearEdgeDist = 40 debugDKD = False length = Constant.FIELD_LENGTH width = Constant.FIELD_WIDTH # Make sure determine ballSource is called if Global.ballSource is None: print "Warning: in getDKDRange() havent determined the ball source before getDKD()" return None X_ = Global.ballX Y_ = Global.ballY if debugDKD: print "Ball Source: ", Global.ballSource, " X_ ", X_, " Y_ ", Y_ # If you don't know where the ball is determine dkd based on where you are. if Global.ballSource == Constant.GPS_BALL\ and VisionLink.getGPSBallMaxVar() >= hMath.get95var(35): X_, Y_ = Global.selfLoc.getPos() if debugDKD: print "Warning: in getDKDRange(): I dont trust GPS ball right now: use my own pos is X_ ", X_, " Y_ ", Y_ Y_ = hMath.CLIPTO(Y_, 0, length) X_ = hMath.CLIPTO(X_, 0, width) # If the ball is near one of the side edges and not in corners then the direction becomes up the field. if Y_ > nearEdgeDist: if X_ < nearEdgeDist: # on the left edge if debugDKD: print "On the left edge" return (90, 70, 100, 10, 110) elif X_ > (width - nearEdgeDist): #on the right edge if debugDKD: print "On the right edge" return (90, 80, 110, 70, 170) # upper 60%, can I use voak? if Y_ > 0.6 * length: # If target goal seen and is close and not many obstacles, it has a good heading, get the best heading like VOAK if (Global.vTGoal.getConfidence() > 3 and Global.ballSource == Constant.VISION_BALL): toY = length - Y_ toX = width / 2.0 - X_ xRange = Constant.GOAL_WIDTH / 2.0 + (toY * math.tan(hMath.DEG2RAD(30))) inVOAKcal = abs(toX) < xRange if inVOAKcal: (lmin, lmax, rmin, rmax) = VisionLink.getHeadingToBestGap() lmin = hMath.normalizeAngle_0_360(lmin + Global.selfLoc.getHeading()) lmax = hMath.normalizeAngle_0_360(lmax + Global.selfLoc.getHeading()) rmin = hMath.normalizeAngle_0_360(rmin + Global.selfLoc.getHeading()) rmax = hMath.normalizeAngle_0_360(rmax + Global.selfLoc.getHeading()) if abs(lmax - lmin) > abs(rmax - rmin): tdkdmin, tdkdmax = lmin, lmax dkd = (lmin + lmax) / 2.0 else: tdkdmin, tdkdmax = rmin, rmax dkd = (rmin + rmax) / 2.0 if (dkd < 150 and dkd > 30): return (dkd, tdkdmin, tdkdmax, 10, 170) # Find the dkd fron the center of the target and own goals dkdup = hMath.getHeadingBetween(X_, Y_, (width / 2.0), length) dkdbottom = hMath.getHeadingBetween((width / 2.0), 0, X_, Y_) # if i cant use voak, then the heading to the center of target goal is used as dkd if Y_ > 0.6 * length: tdkdmin = hMath.getHeadingBetween(X_, Y_, Constant.RIGHT_GOAL_POST, length) tdkdmax = hMath.getHeadingBetween(X_, Y_, Constant.LEFT_GOAL_POST, length) if debugDKD: print "close to target goal, dkd directed to the target goal", Global.DKD[ 0] return (dkdup, tdkdmin, tdkdmax, 10, 170) # if close to own goal, direted out of own goal elif Y_ < 0.4 * length: if debugDKD: print "close to own goal, dkd directed out of the own goal" return (dkdbottom, 20, 160, 10, 170) # Now, the middle 20% of the field should have a continous change of direction. else: myy = Y_ - 0.4 * length ratio = myy / (0.2 * length) dkd = dkdup * ratio + dkdbottom * (1 - ratio) if debugDKD: print "middle 20%, linear interpolation" return (dkd, 40, 140, 20, 160)
def walkToMovingBall(ballD,ballH,velX,velY): #Indicator.showFacePattern([3,3,3,3,3]) id(ballD) #print "" #print "velocity X : ", velX #print "velocity Y : ", velY ballX = ballD * math.sin(hMath.DEG2RAD(ballH)) ballY = ballD * math.cos(hMath.DEG2RAD(ballH)) tx = ballX ty = ballY dx = velX dy = velY #print "ball : (", ballX, ",", ballY, ") ", ballH # closest target point so far minTarget = (Constant.LARGE_VAL,0,0) # try out next 150 frames which is 5 seconds for i in range(150): tty = ty / 1.5 # assuming we walking in 2 cm / frame (over-estimating) ttx = tx / 1.0 tt = max(tty,ttx) # If the robot can reach to the target point comfortably, # then go to the target point. if tt + FRAME_TO_PREPARE < i: #print "success" #print "target : (", tx, ",", ty, ") " fwd = ty left = tx if left > 0: left = hMath.EXTEND(left,Action.MAX_SKE_FF_LEFT_STP*0.7) elif left < 0: left = hMath.EXTEND(left,Action.MAX_SKE_FF_RIGHT_STP*0.7) th = hMath.getHeadingToRelative(tx,ty) turnCCW = hMath.normalizeAngle_180((th + ballH) / 2) Action.walk(fwd,left,turnCCW,"ddd",minorWalkType=Action.SkeFastForwardMWT) return True # If this target point has the smallest difference of time for # ball and time for robot to reach the point, then consider # this target point. elif minTarget[0] > tt - i: minTarget = (tt-i,tx,ty) # updating next possible target point tx += dx ty += dy # updating velocity by decaying dx *= 0.98 dy *= 0.98 if minTarget[0] != Constant.LARGE_VAL: fwd = minTarget[2] left = minTarget[1] if left > 0: left = hMath.EXTEND(left,Action.MAX_SKE_FF_LEFT_STP*0.7) elif left < 0: left = hMath.EXTEND(left,Action.MAX_SKE_FF_RIGHT_STP*0.7) th = hMath.getHeadingToRelative(tx,ty) turnCCW = hMath.normalizeAngle_180((th + ballH) / 2) Action.walk(fwd,left,turnCCW,"ddd",minorWalkType=Action.SkeFastForwardMWT) return True return False
def DecideNextAction(radius, aa, turndir, taOff, raOff, kp, ki, kd): global lastBx, lastBy, sum_reqTurn, last_reqTurn, lastFrame, lastDir skippedFrames = Global.frame - (lastFrame + 1) # Reset the running sum if the routine is not called. lastFrame = Global.frame # record the last frame Indicator.showFacePattern(Constant.FP_DIR_KICK) # No need to showFacePattern, because rAttacker has called already before entering this. myx, myy = Global.selfLoc.getPos() # My (the dog) position and its heading myh = Global.selfLoc.getHeading() if Global.vBall.getConfidence() > 2: bx, by = Global.vBall.getPos() # ball position lastBx, lastBy = Global.vBall.getPos() else: bx = lastBx by = lastBy # Find the lp, given the ball position, taOff and the attack angle # aa+180 because u want the tagental offset extended to behind the ball # the point where the dog leaves the locus (leaving point) lpx, lpy = hMath.getPointRelative(bx, by, (aa + 180), taOff) # Find the center of the circle, given the lp, the radius and attack angle, and where u are. angleFromBall = hMath.getHeadingBetween(bx, by, myx, myy) angleRelativeToAA = hMath.normalizeAngle_180(angleFromBall - aa) if angleRelativeToAA >= 0 and angleRelativeToAA <= 180: turndir = Constant.dANTICLOCKWISE else: turndir = Constant.dCLOCKWISE if turndir == Constant.dCLOCKWISE: cx, cy = hMath.getPointRelative(lpx, lpy, (aa - 90), (radius + raOff)) else: cx, cy = hMath.getPointRelative(lpx, lpy, (aa + 90), (radius + raOff)) c2my = hMath.getDistanceBetween(cx, cy, myx, myy) # distance btw me and center errL = radius - c2my # Indicates inside or outside of the circle. Used as adjustment in vector field. # If not outside of the circle, no point to use alpha(Angle between center and cloest tangent point) if c2my <= radius: alpha = 0 else: alpha = hMath.RAD2DEG(math.asin(radius / c2my)) # 0 <= alpha <= 90 beta = hMath.getHeadingBetween( myx, myy, cx, cy) # Angle between global x-axis and center # Fing the current vector if turndir == Constant.dANTICLOCKWISE: vectorArr = beta - 90 # Find the tangent vector perp to the c2my line first if errL > 0: vectorArr = vectorArr - (errL / radius * 45.0 ) # linear discrepency elif errL < 0: # When outside of the circle, the vector is vectorArr = beta - alpha # the tangent closest to the circle. else: vectorArr = beta + 90 if errL > 0: vectorArr = vectorArr + (errL / radius * 45.0) elif errL < 0: vectorArr = beta + alpha b2my = hMath.getDistanceBetween(bx, by, myx, myy) angleFromCenToMe = hMath.normalizeAngle_180( hMath.getHeadingBetween(cx, cy, myx, myy) - aa) distanceToAALine = b2my * math.sin(hMath.DEG2RAD(angleRelativeToAA)) ##~ verticalDistToBall = b2my * math.cos(hMath.DEG2RAD(angleRelativeToAA)) if (raOff > 0 and abs(distanceToAALine) < raOff): vectorArr = aa ##~ leftAdjust = -hMath.CLIP(distanceToAALine,3) elif (turndir == Constant.dANTICLOCKWISE and angleFromCenToMe < 0 and angleFromCenToMe > -90 and abs(distanceToAALine) < (raOff + radius)): sHoverToBall.DecideNextAction() Indicator.showFacePattern([1, 2, 1, 2, 0]) ##~ reverse(aa,turndir,taOff,verticalDistToBall) return elif (turndir == Constant.dCLOCKWISE and angleFromCenToMe < 90 and angleFromCenToMe > 0 and abs(distanceToAALine) < (raOff + radius)): sHoverToBall.DecideNextAction() Indicator.showFacePattern([1, 2, 1, 2, 0]) ##~ reverse(aa,turndir,taOff,verticalDistToBall) return # After the vector is found, the PID controller will handle the job. # Note: # When u tune, first tune the KP to improve the rise time, then the KD to improve # the overshoot, finally the KI for better steady state. # better see http:##www.engin.umich.edu/group/ctm/PID/PID.html before u touch them # thisTurn - The vector that brings the dog heading to the vector arrow thisTurn = hMath.CLIP(hMath.normalizeAngle_180(vectorArr - myh), 30.0) # For normal walk , kp = 40 ki = 8 kd = 5 KP = kp / 100.0 #0.5 ## Less Than 1 KD = kd / 100.0 #0.1 KI = ki / 100.0 #0.1 ## Has to be very small if skippedFrames > 0: sum_reqTurn = sum_reqTurn * math.pow(0.8, skippedFrames) last_reqTurn = 0 # clip the running sum to be as most causing a 8 degree change. if abs(sum_reqTurn) > (3.0 / KI): sum_reqTurn = hMath.CLIP(sum_reqTurn, 3.0 / KI) # if far away, there is no I and D effects, only the P. if c2my > (4.0 * radius): sum_reqTurn = 0 last_reqTurn = thisTurn # The real PID calculations #print thisTurn #print last_reqTurn #print sum_reqTurn ##~ print "==========new frame" ##~ print "ball pos %.2f , %.2f and my h %.2f" % (bx,by,myh) ##~ print "lp pos %.2f , %.2f and cx pos %.2f , %.2f" % (lpx,lpy,cx,cy) ##~ print "vectorArr %.2f and thisTurn %.2f " % (vectorArr,thisTurn) ##~ print "last %.2f and sum_reqTurn %.2f) " % (last_reqTurn,sum_reqTurn) ##~ print "P %.5f I %.5f D %.5f " % (KP * thisTurn,KI * sum_reqTurn,\ ##~ KD * (thisTurn - last_reqTurn)) ##~ reqTurn = (KP * thisTurn) + (KI * sum_reqTurn) + (KD * (thisTurn - last_reqTurn)) ##~ print "the reqTurn is %.2f" % (reqTurn) sum_reqTurn = sum_reqTurn + thisTurn # The integral part last_reqTurn = thisTurn # Used in the derivative part # Print the debugging info Action.walk(7, 0, hMath.CLIP(reqTurn, 30), Action.EllipticalWalk) ##~ print "Direction: " , turndir , "vectorArr %.2f" % (vectorArr) if ((angleRelativeToAA > 160 or angleRelativeToAA < -160) and abs(hMath.normalizeAngle_180(myh - aa)) < 20): sPawKick.firesPawKick(sPawKick.FIRE_PAWKICK_AUTO) Indicator.showFacePattern([1, 1, 1, 1, 0])
def walkToBall(ballD, ballH, getBehind=False, useVelocity=False): vel = VisionLink.getGPSBallVInfo(Constant.CTLocal) velX, velY = vel[0], vel[1] if useVelocity\ and (abs(velX) > 2 or abs(velY) > 2): #Indicator.showFacePattern([3,3,3,3,3]) velXY = hMath.getDistanceBetween(0, 0, velX, velY) if velXY == 0: velXY = 0.01 ballX, ballY = Global.ballX, Global.ballY tx, ty = ballX, ballY offsetX = DISTANCE_CONSTANT * velX / velXY offsetY = DISTANCE_CONSTANT * velY / velXY for i in range(30): # time that the ball needs to get to the target point timeBall = ( (DISTANCE_CONSTANT * i) / velXY) * (1 / 30.0) # seconds # time that the robot needs to get to the target point tx += offsetX ty += offsetY td = hMath.getDistanceBetween(0, 0, tx, ty) # timeRobot = (td / 30.0) # seconds # If the robot can reach to the target point comfortably, # then go to the target point. if timeRobot + TIME_TO_PREPARE < timeBall: th = hMath.getHeadingToRelative(tx, ty) relh = hMath.normalizeAngle_180(th - ballH) fComp = math.cos(hMath.DEG2RAD(relh)) lComp = math.sin(hMath.DEG2RAD(relh)) if fComp > 0: maxF = Action.MAX_FORWARD_SKE else: maxF = Action.MAX_BACKWARD_SKE if lComp > 0: maxL = Action.MAX_LEFT_SKE else: maxL = Action.MAX_RIGHT_SKE if maxF * abs(fComp) >= maxL * abs(lComp): scale = 1.0 / abs(fComp) else: scale = 1.0 / abs(lComp) fComp = scale * fComp lComp = scale * lComp fwd = maxF * fComp left = maxL * lComp Action.walk(fwd, left, ballH / 2, "ddd") return walkToStillBall(ballD, ballH, getBehind)