def returnToGoal(player):
if player.firstFrame():
if player.lastDiffState == 'didIKickIt':
correctedDest = (RelRobotLocation(0.0, 0.0, 0.0) -
returnToGoal.kickPose)
else:
correctedDest = (
RelRobotLocation(0.0, 0.0, 0.0) -
RelRobotLocation(player.brain.interface.odometry.x,
player.brain.interface.odometry.y, 0.0))
if fabs(correctedDest.relX) < 5:
correctedDest.relX = 0.0
if fabs(correctedDest.relY) < 5:
correctedDest.relY = 0.0
if fabs(correctedDest.relH) < 5:
correctedDest.relH = 0.0
print "I'm returning to goal now!"
print("my correctedDest: ", correctedDest.relX, correctedDest.relY,
correctedDest.relH)
print("My odometry: ", player.brain.interface.odometry.x,
player.brain.interface.odometry.y,
player.brain.interface.odometry.h)
player.brain.nav.walkTo(correctedDest)
return Transition.getNextState(player, returnToGoal)
def positionForGoalieKick(player):
if player.firstFrame():
player.brain.tracker.lookStraightThenTrack()
if clearBall.ballSide == RIGHT:
player.kick = kicks.RIGHT_SHORT_STRAIGHT_KICK
else:
player.kick = kicks.LEFT_SHORT_STRAIGHT_KICK
ball = player.brain.ball
positionForGoalieKick.kickPose = RelRobotLocation(ball.rel_x - player.kick.setupX,
ball.rel_y - player.kick.setupY,
0)
positionForGoalieKick.speed = nav.GRADUAL_SPEED
player.brain.nav.goTo(positionForGoalieKick.kickPose,
speed = positionForGoalieKick.speed,
precision = nav.CLOSE_ENOUGH)
ball = player.brain.ball
positionForGoalieKick.kickPose = RelRobotLocation(ball.rel_x - player.kick.setupX,
ball.rel_y - player.kick.setupY,
0)
player.brain.nav.updateDest(positionForGoalieKick.kickPose)
if GoalieTransitions.ballReadyToKick(player, positionForGoalieKick.kickPose):
player.brain.nav.stand()
return player.goNow('kickBall')
return Transition.getNextState(player, positionForGoalieKick)
def orbitAngle(self, radius, angle):
"""
Orbits a point at a certain radius for a certain angle using walkTo
Splits the command into multiple smaller commands
Don't rely on it too much since it depends on the odometry of strafes
and turns which slip a lot
It will orbit in steps, each orbit taking ~30 degrees (more like 45
when I test it out)
"""
NavStates.walkingTo.destQueue.clear()
#@todo: make this a bit nicer or figure out a better way to do it
# split it up in 15 degree moves; good enough approximation for small radii
for k in range(0, abs(angle) / 15):
if angle > 0:
NavStates.walkingTo.destQueue.append(
RelRobotLocation(0.0, radius / 6, 0.0))
NavStates.walkingTo.destQueue.append(
RelRobotLocation(0.0, 0.0, -15))
else:
NavStates.walkingTo.destQueue.append(
RelRobotLocation(0.0, -radius / 6, 0.0))
NavStates.walkingTo.destQueue.append(
RelRobotLocation(0.0, 0.0, 15))
NavStates.walkingTo.speed = FAST_SPEED
self.switchTo('walkingTo')
def spinToFaceBall(player):
if player.firstFrame():
facingDest = RelRobotLocation(0.0, 0.0, 0.0)
if player.brain.ball.loc.bearing < 0.0:
facingDest.relH = -90
else:
facingDest.relH = 90
player.brain.nav.goTo(facingDest, nav.CLOSE_ENOUGH, nav.CAREFUL_SPEED)
return Transition.getNextState(player, spinToFaceBall)
def getDeltaOdometry(loc, startingOdo):
"""
returns a RelRobotLocation representing the RelRobotLocation of the current
odometry relative to a starting odometry
"""
(x1, y1, h1) = startingOdo
deltaOdometry = RelRobotLocation(loc.lastOdoX - x1,
loc.lastOdoY - y1,
loc.lastOdoTheta)
deltaOdometry.rotate(-h1)
return deltaOdometry
def spinToFaceBall(player):
if player.firstFrame():
facingDest = RelRobotLocation(0.0, 0.0, 0.0)
if player.brain.ball.bearing_deg < 0.0:
facingDest.relH = -90
else:
facingDest.relH = 90
player.brain.nav.goTo(facingDest,
nav.CLOSE_ENOUGH, nav.CAREFUL_SPEED)
return Transition.getNextState(player, spinToFaceBall)
def getOrbitLocation(radius, angle):
"""
Returns the RelRobotLocation destination of an orbit
"""
if angle > 0:
return RelRobotLocation(0.0, radius / 2, -angle)
else:
return RelRobotLocation(0.0, -radius / 2, -angle)
dest = RelRobotLocation(radius, 0, 0)
dest.rotate(-angle)
return RelRobotLocation(0.0, -dest.relY, -angle)
def spinToFaceBall(player):
facingDest = RelRobotLocation(0.0, 0.0, 0.0)
if player.brain.ball.bearing_deg < 0.0:
player.side = RIGHT
facingDest.relH = -90
else:
player.side = LEFT
facingDest.relH = 90
player.brain.nav.goTo(facingDest, nav.CLOSE_ENOUGH, nav.CAREFUL_SPEED)
if player.counter > 180:
return player.goLater('spinAtGoal')
return Transition.getNextState(player, spinToFaceBall)
def sideLineCheckShouldReposition(player):
x_dest = 0.0
y_dest = 0.0
h_dest = 0.0
if GoalieStates.watchWithLineChecks.correctFacing is False:
# print("incorrect facing")
return False
# reasonAbleFrontLine = False
# for line in GoalieStates.watchWithLineChecks.lines:
# if math.fabs(math.degrees(line.t) - constants.EXPECTED_FRONT_LINE_T) < 15.0:
# if line.r > 15.0:
# reasonAbleFrontLine = True
# if GoalieStates.watchWithLineChecks.numFixes < 1:
# # print "not enough fixes"
# return False
for line in GoalieStates.watchWithLineChecks.lines:
r = line.r
t = math.degrees(line.t)
# Assumptions: facing forward
# If we find a line that, judging by its t value, is likely the right line,
# of the goalbox, use the r value to correct the robot's x position
# Additional r < 200 check to throw away the side field lines
if math.fabs(t - constants.EXPECTED_RIGHT_LINE_T) < constants.T_THRESH \
and math.fabs(r - constants.EXPECTED_SIDE_LINE_R) > constants.R_THRESH \
and r < 170.0 and r != 0.0:
y_dest = constants.EXPECTED_SIDE_LINE_R - r
print "Right side was TRUE"
print("ydest: ", y_dest)
print("r: ", r)
player.homeDirections += [RelRobotLocation(x_dest, y_dest, h_dest)]
return True
# Assumptions: facing forward
# Same as above, except with the left side line
if math.fabs(t - constants.EXPECTED_LEFT_LINE_T) < constants.T_THRESH \
and math.fabs(r - constants.EXPECTED_SIDE_LINE_R) > constants.R_THRESH \
and r < 170.0 and r != 0.0:
y_dest = r - constants.EXPECTED_SIDE_LINE_R
print "Left side was TRUE"
print("ydest: ", y_dest)
print("r: ", r)
player.homeDirections += [RelRobotLocation(x_dest, y_dest, h_dest)]
return True
return False
def faceBall(player):
if player.firstFrame():
player.brain.tracker.trackBall()
print("ball at ", player.brain.ball.bearing_deg)
facingDest = RelRobotLocation(0.0, 0.0, 0.0)
facingDest.relH = player.brain.ball.bearing_deg
if player.inPosition == constants.FAR_LEFT_POSITION:
facingDest.relH += 10.0
player.brain.nav.walkTo(facingDest, speed=nav.FAST_SPEED)
if player.counter > 300 or player.brain.ball.vis.frames_off > 10:
return player.goLater('watch')
return Transition.getNextState(player, faceBall)
def average(locations):
x = 0.0
y = 0.0
h = 0.0
for item in locations:
x += item.relX
y += item.relY
h += item.relH
if len(locations) == 0:
return RelRobotLocation(0.0, 0.0, 0.0)
return RelRobotLocation(x / len(locations), y / len(locations),
h / len(locations))
def pace(player):
if player.firstFrame():
player.pacingCounter = 0
player.destinationArray = [#RelRobotLocation(7, 0, 0),
RelRobotLocation(0, -7, 0),
#RelRobotLocation(-7, 0, 0),
RelRobotLocation(0, 7, 0)]
if player.brain.nav.isStopped():
if player.pacingCounter > 3:
player.pacingCounter = 0
player.brain.nav.walkTo(player.destinationArray[player.pacingCounter])
player.pacingCounter += 1
return player.stay()
def dodge(nav):
# TODO: HACK FOR BRAZIL - THIS IS USED WHEN IT'S ARM-ONLY DETECTION
if nav.firstFrame():
## SET UP the dodge direction based on where the obstacle is
# if directly in front of us, move back and to one side based on
# where the goToPosition dest is
if dodge.armPosition == 1:
print "Dodging NORTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodge.dest = RelRobotLocation(-15, direction * 10, 0)
elif dodge.armPosition == 2:
print "Dodging NORTHEAST obstacle"
dodge.dest = RelRobotLocation(-5, 15, 0)
elif dodge.armPosition == 3:
print "Dodging EAST obstacle"
dodge.dest = RelRobotLocation(0, 20, 0)
elif dodge.armPosition == 4:
print "Dodging SOUTHEAST obstacle"
dodge.dest = RelRobotLocation(5, 15, 0)
# if directly behind us, move forward and to one side based on
# where the goToPosition dest is
elif dodge.armPosition == 5:
print "Dodging SOUTH obstacle"
relDest = helper.getRelativeDestination(nav.brain.loc,
goToPosition.dest)
if relDest.relY <= 0:
direction = -1
else:
direction = 1
dodge.dest = RelRobotLocation(15, direction * 10, 0)
elif dodge.armPosition == 6:
print "Dodging SOUTHWEST obstacle"
dodge.dest = RelRobotLocation(5, -15, 0)
elif dodge.armPosition == 7:
print "Dodging WEST obstacle"
dodge.dest = RelRobotLocation(0, -20, 0)
elif dodge.armPosition == 8:
print "Dodging NORTHWEST obstacle"
dodge.dest = RelRobotLocation(-5, -15, 0)
else:
return
dest = RelRobotLocation(dodge.dest.relX + random(),
dodge.dest.relY + random(),
dodge.dest.relH + random())
helper.setDestination(nav, dest, 0.5)
return Transition.getNextState(nav, dodge)
def destinationWalkingTo(nav):
"""
State to be used for destination walking.
"""
if nav.firstFrame():
destinationWalkingTo.enqueAZeroVector = False
if not nav.brain.motion.calibrated:
helper.stand(nav)
return nav.stay()
if nav.counter < 4:
# print("In dest walking to, walking in place")
helper.walkInPlace(nav)
return nav.stay()
destinationWalkingTo.speed = nav.velocity
if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
nav.velocity += copysign(Navigator.SPEED_CHANGE,
(nav.requestVelocity - nav.velocity))
if len(destinationWalkingTo.destQueue) > 0:
dest = destinationWalkingTo.destQueue.popleft()
helper.setDestination(nav, dest, destinationWalkingTo.speed,
destinationWalkingTo.kick)
# destinationWalkingTo.enqueAZeroVector = True
elif destinationWalkingTo.enqueAZeroVector:
helper.setDestination(nav, RelRobotLocation(0, 0, 0),
destinationWalkingTo.speed,
destinationWalkingTo.kick)
destinationWalkingTo.enqueAZeroVector = False
return nav.stay()
def repositionAfterWhiff(player):
if player.firstFrame():
# reset odometry
player.brain.interface.motionRequest.reset_odometry = True
player.brain.interface.motionRequest.timestamp = int(
player.brain.time * 1000)
if player.brain.ball.rel_y < 0.0:
player.kick = kicks.RIGHT_STRAIGHT_KICK
else:
player.kick = kicks.LEFT_STRAIGHT_KICK
kickPose = player.kick.getPosition()
repositionAfterWhiff.ballDest = RelRobotLocation(
player.brain.ball.rel_x - kickPose[0],
player.brain.ball.rel_y - kickPose[1], 0.0)
player.brain.nav.goTo(repositionAfterWhiff.ballDest, nav.CLOSE_ENOUGH,
nav.FAST_SPEED)
# if it took more than 5 seconds, forget it
if player.counter > 150:
returnToGoal.kickPose.relX += player.brain.interface.odometry.x
returnToGoal.kickPose.relY += player.brain.interface.odometry.y
returnToGoal.kickPose.relH += player.brain.interface.odometry.h
return player.goLater('returnToGoal')
kickPose = player.kick.getPosition()
repositionAfterWhiff.ballDest.relX = (player.brain.ball.rel_x -
kickPose[0])
repositionAfterWhiff.ballDest.relY = (player.brain.ball.rel_y -
kickPose[1])
return Transition.getNextState(player, repositionAfterWhiff)
def getKickPosition(player):
kick = kicks.SHORT_QUICK_LEFT_KICK #check this @!
ballLoc = player.brain.ball.loc
(kick_x, kick_y, kick_heading) = kick.getPosition()
dest = RelRobotLocation(ballLoc.relX - kick_x - 3, ballLoc.relY - kick_y,
0)
return dest
def positionForKick(player):
"""
Get the ball in the sweet spot
"""
if transitions.shouldRedecideKick(player):
return player.goLater('approachBall')
ball = player.brain.ball
positionForKick.kickPose = RelRobotLocation(
ball.rel_x - player.kick.setupX, ball.rel_y - player.kick.setupY, 0)
if player.firstFrame():
player.brain.tracker.lookStraightThenTrack()
if player.kick == kicks.M_LEFT_SIDE or player.kick == kicks.M_RIGHT_SIDE:
positionForKick.speed = Navigator.GRADUAL_SPEED
else:
positionForKick.speed = MIN_SPEED
player.brain.nav.destinationWalkTo(positionForKick.kickPose,
positionForKick.speed)
elif player.brain.ball.vis.on: # don't update if we don't see the ball
player.brain.nav.updateDestinationWalkDest(positionForKick.kickPose)
player.ballBeforeKick = player.brain.ball
if transitions.ballInPosition(player, positionForKick.kickPose):
if player.motionKick:
return player.goNow('executeMotionKick')
else:
player.brain.nav.stand()
return player.goNow('executeKick')
return player.stay()
def walkOut(player):
player.brain.nav.destinationWalkTo(RelRobotLocation(100, 0, 0),
Navigator.BRISK_SPEED)
if player.stateTime > 5:
return player.goNow('determineRole')
return player.stay()
def clearIt(player):
if player.firstFrame():
player.brain.tracker.trackBall()
if player.brain.ball.rel_y < 0.0:
player.side = RIGHT
player.kick = kicks.RIGHT_STRAIGHT_KICK
else:
player.side = LEFT
player.kick = kicks.LEFT_STRAIGHT_KICK
kickPose = player.kick.getPosition()
clearIt.ballDest = RelRobotLocation(
player.brain.ball.rel_x - kickPose[0],
player.brain.ball.rel_y - kickPose[1], 0.0)
# reset odometry
player.brain.interface.motionRequest.reset_odometry = True
player.brain.interface.motionRequest.timestamp = int(
player.brain.time * 1000)
clearIt.odoDelay = True
return player.stay()
if clearIt.odoDelay:
clearIt.odoDelay = False
player.brain.nav.goTo(clearIt.ballDest, nav.CLOSE_ENOUGH,
nav.FAST_SPEED)
kickPose = player.kick.getPosition()
clearIt.ballDest.relX = player.brain.ball.rel_x - kickPose[0]
clearIt.ballDest.relY = player.brain.ball.rel_y - kickPose[1]
return Transition.getNextState(player, clearIt)
def repositionAfterWhiff(player):
if player.firstFrame():
# reset odometry
player.brain.interface.motionRequest.reset_odometry = True
player.brain.interface.motionRequest.timestamp = int(player.brain.time * 1000)
# if player.kick in [kicks.RIGHT_SIDE_KICK, kicks.LEFT_SIDE_KICK]:
# pass
# elif player.brain.ball.rel_y < 0.0:
# player.kick = kicks.RIGHT_SHORT_STRAIGHT_KICK
# else:
# player.kick = kicks.LEFT_SHORT_STRAIGHT_KICK
kickPose = player.kick.getPosition()
repositionAfterWhiff.ballDest = RelRobotLocation(player.brain.ball.rel_x -
kickPose[0],
player.brain.ball.rel_y -
kickPose[1],
0.0)
player.brain.nav.goTo(repositionAfterWhiff.ballDest,
nav.CLOSE_ENOUGH,
nav.GRADUAL_SPEED)
# if it took more than 5 seconds, forget it
if player.counter > 350:
return player.goLater('returnUsingLoc')
kickPose = player.kick.getPosition()
repositionAfterWhiff.ballDest.relX = (player.brain.ball.rel_x -
kickPose[0])
repositionAfterWhiff.ballDest.relY = (player.brain.ball.rel_y -
kickPose[1])
return Transition.getNextState(player, repositionAfterWhiff)
def clearIt(player):
if player.firstFrame():
player.brain.tracker.trackBallFixedPitch()
if player.brain.ball.loc.relY < 0.0:
player.side = RIGHT
player.kick = kicks.RIGHT_STRAIGHT_KICK
else:
player.side = LEFT
player.kick = kicks.LEFT_STRAIGHT_KICK
kickPose = player.kick.getPosition()
clearIt.ballDest = RelRobotLocation(
player.brain.ball.loc.relX - kickPose[0],
player.brain.ball.loc.relY - kickPose[1], 0.0)
# reset odometry
player.brain.motion.resetOdometry()
clearIt.odoDelay = True
return player.stay()
if clearIt.odoDelay:
clearIt.odoDelay = False
player.brain.nav.goTo(clearIt.ballDest, nav.CLOSE_ENOUGH,
nav.FAST_SPEED)
kickPose = player.kick.getPosition()
clearIt.ballDest.relX = player.brain.ball.loc.relX - kickPose[0]
clearIt.ballDest.relY = player.brain.ball.loc.relY - kickPose[1]
return Transition.getNextState(player, clearIt)
def repositionAfterWhiff(player):
if player.firstFrame():
player.brain.motion.resetOdometry()
if player.brain.ball.loc.relY < 0.0:
player.kick = kicks.RIGHT_STRAIGHT_KICK
else:
player.kick = kicks.LEFT_STRAIGHT_KICK
kickPose = player.kick.getPosition()
repositionAfterWhiff.ballDest = RelRobotLocation(
player.brain.ball.loc.relX - kickPose[0],
player.brain.ball.loc.relY - kickPose[1], 0.0)
player.brain.nav.goTo(repositionAfterWhiff.ballDest, nav.CLOSE_ENOUGH,
nav.FAST_SPEED)
# if it took more than 5 seconds, forget it
if player.counter > 150:
returnToGoal.kickPose.relX += player.brain.loc.lastOdoX
returnToGoal.kickPose.relX += player.brain.loc.lastOdoY
returnToGoal.kickPose.relX += player.brain.loc.lastOdoTheta
return player.goLater('returnToGoal')
kickPose = player.kick.getPosition()
repositionAfterWhiff.ballDest.relX = (player.brain.ball.loc.relX -
kickPose[0])
repositionAfterWhiff.ballDest.relY = (player.brain.ball.loc.relY -
kickPose[1])
return Transition.getNextState(player, repositionAfterWhiff)
def frontLineCheckShouldReposition(player):
getLines(player)
x_dest = 0.0
y_dest = 0.0
h_dest = 0.0
if not GoalieStates.watchWithLineChecks.correctFacing:
return False
for line in GoalieStates.watchWithLineChecks.lines:
r = line.r
t = math.degrees(line.t)
# Assumptions: facing forward
# If we find a line that, judging by its t value, is likely the front line of
# the goalbox, use the r value to correct the robot's y position
# If have good t value and bad r value, reposition accordingly
# Additional r < 100 check to throw away the middle line
if math.fabs(t - constants.EXPECTED_FRONT_LINE_T) < constants.T_THRESH \
and math.fabs(r - constants.EXPECTED_FRONT_LINE_R) > constants.R_THRESH \
and r < 100.0:
x_dest = r - constants.EXPECTED_FRONT_LINE_R
print "Front was TRUE"
print x_dest
print r
player.homeDirections += [RelRobotLocation(x_dest, y_dest, h_dest)]
return True
if len(player.homeDirections) > constants.BUFFER_THRESH:
player.homeDirections = player.homeDirections[1:]
return False
def walkingTo(nav):
"""
State to be used for odometry walking.
"""
if nav.firstFrame():
nav.brain.interface.motionRequest.reset_odometry = True
nav.brain.interface.motionRequest.timestamp = int(nav.brain.time *
1000)
helper.stand(nav)
return nav.stay()
# TODO why check standing?
if nav.brain.interface.motionStatus.standing:
walkingTo.currentOdo = RelRobotLocation(nav.brain.interface.odometry.x,
nav.brain.interface.odometry.y,
nav.brain.interface.odometry.h)
if len(walkingTo.destQueue) > 0:
dest = walkingTo.destQueue.popleft()
helper.setOdometryDestination(nav, dest, walkingTo.speed)
elif locationsMatch(nav.destination, walkingTo.currentOdo):
return nav.goNow('standing')
return nav.stay()
def badRightCornerObservation(player):
corner = getRightGoalboxCorner(player)
if not corner:
return False
dDist = math.fabs(constants.EXPECTED_CORNER_DIST_FROM_CENTER -
corner.visual_detection.distance)
dBear = math.fabs(constants.EXPECTED_RIGHT_CORNER_BEARING_FROM_CENTER -
corner.visual_detection.bearing_deg)
if not (dDist > constants.CORNER_DISTANCE_THRESH
or dBear > constants.CORNER_BEARING_THRESH):
return False
homeRelX = -(field.GOALBOX_DEPTH - getCornerRelX(0, corner) -
constants.GOALIE_OFFSET)
homeRelY = field.GOALBOX_WIDTH / 2.0 + getCornerRelY(0, corner)
homeRelH = -getRobotGlobalHeading(0, corner)
player.homeDirections += [RelRobotLocation(homeRelX, homeRelY, homeRelH)]
if len(player.homeDirections) > constants.BUFFER_THRESH:
player.homeDirections = player.homeDirections[1:]
return True
def kickBall(player):
"""
Kick the ball
"""
if player.firstFrame():
# save odometry if this was your first kick
if player.lastDiffState == 'clearIt':
VisualStates.returnToGoal.kickPose = \
RelRobotLocation(player.brain.interface.odometry.x,
player.brain.interface.odometry.y,
player.brain.interface.odometry.h)
#otherwise add to previously saved odo
else:
VisualStates.returnToGoal.kickPose.relX += \
player.brain.interface.odometry.x
VisualStates.returnToGoal.kickPose.relY += \
player.brain.interface.odometry.y
VisualStates.returnToGoal.kickPose.relH += \
player.brain.interface.odometry.h
player.brain.tracker.trackBall()
player.executeMove(player.kick.sweetMove)
if player.counter > 10 and player.brain.nav.isStopped():
return player.goLater('didIKickIt')
return player.stay()
def positionForKick(player):
"""
Get the ball in the sweet spot
"""
if (transitions.shouldApproachBallAgain(player)
or transitions.shouldRedecideKick(player)):
player.inKickingState = False
return player.goLater('chase')
ball = player.brain.ball
kick_pos = player.kick.getPosition()
positionForKick.kickPose = RelRobotLocation(ball.rel_x - kick_pos[0],
ball.rel_y - kick_pos[1], 0)
#only enque the new goTo destination once
if player.firstFrame():
# Safer when coming from orbit in 1 frame. Still works otherwise, too.
player.brain.tracker.lookStraightThenTrack()
player.brain.nav.goTo(positionForKick.kickPose, Navigator.PRECISELY,
Navigator.GRADUAL_SPEED, False,
Navigator.ADAPTIVE)
else:
player.brain.nav.updateDest(positionForKick.kickPose)
if transitions.shouldFindBallKick(player) and player.counter > 15:
player.inKickingState = False
return player.goLater('chase')
if (transitions.ballInPosition(player, positionForKick.kickPose)
or player.brain.nav.isAtPosition()):
player.brain.nav.stand()
return player.goNow('kickBallExecute')
return player.stay()
def penaltyShotsGamePlaying(player):
if player.firstFrame():
player.stand()
player.brain.tracker.trackBall()
player.brain.nav.walkTo(RelRobotLocation(0.0, 30.0, 0.0))
return Transition.getNextState(player, penaltyShotsGamePlaying)
def faceBall(player):
if player.firstFrame():
player.brain.tracker.trackBall()
print("ball at ", player.brain.ball.bearing_deg)
facingDest = RelRobotLocation(0.0, 0.0, 0.0)
facingDest.relH = player.brain.ball.bearing_deg
if player.inPosition == constants.FAR_LEFT_POSITION:
facingDest.relH += 10.0
player.brain.nav.walkTo(facingDest, speed = nav.FAST_SPEED)
if player.counter > 300 or player.brain.ball.vis.frames_off > 10:
return player.goLater('watch')
return Transition.getNextState(player, faceBall)
def gamePlaying(player):
if player.firstFrame():
loc = player.brain.loc
my_current_loc = player.brain.my
my_current_odo = RobotLocation(loc.lastOdoX, loc.lastOdoY,
loc.lastOdoTheta)
# delta_loc = my_current_loc - player.my_last_loc
# delta_odo = my_current_odo - player.my_last_odo
if (player.testCounter > 0):
player.printf("Odometry: {0}".format(my_current_odo -
player.my_last_odo))
player.printf("Delta Odo {0}".format(
NavStates.walkingTo.deltaDest))
player.my_last_loc = RobotLocation(my_current_loc.x, my_current_loc.y,
my_current_loc.h)
player.my_last_odo = my_current_odo
if player.testCounter >= len(player.benchmark):
print "Done!"
return player.goLater("gamePenalized")
command = player.benchmark[player.testCounter]
player.testCounter += 1
player.numTestFrames = command[1]
walkVector = command[0]
player.brain.nav.walkTo(RelRobotLocation(*walkVector))
if player.counter == player.numTestFrames:
return player.goNow('nextCommand')
return player.stay()
def avoidRight(nav):
if nav.firstFrame():
avoidDest = RelRobotLocation(0, -25, 0)
helper.setOdometryDestination(nav, avoidDest)
return nav.stay()
return Transition.getNextState(nav, avoidRight)
def executeMotionKick(player):
"""
Do a motion kick.
"""
ball = player.brain.ball
executeMotionKick.kickPose = RelRobotLocation(ball.rel_x - player.kick.setupX,
ball.rel_y - player.kick.setupY,
0)
if player.firstFrame():
player.brain.nav.destinationWalkTo(executeMotionKick.kickPose,
nav.CAREFUL_SPEED,
player.kick)
elif player.brain.ball.vis.on: # don't update if we don't see the ball
player.brain.nav.updateDestinationWalkDest(executeMotionKick.kickPose)
elif player.kickedOut and not player.brain.ball.vis.on:
player.kickedOut = False
return player.goNow('spinSearch')
# TODO not ideal at all!
if player.counter > 40:
player.inKickingState = False
return player.goNow('afterKick')
return player.stay()
def spinToFaceBall(player):
if player.firstFrame():
facingDest = RelRobotLocation(0.0, 0.0, 0.0)
if player.brain.ball.bearing_deg < 0.0:
player.side = RIGHT
facingDest.relH = -90
else:
player.side = LEFT
facingDest.relH = 90
player.brain.nav.goTo(facingDest,
nav.CLOSE_ENOUGH, nav.CAREFUL_SPEED)
if player.counter > 180:
return player.goLater('spinAtGoal')
return Transition.getNextState(player, spinToFaceBall)
def shouldTurn(player):
if GoalieStates.watchWithLineChecks.numTurns > 1 \
and GoalieStates.watchWithLineChecks.numFixes < 2:
return False
if GoalieStates.watchWithLineChecks.numTurns == 2:
return False
h_dest = 0.0
for line in GoalieStates.watchWithLineChecks.lines:
r = line.r
t = math.degrees(line.t)
length = getLineLength(line)
if length < 30.0 and r > 30.0:
continue
# Fix this.. very hacky: basically return that we DON'T need to turn
# if we see a reasonable front line
if math.fabs(t - constants.EXPECTED_FRONT_LINE_T) < constants.T_THRESH:
h_dest = 0.0
player.homeDirections += [RelRobotLocation(0.0, 0.0, h_dest)]
print("t: ", t)
print("r: ", r)
return True
if math.fabs(t - constants.EXPECTED_RIGHT_LINE_T) < constants.T_THRESH \
or math.fabs(t - constants.EXPECTED_LEFT_LINE_T) < constants.T_THRESH \
or math.fabs(t - constants.EXPECTED_FRONT_LINE_T) < constants.T_THRESH \
or r > 100.0 or t == 0.0:
continue
# Assumptions: not facing forward....?
# Hopefully will find a line close by (r < 100) with an unusual t value
# and use the information to help correct itself
# Theoretically will usually be the long front line?
else:
h_dest = t - 90.0
player.homeDirections += [RelRobotLocation(0.0, 0.0, h_dest)]
print "Should turn was TRUE"
print("hdest: ", h_dest)
print("t was: ", t)
print("r was:", r)
return True
return False
def getStrafelessDest(dest):
if ((dest.relX > 150 and dest.relY < 50)
or (dest.relX <= 150 and dest.relX > 50 and dest.relY < 20)
or (dest.relX <= 50 and dest.relX > 20 and dest.relY < 10)):
#print "old dest: " + str(dest)
return RelRobotLocation(dest.relX, 0, dest.relH)
else:
return dest
def spinToFaceBall(player):
facingDest = RelRobotLocation(0.0, 0.0, 0.0)
# if player.brain.ball.bearing_deg < 0.0:
# player.side = RIGHT
# facingDest.relH = -90
# else:
# player.side = LEFT
# facingDest.relH = 90
# player.brain.interface.motionRequest.reset_odometry = True
# player.brain.interface.motionRequest.timestamp = int(player.brain.time * 1000)
facingDest.relH = player.brain.ball.bearing_deg
player.brain.nav.goTo(facingDest,
nav.CLOSE_ENOUGH,
nav.CAREFUL_SPEED)
# if player.counter > 180:
# return player.goLater('spinAtGoal')
return Transition.getNextState(player, spinToFaceBall)
