class DriveController:
wheel_radius = 0.1 # meters
encoder_ticks_per_revolution = 256 * 3 * 1.8
def __init__(self, kP, kI, kD, kF, tolerance, encoder_function,
update_function, direction, period, is_enabled):
self.is_enabled = is_enabled
self.direction = direction
self.encoder_function = encoder_function
self.motor_output = 0
self.pid_controller = PIDController(kP,
kI,
kD,
kF,
source=self,
output=self,
period=period)
#self.pid_controller.controlLoop._thread.setDaemon(True)
self.pid_controller.setInputRange(-5, 5)
self.pid_controller.setOutputRange(-1.0, 1.0)
self.pid_controller.setAbsoluteTolerance(tolerance)
self.update_function = update_function
self.pid_controller.setContinuous(True)
def start(self, setpoint):
self.setpoint = setpoint
self.pid_controller.setSetpoint(setpoint)
self.pid_controller.enable()
def pidWrite(self, output):
print("output voltage", output)
# print("distance for cotroller", self.pid_controller.getError())
self.pid_controller.setSetpoint(self.setpoint)
self.update_function(self.direction * output)
def is_finished(self):
return self.pid_controller.onTarget()
def close(self):
self.pid_controller.close()
def pidGet(self):
distance_in_encoder = self.encoder_function()
distance_in_meters = (
distance_in_encoder /
self.encoder_ticks_per_revolution) * self.wheel_radius
print("dist", distance_in_meters)
return distance_in_meters
def getPIDSourceType(self):
return "meter"
def getPIDOutputType(self):
return "output"
class DriveController(StateMachine):
chassis: Chassis
wheel_diameter = 0.1 # wheel diameter in meters
encoder_ticks_per_revolution = 256 * 1.8 * 3 * 5 # encoder ticks per rev
distance_setpoint = 0 # Distance setpoint
angle_setpoint = 0 # Angle setpoint
left_speed = 0 # Current left speed
right_speed = 0 # Current right speed
turn_speed = 0 # Current turn speed
loop_counter = 0 # A counter that represents the number of times the drive function has been called.
finished = False # Is finished?
enabled = False # Is enable?
def setup(self):
self.motor_updater = Notifier(
self.update_motors) # A motor updater thread
self.setup_values()
self.start()
def setup_values(self,
input_range=15,
output_range=1,
tolerance=0.05,
period=0.02,
kp=0.08,
ki=0,
kd=0,
kf=0):
""" Sets up all the different values that will be used during the course of the PID control loop. """
self.input_range = input_range
self.output_range = output_range
self.tolerance = tolerance
self.period = period
self.kp = kp
self.ki = ki
self.kd = kd
self.kf = kf
def start(self):
# Setting up the left controller
self.left_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_left_encoder,
self.left_output)
self.left_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.left_holder,
output=self.left_holder,
period=self.period)
self.left_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.left_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.left_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.left_pid_contorller.setContinuous(True)
# Setting up the right controller
self.right_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_right_encoder,
self.right_output)
self.right_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.right_holder,
output=self.right_holder,
period=self.period)
self.right_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.right_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.right_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.right_pid_contorller.setContinuous(True)
# Setting up the turn controller
self.turn_holder = IOGyroHolder(self.chassis.get_angle,
self.turn_output)
self.turn_pid_contorller = PIDController(0.02,
self.ki,
self.kd,
self.kf,
source=self.turn_holder,
output=self.turn_holder,
period=self.period)
self.turn_pid_contorller.setInputRange(0, 360)
self.turn_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.turn_pid_contorller.setAbsoluteTolerance(10)
self.turn_pid_contorller.setContinuous(True)
# put different pidf values for angle control
def enable(self, distance: float, angle: float):
self.distance_setpoint = distance
self.angle_setpoint = angle
# Setting the setpoints
self.left_pid_contorller.setSetpoint(distance)
self.right_pid_contorller.setSetpoint(distance)
self.turn_pid_contorller.setSetpoint(angle)
# Enabling the PID Controllers
self.left_pid_contorller.enable()
self.right_pid_contorller.enable()
self.turn_pid_contorller.enable()
self.motor_updater.startPeriodic(self.period)
self.enabled = True
def left_output(self, speed):
self.left_speed = speed
def right_output(self, speed):
self.right_speed = speed
def turn_output(self, linear_speed):
self.turn_speed = linear_speed
def update_motors(self):
# print("distance for cotroller", self.left_pid_contorller.getError(), self.right_pid_contorller.getError(),
# self.turn_pid_contorller.getError())
self.left_pid_contorller.setSetpoint(self.distance_setpoint)
self.right_pid_contorller.setSetpoint(self.distance_setpoint)
self.turn_pid_contorller.setSetpoint(self.angle_setpoint)
self.chassis.set_motors_values(self.left_speed - self.turn_speed,
self.right_speed + self.turn_speed)
def set_input_range(self, range):
self.input_range = range
def set_tolerance(self, tolerance):
self.tolerance = tolerance
def set_kp(self, kp):
self.kp = kp
def set_ki(self, ki):
self.ki = ki
def set_kd(self, kd):
self.kd = kd
def set_kf(self, kf):
self.kf = kf
def set_period(self, period):
self.period = period
def restart(self):
self.chassis.reset_encoders()
self.chassis.reset_navx()
self.left_speed = 0
self.right_speed = 0
self.turn_speed = 0
def is_finished(self):
return self.left_pid_contorller.onTarget(
) and self.right_pid_contorller.onTarget(
) and self.turn_pid_contorller.onTarget()
def close(self):
self.left_speed = 0
self.right_speed = 0
self.turn_speed = 0
self.motor_updater.close()
self.left_pid_contorller.close()
self.right_pid_contorller.close()
self.turn_pid_contorller.close()
@state(first=True)
def drive(self, initial_call):
print(self.loop_counter)
self.loop_counter += 1
if initial_call:
self.restart()
self.enable(1, 90)
elif self.is_finished():
self.close()
self.finished = True
self.enabled = False
self.done()
