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()