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)