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