def updateDistance(self):
if (self.E.update() == True):
with self.location_update_lock:
self.distance = self.E.distance
else: #self.E.update() == False
print "ERROR: E returned false in Motion Thread"
noise.signalBad(self.power)
def updateYaw(self):
if (self.D.updateAll() == True):
with self.location_update_lock:
self.yaw = self.D.yaw_without_drift - self.mpu_yaw_offset
else: #self.D.update() == False
print "ERROR: D returned false in Motion Thread"
noise.signalBad(self.power)
def updateMotors(self, new_steering, new_speed):
if (new_steering == True or new_speed == True):
if (self.M.setDesiredSpeedAndSteering(self.speed,
self.steering) == False):
print "ERROR: speed and steering not set in MotorHandler"
noise.signalBad(self.power)
else:
print "ERROR: speed and steering not set in Motion Thread"
noise.signalBad(self.power)
def runDistancePid(self):
new_speed = False
distance_pid_input = self.distance
if (self.S.run(distance_pid_input) == True):
self.speed = self.S.output
new_speed = True
else:
print "ERROR: S returned false in Motion Thread"
noise.signalBad(self.power)
return new_speed
def runYawPid(self):
new_steering = False
yaw_pid_input = angleMod(self.desired_yaw - self.yaw)
if (self.Y.run(yaw_pid_input) == True):
self.steering = self.Y.output
new_steering = True
else:
print "ERROR: Y returned false in Motion Thread"
noise.signalBad(self.power)
return new_steering
def calibrationCheck(self):
def check():
yaws = ()
distances = ()
for x in range(0, SAMPLE_SIZE):
self.updateYaw()
self.updateDistance()
yaws += (self.yaw, )
distances += (self.distance, )
time.sleep(SAMPLE_TIMEPERIOD)
yaw_range = abs(angleMod(max(yaws) - min(yaws)))
distance_range = max(distances) - min(distances)
passed = True
if (yaw_range < MAX_YAW_SAMPLE_RANGE):
print "MPU calibration test passed with yaw_range = " + str(
yaw_range)
else: #range >= MAX_YAW_SAMPLE_RANGE
print "MPU calibration test failed with yaw_range = " + str(
yaw_range) + ", yaws = " + str(yaws)
passed = False
if (distance_range == 0):
print "encoder calibration test passed"
else: #distance_range != 0
print "encoder calibration test failed with distance_range = " + str(
distance_range) + ", distances = " + str(distances)
passed = False
return passed
print "Running MPU and encoder calibration check..."
attempts = 1
while (check() == False):
print "Test failed, trying again attempts = " + str(attempts)
noise.signalBad(self.power)
attempts += 1
print "Test passed with attempts = " + str(attempts)
noise.signalGood(self.power)
def processActionBundleStack(self):
with self.action_lock:
while (len(self.action_bundle_stack) != 0):
action_bundle = self.action_bundle_stack.pop(0)
action = action_bundle[0]
action_value_1 = action_bundle[1]
action_value_2 = action_bundle[2]
if (action == STILL):
self.Y.stop()
self.S.stop()
print "New Action: STILL"
noise.signalAction(self.power)
elif (action == TURN):
self.Y.restart()
self.desired_yaw = self.yaw + action_value_1
self.S.stop()
print "New Action: TURN, with value = " + str(
action_value_1)
noise.signalAction(self.power)
elif (action == TURN_CHANGE):
self.desired_yaw = self.yaw + action_value_1
#print "New Action: TURN_CHANGE, with value = " + str(action_value_1)
elif (action == TURN_TO):
self.Y.restart()
self.desired_yaw = action_value_1
self.S.stop()
print "New Action: TURN_TO, with value = " + str(
action_value_1)
noise.signalAction(self.power)
elif (action == TURN_TO_CHANGE):
self.desired_yaw = action_value_1
#print "New Action: TURN_TO_CHANGE, with value = " + str(action_value_1)
elif (action == MOVE):
self.Y.stop()
self.S.restart()
self.desired_distance = self.distance + action_value_1
self.S.setSetpoint(self.desired_distance)
print "New Action: MOVE, with value = " + str(
action_value_1)
noise.signalAction(self.power)
elif (action == MOVE_CHANGE):
self.desired_distance = self.distance + action_value_1
self.S.setSetpoint(self.desired_distance)
#print "New Action: MOVE_CHANGE, with value = " + str(action_value_1)
elif (action == MOVE_TO):
self.Y.stop()
self.S.restart()
self.desired_distance = action_value_1
self.S.setSetpoint(self.desired_distance)
print "New Action: MOVE_TO, with value = " + str(
action_value_1)
noise.signalAction(self.power)
elif (action == MOVE_TO_CHANGE):
self.desired_distance = action_value_1
self.S.setSetpoint(self.desired_distance)
#print "New Action: MOVE_TO_CHANGE, with value = " + str(action_value_1)
elif (action == MOVE_HOLD):
self.Y.restart()
self.desired_yaw = self.yaw
self.S.restart()
self.desired_distance = self.distance + action_value_1
self.S.setSetpoint(self.desired_distance)
print "New Action: MOVE_HOLD, with value = " + str(
action_value_1)
noise.signalAction(self.power)
elif (action == MOVE_TO_HOLD):
self.Y.restart()
self.desired_yaw = self.yaw
self.S.restart()
self.desired_distance = action_value_1
self.S.setSetpoint(self.desired_distance)
print "New Action: MOVE_TO_HOLD, with value = " + str(
action_value_1)
noise.signalAction(self.power)
elif (action == MOVE_AND_TURN_TO):
self.Y.restart()
self.desired_yaw = action_value_2
self.S.restart()
self.desired_distance = self.distance + action_value_1
self.S.setSetpoint(self.desired_distance)
print "New Action: MOVE_AND_TURN_TO, with value_1 = " + str(
action_value_1) + ", value_2 = " + str(action_value_2)
noise.signalAction(self.power)
elif (action == MOVE_AND_TURN_TO_CHANGE):
self.desired_yaw = action_value_2
self.desired_distance = self.distance + action_value_1
self.S.setSetpoint(self.desired_distance)
#print "New Action: MOVE_AND_TURN_TO_CHANGE, with value_1 = " + str(action_value_1) + ", value_2 = " + str(action_value_2)
else:
print "ERROR: unknown action processed in motionThread.processAction"
noise.signalBad(self.power)