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