def update():
global lastSonarValues
leftSonar = Robot.sensorValues()['sonar'][0]
rightSonar = Robot.sensorValues()['sonar'][10]
lastSonarValues[0].append(leftSonar)
lastSonarValues[1].append(rightSonar)
if len(lastSonarValues[0]) > 10:
lastSonarValues[0].pop(0)
if len(lastSonarValues[1]) > 10:
lastSonarValues[1].pop(0)
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
