def getWalkStraightParam(my, dest): relX, relH = 0, 0 if hasattr(dest, "relX") and \ hasattr(dest, "relH"): relX = dest.relX relH = dest.relH else: bearingDeg = my.getRelativeBearing(dest) distToDest = my.distTo(dest) relX = MyMath.getRelativeX(distToDest, bearingDeg) relH = MyMath.sub180Angle(dest.h - my.h) # calculate spin speed if (fabs(relH) < 25.0): sTheta = 0.0 else: #spin first spinGain = 20. / constants.APPROACH_THETA_WITH_GAIN_DIST # 20degs/sec in theta sTheta = relH * spinGain sTheta = MyMath.clip(sTheta, constants.OMNI_MAX_RIGHT_SPIN_SPEED, constants.OMNI_MAX_LEFT_SPIN_SPEED) return (0, 0, sTheta) # calculate forward speed if h is good. forwardGain = 20. / constants.APPROACH_X_WITH_GAIN_DIST # 20cm/sec in x direction sX = relX * forwardGain if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE: sX = 0 else: sX = MyMath.clip(sX, constants.GOTO_BACKWARD_SPEED, constants.GOTO_FORWARD_SPEED) return (sX, 0, 0)
def getWalkStraightParam(my, dest): distToDest = my.distTo(dest) if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST: gain = constants.GOTO_FORWARD_GAIN * distToDest else: gain = 1.0 sX = MyMath.clip(constants.GOTO_FORWARD_SPEED*gain, constants.WALK_TO_MIN_X_SPEED, constants.WALK_TO_MAX_X_SPEED) bearingDeg= my.getRelativeBearing(dest) if (fabs(bearingDeg) < 2.0): sTheta = 0.0 else: sTheta = MyMath.clip(MyMath.sign(bearingDeg) * constants.GOTO_STRAIGHT_SPIN_SPEED * getRotScale(bearingDeg), -constants.GOTO_STRAIGHT_SPIN_SPEED, constants.GOTO_STRAIGHT_SPIN_SPEED ) sY = 0 return (sX, sY, sTheta)
def getWalkBackParam(my, dest): relX, relH = 0, 0 if hasattr(dest, "relX") and \ hasattr(dest, "relH"): relX = dest.relX relH = dest.relH else: bearingDeg = my.getRelativeBearing(dest) distToDest = my.distTo(dest) relX = MyMath.getRelativeX(distToDest, bearingDeg) relH = MyMath.sub180Angle(dest.h - my.h) if not fabs(relH) > 150 : spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST sTheta = (180-relH) * spinGain sTheta = MyMath.clip(sTheta, constants.OMNI_MAX_RIGHT_SPIN_SPEED, constants.OMNI_MAX_LEFT_SPIN_SPEED) return ( 0, 0, sTheta) forwardGain = constants.APPROACH_X_WITH_GAIN_DIST sX = relX * forwardGain sX = MyMath.clip(sX, constants.GOTO_BACKWARD_SPEED, constants.GOTO_FORWARD_SPEED) return (sX, 0, 0)
def getOmniWalkParam(my, dest): # we use distance and bearing to get relX, relY which we already have # for the ball. be nice not to recalculate it. relX, relY, relH = 0, 0, 0 if hasattr(dest, "relX") and \ hasattr(dest, "relY"): relX = dest.relX relY = dest.relY else: bearingDeg = my.getRelativeBearing(dest) distToDest = my.distTo(dest) relX = MyMath.getRelativeX(distToDest, bearingDeg) relY = MyMath.getRelativeY(distToDest, bearingDeg) if hasattr(dest, "relH"): relH = dest.relH elif hasattr(dest, "bearing"): relH = dest.bearing else: relH = MyMath.sub180Angle(dest.h - my.h) # calculate forward speed forwardGain = constants.APPROACH_X_WITH_GAIN_DIST sX = relX * forwardGain if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE: sX = 0 else: sX = MyMath.clip(sX, constants.OMNI_REV_MAX_SPEED, constants.OMNI_FWD_MAX_SPEED) # calculate sideways speed strafeGain = constants.APPROACH_Y_WITH_GAIN_DIST sY = relY * strafeGain if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE: sY = 0 else: sY = MyMath.clip(sY, constants.OMNI_RIGHT_MAX_SPEED, constants.OMNI_LEFT_MAX_SPEED,) # calculate spin speed if (fabs(relH) < 10.0): sTheta = 0.0 else: spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST sTheta = relH * spinGain sTheta = MyMath.clip(sTheta, constants.OMNI_MAX_RIGHT_SPIN_SPEED, constants.OMNI_MAX_LEFT_SPIN_SPEED) denom = sqrt(sX*sX + sY*sY + sTheta*sTheta) / constants.OMNI_GAIN if denom != 0: sX /= denom sY /= denom sTheta /= denom return (sX, sY, sTheta)
def getOmniWalkParam(my, dest): # we use distance and bearing to get relX, relY which we already have # for the ball. be nice not to recalculate it. relX, relY, relH = 0, 0, 0 if hasattr(dest, "relX") and \ hasattr(dest, "relY") and \ hasattr(dest, "relH"): relX = dest.relX relY = dest.relY relH = dest.relH else: bearingDeg = my.getRelativeBearing(dest) distToDest = my.distTo(dest) relX = MyMath.getRelativeX(distToDest, bearingDeg) relY = MyMath.getRelativeY(distToDest, bearingDeg) relH = MyMath.sub180Angle(dest.h - my.h) # calculate forward speed forwardGain = 20. / constants.APPROACH_X_WITH_GAIN_DIST # 20cm/sec in x direction sX = relX * forwardGain if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE: sX = 0 else: sX = MyMath.clip(sX, constants.OMNI_REV_MAX_SPEED, constants.OMNI_FWD_MAX_SPEED) # calculate sideways speed strafeGain = 15. / constants.APPROACH_Y_WITH_GAIN_DIST # 15cm/sec in y direction sY = relY * strafeGain if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE: sY = 0 else: sY = MyMath.clip(sY, constants.OMNI_RIGHT_MAX_SPEED, constants.OMNI_LEFT_MAX_SPEED,) # calculate spin speed if (fabs(relH) < 25.0): sTheta = 0.0 else: spinGain = 20. / constants.APPROACH_THETA_WITH_GAIN_DIST # 20degs/sec in theta sTheta = relH * spinGain sTheta = MyMath.clip(sTheta, constants.OMNI_MAX_RIGHT_SPIN_SPEED, constants.OMNI_MAX_LEFT_SPIN_SPEED) # refine x and y speeds if (fabs(relH) > 50): sX = 0 sY = 0 elif (fabs(relH) > 35): sY = 0 return (sX, sY, sTheta)
def getOmniWalkParam(my, dest): # we use distance and bearing to get relX, relY which we already have # for the ball. be nice not to recalculate it. relX, relY = 0, 0 if hasattr(dest, "relX") and \ hasattr(dest, "relY") and \ hasattr(dest, "relH"): relX = dest.relX relY = dest.relY relH = dest.relH else: bearingDeg = my.getRelativeBearing(dest) distToDest = my.distTo(dest) relX = MyMath.getRelativeX(distToDest, bearingDeg) relY = MyMath.getRelativeY(distToDest, bearingDeg) relH = MyMath.sub180Angle(dest.h - my.h) # calculate forward speed forwardGain = constants.OMNI_GOTO_X_GAIN * relX sX = constants.OMNI_GOTO_FORWARD_SPEED * forwardGain sX = MyMath.clip(sX, constants.OMNI_MIN_X_SPEED, constants.OMNI_MAX_X_SPEED) if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE: sX = 0 # calculate sideways speed strafeGain = constants.OMNI_GOTO_Y_GAIN * relY sY = constants.OMNI_GOTO_STRAFE_SPEED * strafeGain sY = MyMath.clip(sY, constants.OMNI_MIN_Y_SPEED, constants.OMNI_MAX_Y_SPEED,) if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE: sY = 0 # calculate spin speed spinGain = constants.GOTO_SPIN_GAIN hDiff = MyMath.sub180Angle(dest.h - my.h) if (fabs(hDiff) < 2.0): sTheta = 0.0 else: sTheta = MyMath.sign(hDiff) * getRotScale(hDiff) * \ constants.OMNI_MAX_SPIN_SPEED * spinGain sTheta = MyMath.clip(sTheta, constants.OMNI_MIN_SPIN_SPEED, constants.OMNI_MAX_SPIN_SPEED) return (sX, sY, sTheta)
def approachBallWalk(player): """ Method that is used by both approach ball and dribble We use things as to when we should leave and how we should walk """ if not player.brain.play.isRole(GOALIE): if transitions.shouldNotGoInBox(player): return player.goLater('ballInMyBox') elif transitions.shouldChaseAroundBox(player): return player.goLater('chaseAroundBox') elif transitions.shouldApproachBallWithLoc(player): return player.goNow('approachBallWithLoc') elif transitions.shouldTurnToBall_ApproachBall(player): return player.goLater('turnToBall') elif not player.brain.tracker.activeLocOn and \ transitions.shouldScanFindBall(player): return player.goLater('scanFindBall') elif player.brain.tracker.activeLocOn and \ transitions.shouldScanFindBallActiveLoc(player): return player.goLater('scanFindBall') elif transitions.shouldAvoidObstacleDuringApproachBall(player): return player.goLater('avoidObstacle') # Determine our speed for approaching the ball ball = player.brain.ball if player.brain.play.isRole(GOALIE) and goalTran.dangerousBall(player): return player.goNow('approachDangerousBall') if ball.dist < constants.APPROACH_WITH_GAIN_DIST: sX = MyMath.clip(ball.dist * constants.APPROACH_X_GAIN, constants.MIN_APPROACH_X_SPEED, constants.MAX_APPROACH_X_SPEED) else: sX = constants.MAX_APPROACH_X_SPEED # Determine the speed to turn to the ball sTheta = MyMath.clip(ball.bearing * constants.APPROACH_SPIN_GAIN, -constants.APPROACH_SPIN_SPEED, constants.APPROACH_SPIN_SPEED) # Avoid spinning so slowly that we step in place if fabs(sTheta) < constants.MIN_APPROACH_SPIN_MAGNITUDE: sTheta = 0.0 # Set our walk towards the ball if ball.on: player.setWalk(sX, 0, sTheta) return player.stay()
def approachBallWalk(player): """ Method that is used by both approach ball and dribble We use things as to when we should leave and how we should walk """ if player.brain.playbook.role != pbc.GOALIE: if transitions.shouldNotGoInBox(player): return player.goLater('ballInMyBox') elif transitions.shouldChaseAroundBox(player): return player.goLater('chaseAroundBox') elif transitions.shouldApproachBallWithLoc(player): return player.goNow('approachBallWithLoc') elif transitions.shouldTurnToBall_ApproachBall(player): return player.goLater('turnToBall') elif not player.brain.tracker.activeLocOn and \ transitions.shouldScanFindBall(player): return player.goLater('scanFindBall') elif player.brain.tracker.activeLocOn and \ transitions.shouldScanFindBallActiveLoc(player): return player.goLater('scanFindBall') elif transitions.shouldAvoidObstacleDuringApproachBall(player): return player.goLater('avoidObstacle') # Determine our speed for approaching the ball ball = player.brain.ball if player.brain.playbook.role == pbc.GOALIE and goalTran.dangerousBall(player): return player.goNow('approachDangerousBall') if ball.dist < constants.APPROACH_WITH_GAIN_DIST: sX = MyMath.clip(ball.dist*constants.APPROACH_X_GAIN, constants.MIN_APPROACH_X_SPEED, constants.MAX_APPROACH_X_SPEED) else : sX = constants.MAX_APPROACH_X_SPEED # Determine the speed to turn to the ball sTheta = MyMath.clip(ball.bearing*constants.APPROACH_SPIN_GAIN, -constants.APPROACH_SPIN_SPEED, constants.APPROACH_SPIN_SPEED) # Avoid spinning so slowly that we step in place if fabs(sTheta) < constants.MIN_APPROACH_SPIN_MAGNITUDE: sTheta = 0.0 # Set our walk towards the ball if ball.on: player.setSpeed(sX,0,sTheta) return player.stay()
def positionForKick(player): """ State to align on the ball once we are near it """ if player.firstFrame(): player.brain.CoA.setRobotSlowGait(player.brain.motion) ball = player.brain.ball player.inKickingState = True # Leave this state if necessary if transitions.shouldKick(player): player.brain.CoA.setRobotGait(player.brain.motion) return player.goNow('waitBeforeKick') elif transitions.shouldScanFindBall(player): player.inKickingState = False player.brain.CoA.setRobotGait(player.brain.motion) return player.goLater('scanFindBall') elif transitions.shouldTurnToBallFromPositionForKick(player): player.inKickingState = False player.brain.CoA.setRobotGait(player.brain.motion) return player.goLater('turnToBall') elif transitions.shouldApproachFromPositionForKick(player): player.inKickingState = False player.brain.CoA.setRobotGait(player.brain.motion) return player.goLater('approachBall') # Determine approach speed targetY = ball.relY sY = MyMath.clip(targetY * constants.PFK_Y_GAIN, constants.PFK_MIN_Y_SPEED, constants.PFK_MAX_Y_SPEED) sY = max(constants.PFK_MIN_Y_MAGNITUDE,sY) * MyMath.sign(sY) if transitions.shouldApproachForKick(player): targetX = (ball.relX - (constants.BALL_KICK_LEFT_X_CLOSE + constants.BALL_KICK_LEFT_X_FAR) / 2.0) sX = MyMath.clip(ball.relX * constants.PFK_X_GAIN, constants.PFK_MIN_X_SPEED, constants.PFK_MAX_X_SPEED) else: sX = 0.0 if ball.on: player.setSpeed(sX,sY,0) return player.stay()
def spinToBall(player): """ State to spin to turn to the ball """ if player.firstFrame(): player.stopWalking() player.brain.tracker.trackBall() ball = player.brain.ball turnRate = MyMath.clip(ball.locBearing*ChaseConstants.BALL_SPIN_GAIN, -ChaseConstants.BALL_SPIN_SPEED, ChaseConstants.BALL_SPIN_SPEED) if transitions.atSpinBallDir(player): return player.goLater('atSpinBallPosition') elif transitions.shouldSpinFindBallPosition(player): return player.goLater('spinFindBallPosition') elif player.currentSpinDir != MyMath.sign(turnRate): player.stopWalking() player.currentSpinDir = MyMath.sign(turnRate) elif player.stoppedWalk and ball.on and player.brain.nav.isStopped(): player.setWalk(x=0,y=0,theta=turnRate) return player.stay()
def turnToBall(player): """ Rotate to align with the ball. When we get close, we will approach it """ ball = player.brain.ball if player.firstFrame(): player.hasAlignedOnce = False player.brain.tracker.trackBall() player.brain.CoA.setRobotGait(player.brain.motion) # Determine the speed to turn to the ball turnRate = MyMath.clip(ball.bearing*constants.BALL_SPIN_GAIN, -constants.BALL_SPIN_SPEED, constants.BALL_SPIN_SPEED) # Avoid spinning so slowly that we step in place if fabs(turnRate) < constants.MIN_BALL_SPIN_MAGNITUDE: turnRate = MyMath.sign(turnRate)*constants.MIN_BALL_SPIN_MAGNITUDE if ball.on: player.setSpeed(x=0,y=0,theta=turnRate) if transitions.shouldKick(player): return player.goNow('waitBeforeKick') elif transitions.shouldPositionForKick(player): return player.goNow('positionForKick') elif transitions.shouldApproachBall(player): return player.goLater('approachBall') elif transitions.shouldScanFindBall(player): return player.goLater('scanFindBall') return player.stay()
def spinToBall(player): """ State to spin to turn to the ball """ if player.firstFrame(): player.stopWalking() player.brain.tracker.trackBall() ball = player.brain.ball turnRate = MyMath.clip(ball.locBearing * ChaseConstants.BALL_SPIN_GAIN, -ChaseConstants.BALL_SPIN_SPEED, ChaseConstants.BALL_SPIN_SPEED) if transitions.atSpinBallDir(player): return player.goLater('atSpinBallPosition') elif transitions.shouldSpinFindBallPosition(player): return player.goLater('spinFindBallPosition') elif player.currentSpinDir != MyMath.sign(turnRate): player.stopWalking() player.currentSpinDir = MyMath.sign(turnRate) elif player.stoppedWalk and ball.on and player.brain.nav.isStopped(): player.setWalk(x=0, y=0, theta=turnRate) return player.stay()
def pfk_final(nav): """ we're done spinning and aligned for y. approach to final relX with x only. (may need to accept minor y changes in future) """ ball = nav.brain.ball (x_offset, y_offset, heading) = nav.kick.getPosition() x_diff = ball.relX - x_offset # arbitrary if fabs(x_diff) < PFK_CLOSE_ENOUGH_XY: sX = 0.0 else: sX = MyMath.clip(x_diff * PFK_X_GAIN, PFK_MIN_X_SPEED, PFK_MAX_X_SPEED) sX = max(PFK_MIN_X_MAGNITUDE,sX) * MyMath.sign(sX) helper.setSlowSpeed(nav,sX, 0.0, 0.0) # kicking time! if sX == 0.0: return nav.goNow('stop') return nav.stay()
def turnToBall(player): """ Rotate to align with the ball. When we get close, we will approach it """ ball = player.brain.ball if player.firstFrame(): player.hasAlignedOnce = False player.brain.tracker.trackBall() player.brain.CoA.setRobotGait(player.brain.motion) # Determine the speed to turn to the ball turnRate = MyMath.clip(ball.bearing * constants.BALL_SPIN_GAIN, -constants.BALL_SPIN_SPEED, constants.BALL_SPIN_SPEED) # Avoid spinning so slowly that we step in place if fabs(turnRate) < constants.MIN_BALL_SPIN_MAGNITUDE: turnRate = MyMath.sign(turnRate) * constants.MIN_BALL_SPIN_MAGNITUDE if ball.on: player.setWalk(x=0, y=0, theta=turnRate) if transitions.shouldKick(player): return player.goNow('waitBeforeKick') elif transitions.shouldPositionForKick(player): return player.goNow('positionForKick') elif transitions.shouldApproachBall(player): return player.goLater('approachBall') elif transitions.shouldScanFindBall(player): return player.goLater('scanFindBall') return player.stay()
def positionForKick(player): """ State to align on the ball once we are near it """ if player.firstFrame(): player.brain.CoA.setRobotSlowGait(player.brain.motion) ball = player.brain.ball player.inKickingState = True # Leave this state if necessary if transitions.shouldKick(player): player.brain.CoA.setRobotGait(player.brain.motion) return player.goNow('waitBeforeKick') elif transitions.shouldScanFindBall(player): player.inKickingState = False player.brain.CoA.setRobotGait(player.brain.motion) return player.goLater('scanFindBall') elif transitions.shouldTurnToBallFromPositionForKick(player): player.inKickingState = False player.brain.CoA.setRobotGait(player.brain.motion) return player.goLater('turnToBall') elif transitions.shouldApproachFromPositionForKick(player): player.inKickingState = False player.brain.CoA.setRobotGait(player.brain.motion) return player.goLater('approachBall') # Determine approach speed targetY = ball.relY sY = MyMath.clip(targetY * constants.PFK_Y_GAIN, constants.PFK_MIN_Y_SPEED, constants.PFK_MAX_Y_SPEED) sY = max(constants.PFK_MIN_Y_MAGNITUDE, sY) * MyMath.sign(sY) if transitions.shouldApproachForKick(player): # targetX = (ball.relX - # (constants.BALL_KICK_LEFT_X_CLOSE + # constants.BALL_KICK_LEFT_X_FAR) / 2.0) sX = MyMath.clip(ball.relX * constants.PFK_X_GAIN, constants.PFK_MIN_X_SPEED, constants.PFK_MAX_X_SPEED) else: sX = 0.0 if ball.on: player.setWalk(sX, sY, 0) return player.stay()
def pfk_xy(nav): """ ball bearing is outside safe limit, we're in danger of losing the ball. position x,y only """ ball = nav.brain.ball if ball.relX < SAFE_BALL_REL_X and \ ball.dist < SAFE_TO_STRAFE_DIST: return nav.goNow('pfk_x') """ if fabs(ball.bearing) < START_SPIN_BEARING: print "bearing to ball: %g" % ball.bearing return nav.goNow('pfk_all') """ (x_offset, y_offset, heading) = nav.kick.getPosition() target_y = ball.relY - y_offset # arbitrary if fabs(target_y) < PFK_CLOSE_ENOUGH_XY: return nav.goNow('pfk_final') else: sY = MyMath.clip(target_y * PFK_Y_GAIN, PFK_MIN_Y_SPEED, PFK_MAX_Y_SPEED) sY = max(PFK_MIN_Y_MAGNITUDE,sY) * MyMath.sign(sY) x_diff = ball.relX - SAFE_BALL_REL_X # arbitrary if fabs(x_diff) < PFK_CLOSE_ENOUGH_XY: sX = 0.0 else: sX = MyMath.clip(x_diff * PFK_X_GAIN, PFK_MIN_X_SPEED, PFK_MAX_X_SPEED) sX = max(PFK_MIN_X_MAGNITUDE,sX) * MyMath.sign(sX) # in position, let's kick the ball! if (sX == 0.0 and sY == 0.0): return nav.goNow('stop') helper.setSlowSpeed(nav,sX,sY,0.0) return nav.stay()
def getOmniWalkParam(my, dest): bearing = radians(my.getRelativeBearing(dest)) distToDest = my.dist(dest) # calculate forward speed forwardGain = constants.OMNI_GOTO_X_GAIN * distToDest* \ cos(bearing) sX = constants.OMNI_GOTO_FORWARD_SPEED * forwardGain sX = MyMath.clip(sX, constants.OMNI_MIN_X_SPEED, constants.OMNI_MAX_X_SPEED) if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE: sX = 0 # calculate sideways speed strafeGain = constants.OMNI_GOTO_Y_GAIN * distToDest* \ sin(bearing) sY = constants.OMNI_GOTO_STRAFE_SPEED * strafeGain sY = MyMath.clip(sY, constants.OMNI_MIN_Y_SPEED, constants.OMNI_MAX_Y_SPEED,) if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE: sY = 0 if atDestinationCloser(my, dest): sX = sY = 0.0 # calculate spin speed spinGain = constants.GOTO_SPIN_GAIN spinDir = my.spinDirToHeading(dest.h) sTheta = spinDir * fabs(my.h - dest.h) * spinGain sTheta = MyMath.clip(sTheta, constants.OMNI_MIN_SPIN_SPEED, constants.OMNI_MAX_SPIN_SPEED) if fabs(sTheta) < constants.OMNI_MIN_SPIN_MAGNITUDE: sTheta = 0.0 if atHeading(my, dest.h): sTheta = 0.0 return (sX, sY, sTheta)
def rotAlignOnBall(player): '''Rotate to align with the ball. When we get close, we will approach it ''' turnRate = MyMath.clip(player.brain.ball.bearing*0.5,-10.,10.) player.printf( "Ball bearing i "+ str(player.brain.ball.bearing)+ " turning at "+str(turnRate)) player.setSpeed(x=0,y=0,theta=turnRate) if player.brain.ball.on and player.brain.ball.bearing < BALL_BEARING_THRESH : return player.goNow('approachBall') if player.brain.ball.framesOff > FRAMES_OFF_THRESH: return player.goNow('scanFindBall') return player.stay()
def getSpinOnlyParam(my, dest): # Determine the speed to turn bearing = my.getRelativeBearing(dest) if (fabs(bearing) < 5.0): sTheta = 0.0 else: spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST sTheta = bearing * spinGain sTheta = MyMath.clip(sTheta, constants.GOTO_RIGHT_SPIN_SPEED, constants.GOTO_LEFT_SPIN_SPEED) sX, sY = 0, 0 return (sX, sY, sTheta)
def getWalkSpinParam(my, dest): """ Takes an x, y destinatino and walks towards it. Does not worry about final heading, only about getting to the x,y. """ if hasattr(dest, "bearing"): bearing = dest.bearing else: bearing = my.getRelativeBearing(dest) if hasattr(dest, "dist"): dist = dest.dist else: dist = my.distTo(dest) # calculate forward speed sX = dist * constants.APPROACH_X_WITH_GAIN_DIST if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE or \ fabs(bearing) > 40.: sX = 0 else: sX = MyMath.clip(sX, constants.OMNI_REV_MAX_SPEED, constants.OMNI_FWD_MAX_SPEED) # calculate spin speed if (fabs(bearing) < 5.0): sTheta = 0.0 else: sTheta = bearing * constants.APPROACH_THETA_WITH_GAIN_DIST sTheta = MyMath.clip(sTheta, constants.OMNI_MAX_RIGHT_SPIN_SPEED, constants.OMNI_MAX_LEFT_SPIN_SPEED) return (sX, 0, sTheta)
def getWalkSpinParam(my, dest): relX, relH = 0, 0 if hasattr(dest, "relX") and \ hasattr(dest, "relH"): relX = dest.relX relH = dest.relH else: bearingDeg = my.getRelativeBearing(dest) distToDest = my.distTo(dest) relX = MyMath.getRelativeX(distToDest, bearingDeg) relH = MyMath.sub180Angle(dest.h - my.h) # calculate forward speed forwardGain = 1./constants.APPROACH_X_WITH_GAIN_DIST sX = relX * forwardGain if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE: sX = 0 else: sX = MyMath.clip(sX, constants.OMNI_REV_MAX_SPEED, constants.OMNI_FWD_MAX_SPEED) # calculate spin speed if (fabs(relH) < 5.0): sTheta = 0.0 else: spinGain = 1./constants.APPROACH_THETA_WITH_GAIN_DIST sTheta = relH * spinGain sTheta = MyMath.clip(sTheta, constants.OMNI_MAX_RIGHT_SPIN_SPEED, constants.OMNI_MAX_LEFT_SPIN_SPEED) # Correct sX if fabs(relH) > 50.: sX = 0 return (sX, 0, sTheta)
def pfk_y(nav, ball, targetY): targetDist = ball.relY - targetY if (fabs(targetDist) <= PFK_CLOSE_ENOUGH_Y): nav.stopY = True return 0 if (targetDist > 0): # Move left to match ball with target # Change the distance to a speed and clip within vector limits sY = MyMath.clip(targetDist/PFK_MAX_Y_SPEED_DIST, PFK_MIN_Y_MAGNITUDE, PFK_LEFT_SPEED) return sY else: # Move right to match ball with target # Change the distance to a speed and clip within vector limits sY = MyMath.clip(targetDist/PFK_MAX_Y_SPEED_DIST, PFK_RIGHT_SPEED, -PFK_MIN_Y_MAGNITUDE) return sY
def pfk_x(nav, ball, targetX): targetDist = ball.relX - targetX if (fabs(targetDist) <= PFK_CLOSE_ENOUGH_X): nav.stopX = True return 0 if (targetDist > 0): # Move foward to match ball with target # Change the distance to a speed and clip within vector limits sX = MyMath.clip(targetDist/PFK_MAX_X_SPEED_DIST, PFK_MIN_X_MAGNITUDE, PFK_FWD_SPEED) return sX else: # Move Backwards Slowly return -.5
def getWalkSpinParam(my, dest): relX = 0 bearingDeg = my.getRelativeBearing(dest) distToDest = my.distTo(dest) if hasattr(dest, "relX"): relX = dest.relX else: relX = MyMath.getRelativeX(distToDest, bearingDeg) # calculate ideal max forward speed sX = constants.GOTO_FORWARD_SPEED * MyMath.sign(relX) if (fabs(bearingDeg) < 2.0): sTheta = 0.0 else: # calculate ideal max spin speed sTheta = (MyMath.sign(bearingDeg) * getRotScale(bearingDeg) * constants.OMNI_MAX_SPIN_SPEED) absSTheta = fabs(sTheta) if fabs(bearingDeg) > 20: sX = MyMath.clip(sX, constants.OMNI_MIN_X_SPEED, constants.OMNI_MAX_X_SPEED) sTheta = MyMath.sign(sTheta)* constants.OMNI_MAX_SPIN_SPEED elif fabs(bearingDeg) > 35: sX = 0 sTheta = constants.MAX_SPIN_SPEED * MyMath.sign(sTheta) gain = 1.0 if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST: gain = constants.GOTO_CLOSE_GAIN return (sX * gain, 0, sTheta * gain)
def pfk_all(nav): """ ball bearing is inside safe margin for spinning try to get almost all heading adjustment done here move x, y, and theta """ if nav.firstFrame(): nav.stopTheta = 0 nav.stopY_Theta = 0 print "entered from: ", nav.lastDiffState (x_offset, y_offset, heading) = nav.kick.getPosition() ball = nav.brain.ball # calculate spin speed # hDiff = MyMath.sub180Angle(heading - nav.brain.my.h) # rotate to ball here, we will strafe to aim kick later hDiff = MyMath.sub180Angle(ball.bearing) if (fabs(hDiff) < PFK_CLOSE_ENOUGH_THETA): sTheta = 0.0 else: sTheta = MyMath.sign(hDiff) * constants.GOTO_SPIN_SPEED * \ walker.getCloseRotScale(hDiff) sTheta = MyMath.clip(sTheta, constants.OMNI_MIN_SPIN_SPEED, constants.OMNI_MAX_SPIN_SPEED) if fabs(hDiff) < PFK_CLOSE_ENOUGH_THETA: nav.stopTheta += 1 if nav.stopTheta > BUFFER_FRAMES_THRESHOLD: return nav.goNow('pfk_xy') else: nav.stopTheta = 0 # if the ball is outside safe bearing and we're spinning away from it # or we're spinning towards it but we're likely to spin into it... # or we're close enough to the correct bearing if fabs(ball.bearing) > STOP_SPIN_BEARING and \ ((MyMath.sign(ball.bearing) != MyMath.sign(hDiff)) or ball.relX < SAFE_BALL_REL_X \ or sTheta == 0.0): return nav.goNow('pfk_xy') target_y = ball.relY - y_offset # arbitrary if fabs(target_y) < PFK_CLOSE_ENOUGH_XY: sY = 0 else: sY = MyMath.clip(target_y * PFK_Y_GAIN, PFK_MIN_Y_SPEED, PFK_MAX_Y_SPEED) sY = max(PFK_MIN_Y_MAGNITUDE,sY) * MyMath.sign(sY) if sY == 0.0 and sTheta == 0.0: nav.stopY_Theta += 1 if nav.stopY_Theta > BUFFER_FRAMES_THRESHOLD: return nav.goNow('pfk_final') else: nav.stopY_Theta = 0 x_diff = ball.relX - SAFE_BALL_REL_X # arbitrary if fabs(x_diff) < PFK_CLOSE_ENOUGH_XY: sX = 0.0 else: sX = MyMath.clip(x_diff * PFK_X_GAIN, PFK_MIN_X_SPEED, PFK_MAX_X_SPEED) sX = max(PFK_MIN_X_MAGNITUDE,sX) * MyMath.sign(sX) print "hDiff:%g target_y:%g x_diff:%g" % (hDiff, target_y, x_diff) print "sTheta:%g sY:%g sX:%g" % (sTheta, sY, sX) helper.setSlowSpeed(nav,sX,sY,sTheta) return nav.stay()