def getPriority(self): ball = self.behaviorManager.ballManager.getTarget() if ball==None: return -1 elif utilities.angleCenterZero(ball.angle - self.behaviorManager.lastArduinoInput.heading) > self.maxBallAngle: return -1 else: return utilities.approachBallPriority
def execute(self, previousBehavior): output = PilotCommands(self.behaviorManager.lastArduinoInput) bm = self.behaviorManager.ballManager if (previousBehavior != self): self.init(bm) if bm.getTarget() != None: self.ball = bm.getTarget() self.framesNoBall = 0 else: self.framesNoBall += 1 if self.framesNoBall > self.maxFramesNoBall: return # lost the ball desiredDeltaAngle = utilities.angleCenterZero(self.ball.angle - self.behaviorManager.lastArduinoInput.heading) output.desiredDeltaAngle = desiredDeltaAngle output.rollerCommand = RollerCommands.FORWARD output.winchCommand = WinchCommands.DOWN output.rampCommand = RampCommands.UP return output
def execute(self, previousBehavior):
ai = self.behaviorManager.lastArduinoInput
vi = self.behaviorManager.lastVisionInput
bs = self.behaviorManager.behaviorStack
bm = self.behaviorManager.ballManager
output = PilotCommands(ai)
output.rollerCommand = RollerCommands.FORWARD
output.winchCommand = WinchCommands.DOWN
output.rampCommand = RampCommands.UP
if previousBehavior != self:
self.init(bm.ballCount)
t = time.time()
if t-self.startTime > self.timeout:
return None # TODO ensure that we don't try to approach the ball again
if bm.ballCount > self.ballCount: # w00t, g0t b411
return None
ball = bm.getTarget()
if ball == None:
# lost the ball; hopefully this means that it is too close to us
bs.append(DriveStraightAcquireBall(self.behaviorManager, self.ballCount))
return None
if self.lastRunTime:
angle = utilities.angleCenterZero(ball.angle - ai.heading)
self.logger.debug("Approaching ball at {0} degrees, {1} cm".format(angle, ball.distance))
output.desiredDeltaAngle = angle
output.forwardSpeed = self.forwardSpeed
else:
output = None
self.lastRunTime = t
return output
