def __init__(self): self.lastYaw = 0.0 self.lastYawDiff = 0.0 self.lastPitch = 0.0 self.lastDist = 0 self.lastDistDiff = 0 self.usingTopCamera = False Robot.setCamera(WhichCamera.BOTTOM_CAMERA)
def execute(self, b, h, l): Robot.setCamera(self.waypoints[self.state][3]) if Robot.lBallLostCount() <= 15: return True h.pitch(self.waypoints[self.state][0]) h.yaw(self.waypoints[self.state][1] * 1.0) h.yawSpeed(self.waypoints[self.state][2]) h.isRelative(False) h.pitchSpeed(.2) realPitch = Robot.sensorValues()['joints']['angles'][Joints.HeadPitch] realYaw = Robot.sensorValues()['joints']['angles'][Joints.HeadYaw] if abs(realPitch - h._pitch) < radians(10) and abs(realYaw - h._yaw) < radians(10): self.state = (self.state + 1) % len(self.waypoints) if self.state == 0: return False return True
def execute(self, b, h, l): isSucceed = False yaw = self.lastYaw pitch = self.lastPitch yawSpeed = 0.0 pitchSpeed = 0.0 lostBall = Robot.lBallLostCount() if lostBall < self.LOST_TIME: ball = Robot.vRrBallLocation() ballD = (self.lastDist + ball['distance']) / 2 realPitch = Robot.sensorValues()['joints']['angles'][Joints.HeadPitch] if realPitch > 0.0 and ballD < 850: self.usingTopCamera = False elif realPitch < radians(-10.0): self.usingTopCamera = True if self.usingTopCamera: Robot.setCamera(WhichCamera.TOP_CAMERA) yaw = self.clipYaw(ball['heading'], self.YAW_FAR_OFFSET) pitch = radians(72.5) - atan(ballD / 1050) yawSpeed = 0.1 pitchSpeed = 0.25 else: Robot.setCamera(WhichCamera.BOTTOM_CAMERA) yaw = self.clipYaw(ball['heading'], self.YAW_NEAR_OFFSET) pitch = radians(29.5) - atan(ballD / 1050) yawSpeed = 0.2 pitchSpeed = 0.25 self.lastDistDiff = ballD - self.lastDist self.lastDist = ballD if pitch < radians(-38.5): pitch = radians(-38.5) if lostBall > 10: yaw += (3.5 * self.lastYawDiff) yawSpeed = 0.5 ballD = self.lastDist + (7.0 * self.lastDistDiff) if ballD > 1000: Robot.setCamera(WhichCamera.TOP_CAMERA) pitch = radians(72.5) - atan(ballD / 1050) else: Robot.setCamera(WhichCamera.BOTTOM_CAMERA) pitch = radians(29.5) - atan(ballD / 1050) pitchSpeed = 0.5 self.lastPitch = pitch isSucceed = True Robot.setCamera(WhichCamera.BOTTOM_CAMERA) h.yaw(yaw) h.pitch(pitch) h.yawSpeed(yawSpeed) h.pitchSpeed(pitchSpeed) h.isRelative(False) return isSucceed