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