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 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 orbitBeforeKick(player):
"""
State for circling the ball when we're facing our goal.
"""
brain = player.brain
my = brain.my
if player.firstFrame():
player.orbitStartH = my.h
brain.CoA.setRobotGait(brain.motion)
brain.tracker.trackBall()
shotPoint = KickingHelpers.getShotCloseAimPoint(player)
bearingToGoal = MyMath.getRelativeBearing(my.x, my.y, my.h,
shotPoint[0],
shotPoint[1] )
spinDir = -MyMath.sign(bearingToGoal)
player.brain.nav.orbitAngle(spinDir * 90)
if not player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBall(player):
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('scanFindBall')
elif brain.ball.dist > constants.STOP_ORBIT_BALL_DIST:
return player.goLater('chase')
if player.brain.nav.isStopped() and not player.firstFrame():
return player.goLater('positionForKick')
return player.stay()
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 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 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 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 orbitBeforeKick(player):
"""
State for circling the ball when we're facing our goal.
"""
brain = player.brain
my = brain.my
if player.firstFrame():
player.orbitStartH = my.h
brain.CoA.setRobotGait(brain.motion)
brain.tracker.trackBall()
shotPoint = KickingHelpers.getShotCloseAimPoint(player)
bearingToGoal = my.getRelativeBearing(shotPoint)
spinDir = -MyMath.sign(bearingToGoal)
player.brain.nav.orbitAngle(spinDir * 90)
if not player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBall(player):
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('scanFindBall')
elif brain.ball.dist > constants.STOP_ORBIT_BALL_DIST:
return player.goLater('chase')
if player.brain.nav.isStopped() and not player.firstFrame():
return player.goLater('positionForKick')
return player.stay()
def penaltyRelocalize(player):
if player.firstFrame():
player.setWalk(1, 0, 0)
if player.brain.ball.on:
player.brain.tracker.trackBall()
return player.goLater('gamePlaying')
if player.brain.my.locScore == NogginConstants.GOOD_LOC or \
player.brain.my.locScore == NogginConstants.OK_LOC:
player.shouldRelocalizeCounter += 1
if player.shouldRelocalizeCounter > 30:
player.shouldRelocalizeCounter = 0
return player.goLater('gamePlaying')
else:
player.shouldRelocalizeCounter = 0
if not player.brain.motion.isHeadActive():
player.brain.tracker.locPans()
if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
direction = MyMath.sign(player.getWalk()[2])
if direction == 0:
direction = 1
player.setWalk(0 , 0, constants.RELOC_SPIN_SPEED * direction)
return player.stay()
def atHeading(my, targetHeading):
"""
Returns true if we are at a heading close enough to what we want
"""
hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))
return hDiff < constants.CLOSE_ENOUGH_H and \
my.uncertH < constants.LOC_IS_ACTIVE_H
def relocalize(player):
if player.firstFrame():
player.setWalk(constants.RELOC_X_SPEED, 0, 0)
if player.brain.my.locScore != NogginConstants.locScore.BAD_LOC:
player.shouldRelocalizeCounter += 1
if player.shouldRelocalizeCounter > 30:
player.shouldRelocalizeCounter = 0
return player.goLater(player.lastDiffState)
else:
player.shouldRelocalizeCounter = 0
if not player.brain.motion.isHeadActive():
player.brain.tracker.locPans()
if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
direction = MyMath.sign(player.getWalk()[2])
if direction == 0:
direction = 1
player.setWalk(0, 0, constants.RELOC_SPIN_SPEED * direction)
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 setSpeed(nav, x, y, theta):
"""
Wrapper method to easily change the walk vector of the robot
"""
walk = motion.WalkCommand(x=x,y=y,theta=theta)
nav.brain.motion.setNextWalkCommand(walk)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def step(nav, x, y, theta, numSteps):
"""
Wrapper method to easily change the walk vector of the robot
"""
steps = motion.StepCommand(x=x,y=y,theta=theta,numSteps=numSteps)
nav.brain.motion.sendStepCommand(steps)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
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 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 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 atHeading(nav):
"""
Returns true if we are at a heading close enough to what we want
"""
my = nav.brain.my
dest = nav.getDestination()
if nav.destType is constants.BALL:
return abs(nav.brain.ball.bearing) < constants.CLOSE_ENOUGH_H
hDiff = fabs(MyMath.sub180Angle(my.h - dest.h))
return hDiff < constants.CLOSE_ENOUGH_H and \
my.uncertH < constants.LOC_IS_ACTIVE_H
def getSpinOnlyParam(my, dest):
# Determine the speed to turn
# see if getRotScale can go faster
headingDiff = my.getRelativeBearing(dest)
if (fabs(headingDiff) < 25.0):
sTheta = 0.0
else:
sTheta = MyMath.sign(headingDiff) * constants.MAX_SPIN_MAGNITUDE * \
getRotScale(headingDiff)
sX, sY = 0, 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, 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 ballInMyBox(player):
if player.firstFrame():
player.brain.tracker.activeLoc()
player.brain.CoA.setRobotGait(player.brain.motion)
ball = player.brain.ball
if fabs(ball.bearing) > constants.BALL_APPROACH_BEARING_THRESH:
player.setSpeed(0, 0, constants.BALL_SPIN_SPEED *
MyMath.sign(ball.bearing) )
elif fabs(ball.bearing) < constants.BALL_APPROACH_BEARING_OFF_THRESH :
player.stopWalking()
if not player.ballInMyGoalBox():
return player.goLater('chase')
return player.stay()
def ballInMyBox(player):
if player.firstFrame():
player.brain.tracker.activeLoc()
player.brain.CoA.setRobotGait(player.brain.motion)
ball = player.brain.ball
if fabs(ball.bearing) > constants.BALL_APPROACH_BEARING_THRESH:
player.setWalk(0, 0,
constants.BALL_SPIN_SPEED * MyMath.sign(ball.bearing))
elif fabs(ball.bearing) < constants.BALL_APPROACH_BEARING_OFF_THRESH:
player.stopWalking()
if not player.ballInMyGoalBox():
return player.goLater('chase')
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 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 step(nav, x, y, theta, numSteps):
"""
Wrapper method to easily change the walk vector of the robot
"""
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
else:
nav.brain.CoA.setRobotSlowGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendStepsVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def setDribbleSpeed(nav, x, y, theta):
"""
Wrapper to set walk vector while using dribble gait
"""
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
else:
nav.brain.CoA.setDribbleGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendWalkVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
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 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 setSpeed(nav, x, y, theta):
"""
Wrapper method to easily change the walk vector of the robot
"""
# use backwards gait if appropriate
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
else:
nav.brain.CoA.setRobotGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendWalkVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def relocalize(player):
if player.firstFrame():
pass #player.stopWalking()
if player.brain.my.locScore == NogginConstants.GOOD_LOC or \
player.brain.my.locScore == NogginConstants.OK_LOC:
player.shouldRelocalizeCounter += 1
if player.shouldRelocalizeCounter > 15:
player.shouldRelocalizeCounter = 0
return player.goLater(player.lastDiffState)
else:
player.shouldRelocalizeCounter = 0
if not player.brain.motion.isHeadActive():
player.brain.tracker.locPans()
direction = MyMath.sign(player.getWalk()[2])
if direction == 0:
direction = 1
if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
player.setWalk(0, 0, constants.RELOC_SPIN_SPEED * direction)
return player.stay()
