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