def update(self):
current_time = time.time()
dt = current_time - self.last_time
updated = False
if (dt >= self.time_period):
self.speed = accelerationRestrictor(dt, self.last_speed,
self.desired_speed,
MAX_SPEED_ACCEL)
self.steering = accelerationRestrictor(dt, self.last_steering,
self.desired_steering,
MAX_STEERING_ACCEL)
left_power = powerDeadzoneHandler(self.speed - self.steering)
right_power = powerDeadzoneHandler(self.speed + self.steering)
self.LeftMotor.power = int(
mapToLimits(left_power, MAX_OUTPUT, MIN_OUTPUT))
self.RightMotor.power = int(
mapToLimits(right_power, MAX_OUTPUT, MIN_OUTPUT))
self.last_speed = self.speed
self.last_steering = self.steering
self.last_time = current_time
updated = True
return updated
def setGrab(self, grab):
self.position['grab'] = grab
self.servos[ARMS_SERVO_BOARD][LEFT_GRAB_PIN] = int(
mapToLimits((self.position['grab'] + LEFT_GRAB_OFFSET) *
LEFT_GRAB_DIRECTION))
self.servos[ARMS_SERVO_BOARD][RIGHT_GRAB_PIN] = int(
mapToLimits((self.position['grab'] + RIGHT_GRAB_OFFSET) *
RIGHT_GRAB_DIRECTION))
def setLift(self, lift):
self.position['lift'] = lift
self.servos[ARMS_SERVO_BOARD][LEFT_LIFT_PIN] = int(
mapToLimits((self.position['lift'] + LEFT_LIFT_OFFSET) *
LEFT_LIFT_DIRECTION))
self.servos[ARMS_SERVO_BOARD][RIGHT_LIFT_PIN] = int(
mapToLimits((self.position['lift'] + RIGHT_LIFT_OFFSET) *
RIGHT_LIFT_DIRECTION))
def setRotate(self, rotate):
self.position['rotate'] = rotate
self.servos[ARMS_SERVO_BOARD][LEFT_ROTATE_PIN] = int(
mapToLimits((self.position['rotate'] + LEFT_ROTATE_OFFSET) *
LEFT_ROTATE_DIRECTION))
self.servos[ARMS_SERVO_BOARD][RIGHT_ROTATE_PIN] = int(
mapToLimits((self.position['rotate'] + RIGHT_ROTATE_OFFSET) *
RIGHT_ROTATE_DIRECTION))
def run(self, value):
if (self.stopped == True):
self.output = 0
output_calculated = True
else: #self.stopped == False
current_time = time.time()
dt = current_time - self.last_time
output_calculated = False
if (self.first_run == False):
if (dt >= self.time_period):
error = self.setpoint - value
self.p = self.kp * error
self.i += self.ki * error * dt
self.d = (error - self.last_error) / dt
if (self.i_limit != 0):
self.i = mapToLimits(self.i, self.i_limit,
-self.i_limit)
if ((self.max_output != 0) or (self.min_output != 0)):
self.output = mapToLimits((self.p + self.i + self.d),
self.max_output,
self.min_output)
else:
self.output = self.p + self.i + self.d
output_calculated = True
self.last_time = current_time
self.last_error = error
else: #self.first_run == True
error = value - self.setpoint
self.p = self.kp * error
self.i = self.starting_i
self.d = 0
if ((self.max_output != 0) or (self.min_output != 0)):
self.output = mapToLimits((self.p + self.i + self.d),
self.max_output, self.min_output)
else:
self.output = self.p + self.i + self.d
output_calculated = True
self.last_time = current_time
self.last_error = error
self.first_run = False
return output_calculated
def debugYawDrift():
if (DEBUG_YAW_DRIFT == True):
print "debugging yaw drift"
MotionThread.setAction(STILL)
wake_up_time = time.time()
while (True):
wake_up_time += DEBUG_TIMEPERIOD
print str(MotionThread.yaw)
sleep_time = mapToLimits(wake_up_time - time.time(),
DEBUG_TIMEPERIOD, 0)
time.sleep(DEBUG_TIMEPERIOD)
def getOutputFromAngle(self, new_camera_angle):
appropriate_lower_measurement_index = 0
appropriate_upper_measurement_index = len(CAMERA_MEASUREMENTS) - 1
lower_measurement_index = 0
upper_measurement_index = 1
while (upper_measurement_index < len(CAMERA_MEASUREMENTS)):
if (new_camera_angle <=
CAMERA_MEASUREMENTS[upper_measurement_index][0]
and new_camera_angle >=
CAMERA_MEASUREMENTS[lower_measurement_index][0]):
appropriate_lower_measurement_index = lower_measurement_index
appropriate_upper_measurement_index = upper_measurement_index
lower_measurement_index += 1
upper_measurement_index += 1
# print str(appropriate_lower_measurement_index)
# print str(appropriate_upper_measurement_index)
lower_measurement_angle = CAMERA_MEASUREMENTS[
appropriate_lower_measurement_index][0]
upper_measurement_angle = CAMERA_MEASUREMENTS[
appropriate_upper_measurement_index][0]
lower_measurement_output = CAMERA_MEASUREMENTS[
appropriate_lower_measurement_index][1]
upper_measurement_output = CAMERA_MEASUREMENTS[
appropriate_upper_measurement_index][1]
# print "lower_measurement_angle " + str(lower_measurement_angle)
# print "upper_measurement_angle " + str(upper_measurement_angle)
# print "lower_measurement_output " + str(lower_measurement_output)
# print "upper_measurement_output " + str(upper_measurement_output)
gradient = (upper_measurement_output - lower_measurement_output) / (
upper_measurement_angle - lower_measurement_angle)
d_angle = (new_camera_angle - lower_measurement_angle)
output = lower_measurement_output + (gradient * d_angle)
# print "gradient " + str(gradient)
# print "d_angle " + str(d_angle)
# print "output " + str(output)
# output = (((new_camera_angle - lower_measurement_angle) / (upper_measurement_angle - lower_measurement_angle)) * (upper_measurement_output - lower_measurement_output)) + lower_measurement_output
output = int(mapToLimits(output, MAX_CAMERA_OUTPUT, MIN_CAMERA_OUTPUT))
return output
def moveCameraServo(self, new_camera_angle):
# print "new_camera_angle = " + str(new_camera_angle)
new_camera_angle = mapToLimits(new_camera_angle, MAX_CAMERA_ANGLE,
MIN_CAMERA_ANGLE)
#new_output = int(mapToLimits(((((new_camera_angle - MIN_CAMERA_ANGLE) / (MAX_CAMERA_ANGLE - MIN_CAMERA_ANGLE)) * (MAX_CAMERA_OUTPUT - MIN_CAMERA_OUTPUT)) + MIN_CAMERA_OUTPUT), MAX_CAMERA_OUTPUT, MIN_CAMERA_OUTPUT))
# print "new_camera_angle = " + str(new_camera_angle)
new_output = self.getOutputFromAngle(new_camera_angle)
# print "new_output = " + str(new_output)
change_in_output = abs(new_output - self.last_output)
self.last_output = new_output
# print "new_output = " + str(new_output)
self.ruggeduino.setCameraServoAngle(new_output)
self.camera_angle = new_camera_angle
return change_in_output
def run(self):
print "Starting " + self.name
wake_up_time = time.time() + self.time_period
while (True):
self.time_to_sleep = mapToLimits(wake_up_time - time.time(),
self.time_period, 0)
wake_up_time += self.time_period
time.sleep(self.time_to_sleep)
self.updateSensors()
self.updateRobotLocation()
self.processActionBundleStack()
new_steering = self.runYawPid()
new_speed = self.runDistancePid()
self.updateMotors(new_steering, new_speed)
print "Exiting " + self.name
def setDesiredSpeed(self, desired_speed):
self.desired_speed = mapToLimits(desired_speed, MAX_SPEED, MIN_SPEED)
return self.update()
def accelerationRestrictor(dt, last_value, desired_value, max_accel):
max_change = dt * max_accel
value_upper_limit = last_value + max_change
value_lower_limit = last_value - max_change
output = mapToLimits(desired_value, value_upper_limit, value_lower_limit)
return output
def setDesiredSpeedAndSteering(self, desired_speed, desired_steering):
self.desired_speed = mapToLimits(desired_speed, MAX_SPEED, MIN_SPEED)
self.desired_steering = mapToLimits(desired_steering, MAX_STEERING,
MIN_STEERING)
return self.update()
def setDesiredSteering(self, desired_steering):
self.desired_steering = mapToLimits(desired_steering, MAX_STEERING,
MIN_STEERING)
return self.update()