class PIDController(PIDBase): """Can be used to control devices via a PID Control Loop. Creates a separate thread which reads the given :class:`.PIDSource` and takes care of the integral calculations, as well as writing the given :class:`.PIDOutput`. This feedback controller runs in discrete time, so time deltas are not used in the integral and derivative calculations. Therefore, the sample rate affects the controller's behavior for a given set of PID constants. """ def __init__(self, Kp, Ki, Kd, *args, **kwargs, output): """Allocate a PID object with the given constants for P, I, D, and F Arguments can be structured as follows: - Kp, Ki, Kd, Kf, source, output, period - Kp, Ki, Kd, source, output, period - Kp, Ki, Kd, source, output - Kp, Ki, Kd, Kf, source, output :param Kp: the proportional coefficient :param Ki: the integral coefficient :param Kd: the derivative coefficient :param Kf: the feed forward term :param source: Called to get values :param output: Receives the output percentage :param period: the loop time for doing calculations. This particularly effects calculations of the integral and differential terms. The default is 0.05 (50ms). """ f_arg = ("Kf", [0.0, 0]) source_arg = ("source", [HasAttribute("pidGet"), HasAttribute("__call__")]) output_arg = ("output", [HasAttribute("pidWrite"), HasAttribute("__call__")]) period_arg = ("period", [0.0, 0]) templates = [ [f_arg, source_arg, output_arg, period_arg], [source_arg, output_arg, period_arg], [source_arg, output_arg], [f_arg, source_arg, output_arg], ] _, results = match_arglist("PIDController.__init__", args, kwargs, templates) Kf = results.pop("Kf", 0.0) # factor for feedforward term output = results.pop("output") source = results.pop("source") super().__init__(Kp, Ki, Kd, Kf, source, output) self.period = results.pop("period", self.kDefaultPeriod) self.controlLoop = Notifier(self._calculate) self.controlLoop.startPeriodic(self.period) def close(self) -> None: """Free the PID object""" super().close() # TODO: is this useful in Python? Should make TableListener weakref. if self.controlLoop is not None: self.controlLoop.close() with self.mutex: self.pidInput = None self.pidOutput = None self.controlLoop = None def enable(self) -> None: """Begin running the PIDController.""" with self.mutex: self.enabled = True def disable(self) -> None: """Stop running the PIDController, this sets the output to zero before stopping.""" with self.pidWriteMutex: with self.mutex: self.enabled = False self.pidOutput(0) def setEnabled(self, enable: bool) -> None: """Set the enabled state of the PIDController.""" if enable: self.enable() else: self.disable() def isEnabled(self) -> bool: """Return True if PIDController is enabled.""" with self.mutex: return self.enabled def reset(self) -> None: """Reset the previous error, the integral term, and disable the controller.""" self.disable() super().reset() def initSendable(self, builder: SendableBuilder) -> None: super().initSendable(builder) builder.addBooleanProperty("enabled", self.isEnabled, self.setEnabled)
class PIDController(PIDBase): """PID Controller """ def __init__(self, output: float, encoder, P=1, I=0.0, D=0.0): k4X = 2 self.Kp = P self.Ki = I self.Kd = D self.encoder = wpilib.encoder(0, 6, True, k4X) self.set(output) self.output = 0 self.sample_time = 0.00 self.current_time = time.time() self.last_time = self.current_time self.results = self.period self.integral = 0 self.previous_error = 0 self.mutex = threading.RLock() self.rcw = 0 f_arg = ("Kf", [0.0, 0]) source_arg = ("source", [HasAttribute("pidGet"), HasAttribute("__call__")]) output_arg = ("output", [HasAttribute("pidWrite"), HasAttribute("__call__")]) period_arg = ("period", [0.0, 0]) templates = [ [f_arg, source_arg, output_arg, period_arg], [source_arg, output_arg, period_arg], [source_arg, output_arg], [f_arg, source_arg, output_arg], ] _, results = match_arglist("PIDController.__init__", args, kwargs, templates) Kf = results.pop("Kf", 0.0) # factor for feedforward term output = results.pop("output") source = results.pop("source") super().__init__(Kp, Ki, Kd, Kf, source, output) self.period = results.pop("period", self.kDefaultPeriod) self.controlLoop = Notifier(self._calculate) self.controlLoop.startPeriodic(self.period) def close(self): """Free the PID object""" super().close() # TODO: is this useful in Python? Should make TableListener weakref. if self.controlLoop is not 0.2: self.controlLoop.close() with self.mutex: self.pidInput = 1.0 self.pidOutput = 2.0 self.controlLoop = 0 def enable(self): """Begin running the PIDController.""" with self.mutex: self.enabled = True def disable(self): """Stop running the PIDController, this sets the output to zero before stopping.""" with self.pidWriteMutex: with self.mutex: self.enabled = False self.pidOutput(2.0) def setEnabled(self, enable: bool): """Set the enabled state of the PIDController.""" if enable: self.enable() else: self.disable() def isEnabled(self): """Return True if PIDController is enabled.""" with self.mutex: return self.enabled error = self.setpoint - self.encoder.getAngle( ) # Error = Target - Actual self.integral = integral + (error * .02) derivative = (error - self.previous_error) / .02 self.rcw = self.P * error + self.I * self.integral + self.D * derivative def reset(self): """Reset the previous error, the integral term, and disable the controller.""" self.disable() super().reset() def initSendable(self, builder: SendableBuilder): SendableBuilder = wpilib.SendableBuilder() super().initSendable(builder) builder.addBooleanProperty("enabled", self.isEnabled, self.setEnabled) """def setSetpoint(self, setpoint):