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)