def avoidRightObstacle(nav):
"""
dodges left if we only detect something to the left of us
"""
if nav.firstFrame():
nav.doneAvoidingCounter = 0
nav.printf(nav.brain.sonar)
nav.printf("Avoid by left dodge");
helper.setSpeed(nav, (0, constants.DODGE_LEFT_SPEED, 0))
avoidLeft = navTrans.shouldAvoidObstacleLeft(nav)
avoidRight = navTrans.shouldAvoidObstacleRight(nav)
if (avoidLeft and avoidRight):
return nav.goLater('avoidFrontObstacle')
elif avoidLeft:
return nav.goLater('avoidLeftObstacle')
elif avoidRight:
nav.doneAvoidingCounter -= 1
nav.doneAvoidingCounter = max(0, nav.doneAvoidingCounter)
return nav.stay()
else:
nav.doneAvoidingCounter += 1
if nav.doneAvoidingCounter > constants.DONE_AVOIDING_FRAMES_THRESH:
nav.shouldAvoidObstacleRight = 0
nav.shouldAvoidObstacleLeft = 0
return nav.goLater(nav.preAvoidState)
return nav.stay()
def dodge(nav):
if nav.firstFrame():
nav.dodging = True
if nav.brain.ball.vis.frames_off < 5:
nav.brain.tracker.trackBall()
else:
nav.brain.tracker.trackObstacle(dodge.obstaclePosition)
## 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
dodge.speed = Navigator.BRISK_SPEED
obstacleInfo = constants.OBS_DICT[dodge.obstaclePosition]
helper.createAndSendWalkVector(nav,
dodge.speed*obstacleInfo[0],
dodge.speed*obstacleInfo[1],
0)
print "Dodging ", obstacleInfo[2], " Obstacle"
if navTrans.doneDodging(nav):
nav.dodging = False
nav.brain.tracker.repeatBasicPan()
return nav.goLater('briefStand')
return nav.stay()
def dodge(nav):
# return
#I'm making an executive decision and TURNING OFF DODGING
if nav.firstFrame():
nav.dodging = True
if nav.brain.ball.vis.frames_off < 5:
nav.brain.tracker.trackBall()
else:
nav.brain.tracker.trackObstacle(dodge.obstaclePosition)
## 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
dodge.speed = Navigator.BRISK_SPEED
obstacleInfo = constants.OBS_DICT[dodge.obstaclePosition]
helper.createAndSendWalkVector(nav, dodge.speed * obstacleInfo[0],
dodge.speed * obstacleInfo[1], 0)
print "Dodging ", obstacleInfo[2], " Obstacle"
if navTrans.doneDodging(nav):
nav.dodging = False
nav.brain.tracker.repeatWideSnapPan()
return nav.goLater('briefStand')
return nav.stay()
def walking(nav):
"""
State to be used for velocity walking.
"""
helper.setSpeed(nav, walking.speeds)
if navTrans.shouldDodge(nav):
return nav.goNow('dodge')
return Transition.getNextState(nav, walking)
def walking(nav):
"""
State to be used for velocity walking.
"""
helper.setSpeed(nav, walking.speeds)
if navTrans.shouldDodge(nav):
return nav.goNow('dodge')
return Transition.getNextState(nav, walking)
def avoidFrontObstacle(nav):
# Backup
# strafe away from the closer one?
# strafe towards dest?
# ever a good time to backup?
# we'll probably want to go forward again and most obstacle
# are moving, so pausing might make more sense
# TODO figure this out maybe potential field will fix this for us???
if nav.firstFrame():
nav.doneAvoidingCounter = 0
nav.printf(nav.brain.sonar)
nav.printf("Avoid by backup");
helper.setSpeed(nav, (constants.DODGE_BACK_SPEED, 0, 0))
avoidLeft = navTrans.shouldAvoidObstacleLeft(nav)
avoidRight = navTrans.shouldAvoidObstacleRight(nav)
if (avoidLeft and avoidRight):
nav.doneAvoidingCounter -= 1
nav.doneAvoidingCounter = max(0, nav.doneAvoidingCounter)
return nav.stay()
elif avoidRight:
return nav.goLater('avoidRightObstacle')
elif avoidLeft:
return nav.goLater('avoidLeftObstacle')
else:
nav.doneAvoidingCounter += 1
if nav.doneAvoidingCounter > constants.DONE_AVOIDING_FRAMES_THRESH:
nav.shouldAvoidObstacleRight = 0
nav.shouldAvoidObstacleLeft = 0
return nav.goLater(nav.preAvoidState)
return nav.stay()
def avoidObstacle(nav):
"""
If we detect something in front of us, dodge it
"""
if nav.firstFrame():
nav.brain.speech.say("Avoid obstacle")
avoidLeft = navTrans.shouldAvoidObstacleLeft(nav)
avoidRight = navTrans.shouldAvoidObstacleRight(nav)
# store previous state here, b/c lastDiffState gets
# replaced when we perform 'goNow'
nav.preAvoidState = nav.lastDiffState
if (avoidLeft and avoidRight):
return nav.goNow('avoidFrontObstacle')
elif avoidLeft:
return nav.goNow('avoidLeftObstacle')
elif avoidRight:
return nav.goNow('avoidRightObstacle')
else:
return nav.goLater(nav.lastDiffState)
def goToPosition(nav):
"""
Go to a position set in the navigator. General go to state. Goes
towards a location on the field stored in dest.
The location can be a RobotLocation, Location, RelRobotLocation, RelLocation
Absolute locations get transformed to relative locations based on current loc
For relative locations we use our bearing to that point as the heading
"""
relDest = helper.getRelativeDestination(nav.brain.loc, goToPosition.dest)
#if nav.counter % 10 is 0:
# print "going to " + str(relDest)
# print "ball is at {0}, {1}, {2} ".format(nav.brain.ball.loc.relX,
# nav.brain.ball.loc.relY,
# nav.brain.ball.loc.bearing)
goToPosition.speed = nav.velocity
if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
nav.velocity += copysign(Navigator.SPEED_CHANGE, (nav.requestVelocity - nav.velocity))
if goToPosition.pb:
# Calc dist to dest
dist = helper.getDistToDest(nav.brain.loc, goToPosition.dest)
if goToPosition.fast and dist < 140:
goToPosition.fast = False
goToPosition.dest = nav.brain.play.getPosition()
elif not goToPosition.fast and dist > 160:
goToPosition.fast = True
goToPosition.dest = nav.brain.play.getPositionCoord()
if goToPosition.fast:
# So that fast mode works for objects of type RobotLocation also
if isinstance(goToPosition.dest, RobotLocation) and not goToPosition.close:
fieldDest = RobotLocation(goToPosition.dest.x, goToPosition.dest.y, 0)
relDest = nav.brain.loc.relativeRobotLocationOf(fieldDest)
relDest.relH = nav.brain.loc.getRelativeBearing(fieldDest)
HEADING_ADAPT_CUTOFF = 103
DISTANCE_ADAPT_CUTOFF = 10
MAX_TURN = .5
BOOK_IT_TURN_THRESHOLD = 23
BOOK_IT_DISTANCE_THRESHOLD = 50
if relDest.relH >= HEADING_ADAPT_CUTOFF:
velH = MAX_TURN
elif relDest.relH <= -HEADING_ADAPT_CUTOFF:
velH = -MAX_TURN
else:
velH = helper.adaptSpeed(relDest.relH,
HEADING_ADAPT_CUTOFF,
MAX_TURN)
if relDest.relX >= DISTANCE_ADAPT_CUTOFF:
velX = goToPosition.speed
elif relDest.relX <= -DISTANCE_ADAPT_CUTOFF:
velX = -goToPosition.speed
else:
velX = helper.adaptSpeed(relDest.relX,
DISTANCE_ADAPT_CUTOFF,
goToPosition.speed)
if relDest.relY >= DISTANCE_ADAPT_CUTOFF:
velY = goToPosition.speed
elif relDest.relY <= -DISTANCE_ADAPT_CUTOFF:
velY = -goToPosition.speed
else:
velY = helper.adaptSpeed(relDest.relY,
DISTANCE_ADAPT_CUTOFF,
goToPosition.speed)
if fabs(relDest.dist) > BOOK_IT_DISTANCE_THRESHOLD:
goToPosition.close = False
if fabs(relDest.relH) > BOOK_IT_TURN_THRESHOLD:
if relDest.relH > 0: velH = MAX_TURN
if relDest.relH < 0: velH = -MAX_TURN
velX = 0
velY = 0
goToPosition.bookingIt = False
else:
velY = 0
goToPosition.bookingIt = True
else:
goToPosition.close = True
goToPosition.speeds = (velX, velY, velH)
helper.setSpeed(nav, goToPosition.speeds)
else:
if goToPosition.adaptive:
#reduce the speed if we're close to the target
speed = helper.adaptSpeed(relDest.dist,
constants.ADAPT_DISTANCE,
goToPosition.speed)
else:
speed = goToPosition.speed
helper.setDestination(nav, relDest, speed)
if navTrans.shouldDodge(nav):
return nav.goNow('dodge')
return Transition.getNextState(nav, goToPosition)
def goToPosition(nav):
"""
Go to a position set in the navigator. General go to state. Goes
towards a location on the field stored in dest.
The location can be a RobotLocation, Location, RelRobotLocation, RelLocation
Absolute locations get transformed to relative locations based on current loc
For relative locations we use our bearing to that point as the heading
"""
relDest = helper.getRelativeDestination(nav.brain.loc, goToPosition.dest)
#if nav.counter % 10 is 0:
# print "going to " + str(relDest)
# print "ball is at {0}, {1}, {2} ".format(nav.brain.ball.loc.relX,
# nav.brain.ball.loc.relY,
# nav.brain.ball.loc.bearing)
goToPosition.speed = nav.velocity
if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
nav.velocity += copysign(Navigator.SPEED_CHANGE,
(nav.requestVelocity - nav.velocity))
if goToPosition.pb:
# Calc dist to dest
dist = helper.getDistToDest(nav.brain.loc, goToPosition.dest)
if goToPosition.fast and dist < 140:
goToPosition.fast = False
goToPosition.dest = nav.brain.play.getPosition()
elif not goToPosition.fast and dist > 160:
goToPosition.fast = True
goToPosition.dest = nav.brain.play.getPositionCoord()
if goToPosition.fast:
# So that fast mode works for objects of type RobotLocation also
if isinstance(goToPosition.dest,
RobotLocation) and not goToPosition.close:
fieldDest = RobotLocation(goToPosition.dest.x, goToPosition.dest.y,
0)
relDest = nav.brain.loc.relativeRobotLocationOf(fieldDest)
relDest.relH = nav.brain.loc.getRelativeBearing(fieldDest)
HEADING_ADAPT_CUTOFF = 103
DISTANCE_ADAPT_CUTOFF = 10
MAX_TURN = .5
BOOK_IT_TURN_THRESHOLD = 23
BOOK_IT_DISTANCE_THRESHOLD = 50
if relDest.relH >= HEADING_ADAPT_CUTOFF:
velH = MAX_TURN
elif relDest.relH <= -HEADING_ADAPT_CUTOFF:
velH = -MAX_TURN
else:
velH = helper.adaptSpeed(relDest.relH, HEADING_ADAPT_CUTOFF,
MAX_TURN)
if relDest.relX >= DISTANCE_ADAPT_CUTOFF:
velX = goToPosition.speed
elif relDest.relX <= -DISTANCE_ADAPT_CUTOFF:
velX = -goToPosition.speed
else:
velX = helper.adaptSpeed(relDest.relX, DISTANCE_ADAPT_CUTOFF,
goToPosition.speed)
if relDest.relY >= DISTANCE_ADAPT_CUTOFF:
velY = goToPosition.speed
elif relDest.relY <= -DISTANCE_ADAPT_CUTOFF:
velY = -goToPosition.speed
else:
velY = helper.adaptSpeed(relDest.relY, DISTANCE_ADAPT_CUTOFF,
goToPosition.speed)
if fabs(relDest.dist) > BOOK_IT_DISTANCE_THRESHOLD:
goToPosition.close = False
if fabs(relDest.relH) > BOOK_IT_TURN_THRESHOLD:
if relDest.relH > 0: velH = MAX_TURN
if relDest.relH < 0: velH = -MAX_TURN
velX = 0
velY = 0
goToPosition.bookingIt = False
else:
velY = 0
goToPosition.bookingIt = True
else:
goToPosition.close = True
goToPosition.speeds = (velX, velY, velH)
helper.setSpeed(nav, goToPosition.speeds)
else:
if goToPosition.adaptive:
#reduce the speed if we're close to the target
speed = helper.adaptSpeed(relDest.dist, constants.ADAPT_DISTANCE,
goToPosition.speed)
else:
speed = goToPosition.speed
helper.setDestination(nav, relDest, speed)
if navTrans.shouldDodge(nav):
return nav.goNow('dodge')
return Transition.getNextState(nav, goToPosition)
