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()