PKN:k;33wpilib_controller.py"""A backport of the upcoming (in 2020) WPILib PIDController.""" __version__ = "0.5.2" import enum import math import threading from typing import Any, Callable, ClassVar, Optional import wpilib __any__ = ("PIDController", "PIDControllerRunner") class PIDControllerRunner(wpilib.SendableBase): def __init__( self, controller: "PIDController", measurement_source: Callable[[], float], controller_output: Callable[[float], Any], ) -> None: """ Allocates a PIDControllerRunner. :param measurement_source: The function that supplies the current process variable measurement. :param controller_output: The function which updates the plant using the controller output passed as the argument. """ super().__init__() self._enabled = False self.controller = controller self.controller_output = controller_output self.measurement_source = measurement_source self._this_mutex = threading.RLock() # Ensures when disable() is called, controller_output() won't # run if controller.update() is already running at that time. self._output_mutex = threading.RLock() self.notifier = wpilib.Notifier(self._run) self.notifier.startPeriodic(controller.period) def enable(self): """Begin running the controller.""" with self._this_mutex: self._enabled = True def disable(self): """Stop running the controller. This sets the output to zero before stopping. """ # Ensures self._enabled modification and self.controller_output() # call occur atomically with self._output_mutex: with self._this_mutex: self._enabled = False self.controller_output(0) def isEnabled(self): """Returns whether controller is running.""" with self._this_mutex: return self._enabled def _run(self): # Ensures self._enabled check and self.controller_output() call occur atomically with self._output_mutex: with self._this_mutex: enabled = self._enabled if enabled: self.controller_output( self.controller.calculate(self.measurement_source()) ) def initSendable(self, builder) -> None: self.controller.initSendable(builder) builder.setSafeState(self.disable) builder.addBooleanProperty( "enabled", self.isEnabled, lambda enabled: self.enable() if enabled else self.disable(), ) class PIDController(wpilib.SendableBase): """Class implements a PID Control Loop.""" instances: ClassVar[int] = 0 #: Factor for "proportional" control Kp: float #: Factor for "integral" control Ki: float #: Factor for "derivative" control Kd: float #: The period (in seconds) of the loop that calls the controller period: float maximum_output: float = 1 minimum_output: float = -1 #: Maximum input - limit setpoint to this _maximum_input: float = 0 #: Minimum input - limit setpoint to this _minimum_input: float = 0 #: input range - difference between maximum and minimum _input_range: float = 0 #: Do the endpoints wrap around? eg. Absolute encoder continuous: bool = False #: The error at the time of the most recent call to calculate() curr_error: float = 0 #: The error at the time of the second-most-recent call to calculate() (used to compute velocity) prev_error: float = math.inf #: The sum of the errors for use in the integral calc total_error: float = 0 class Tolerance(enum.Enum): Absolute = enum.auto() Percent = enum.auto() _tolerance_type: Tolerance = Tolerance.Absolute #: The percentage or absolute error that is considered at setpoint. _tolerance: float = 0.05 _delta_tolerance: float = math.inf setpoint: float = 0 output: float = 0 def __init__( self, Kp: float, Ki: float, Kd: float, *, period: float = 0.02 ) -> None: """Allocate a PID object with the given constants for Kp, Ki, and Kd. :param Kp: The proportional coefficient. :param Ki: The integral coefficient. :param Kd: The derivative coefficient. :param period: The period between controller updates in seconds. The default is 20ms. """ super().__init__(addLiveWindow=False) self._this_mutex = threading.RLock() self.period = period self.Kp = Kp self.Ki = Ki self.Kd = Kd PIDController.instances += 1 self.setName("PIDController", PIDController.instances) def setPID(self, Kp: float, Ki: float, Kd: float) -> None: """Set the PID Controller gain parameters.""" with self._this_mutex: self.Kp = Kp self.Ki = Ki self.Kd = Kd def setP(self, Kp: float) -> None: """Set the Proportional coefficient of the PID controller gain.""" with self._this_mutex: self.Kp = Kp def setI(self, Ki: float) -> None: """Set the Integral coefficient of the PID controller gain.""" with self._this_mutex: self.Ki = Ki def setD(self, Kd: float) -> None: """Set the Differential coefficient of the PID controller gain.""" with self._this_mutex: self.Kd = Kd def setSetpoint(self, setpoint: float) -> None: """Set the setpoint for the PIDController.""" with self._this_mutex: if self._maximum_input > self._minimum_input: self.setpoint = self._clamp( setpoint, self._minimum_input, self._maximum_input ) else: self.setpoint = setpoint def atSetpoint( self, tolerance: Optional[float] = None, delta_tolerance: float = math.inf, tolerance_type: Tolerance = Tolerance.Absolute, ) -> bool: """ Return true if the error is within the percentage of the specified tolerances. This will return false until at least one input value has been computed. If no arguments are given, defaults to the tolerances set by :meth:`setAbsoluteTolerance` or :meth:`setPercentTolerance`. :param tolerance: The maximum allowable error. :param delta_tolerance: The maximum allowable change in error, if tolerance is specified. :param tolerance_type: The type of tolerances specified. """ if tolerance is None: tolerance = self._tolerance delta_tolerance = self._delta_tolerance tolerance_type = self._tolerance_type error = self.getError() with self._this_mutex: delta_error = (error - self.prev_error) / self.period if tolerance_type is self.Tolerance.Percent: input_range = self._input_range return ( abs(error) < tolerance / 100 * input_range and abs(delta_error) < delta_tolerance / 100 * input_range ) else: return abs(error) < tolerance and abs(delta_error) < delta_tolerance def setContinuous(self, continuous: bool = True) -> None: """Set the PID controller to consider the input to be continuous. Rather than using the max and min input range as constraints, it considers them to be the same point and automatically calculates the shortest route to the setpoint. :param continuous: True turns on continuous, False turns off continuous """ with self._this_mutex: self.continuous = continuous def setInputRange(self, minimum_input: float, maximum_input: float) -> None: """Sets the maximum and minimum values expected from the input. :param minimum_input: The minimum value expected from the input. :param maximum_input: The maximum value expected from the input. """ with self._this_mutex: self._minimum_input = minimum_input self._maximum_input = maximum_input self._input_range = maximum_input - minimum_input # Clamp setpoint to new input if maximum_input > minimum_input: self.setpoint = self._clamp(self.setpoint, minimum_input, maximum_input) def setOutputRange(self, minimum_output: float, maximum_output: float) -> None: """Sets the minimum and maximum values to write. :param minimum_output: the minimum value to write to the output :param maximum_output: the maximum value to write to the output """ with self._this_mutex: self.minimum_output = minimum_output self.maximum_output = maximum_output def setAbsoluteTolerance( self, tolerance: float, delta_tolerance: float = math.inf ) -> None: """ Set the absolute error which is considered tolerable for use with atSetpoint(). :param tolerance: Absolute error which is tolerable. :param delta_tolerance: Change in absolute error per second which is tolerable. """ with self._this_mutex: self._tolerance_type = self.Tolerance.Absolute self._tolerance = tolerance self._delta_tolerance = delta_tolerance def setPercentTolerance( self, tolerance: float, delta_tolerance: float = math.inf ) -> None: """ Set the percent error which is considered tolerable for use with atSetpoint(). :param tolerance: Percent error which is tolerable. :param delta_tolerance: Change in percent error per second which is tolerable. """ with self._this_mutex: self._tolerance_type = self.Tolerance.Percent self._tolerance = tolerance self._delta_tolerance = delta_tolerance def getError(self) -> float: """Returns the difference between the setpoint and the measurement.""" with self._this_mutex: return self.getContinuousError(self.curr_error) def getDeltaError(self) -> float: """Returns the change in error per second.""" error = self.getError() with self._this_mutex: return (error - self.prev_error) / self.period def calculate(self, measurement: float, setpoint: Optional[float] = None) -> float: """ Returns the next output of the PID controller. :param measurement: The current measurement of the process variable. :param setpoint: The new setpoint of the controller if specified. """ with self._this_mutex: if setpoint is not None: self.setSetpoint(setpoint) Ki = self.Ki minimum_output = self.minimum_output maximum_output = self.maximum_output prev_error = self.prev_error = self.curr_error error = self.curr_error = self.getContinuousError( self.setpoint - measurement ) total_error = self.total_error period = self.period if Ki: total_error = self.total_error = self._clamp( total_error + error * period, minimum_output / Ki, maximum_output / Ki, ) output = self.output = self._clamp( self.Kp * error + Ki * total_error + self.Kd * (error - prev_error) / period, minimum_output, maximum_output, ) return output def reset(self) -> None: """Reset the previous error, the integral term, and disable the controller.""" with self._this_mutex: self.prev_error = 0 self.total_error = 0 self.output = 0 def initSendable(self, builder) -> None: builder.setSmartDashboardType("PIDController") builder.setSafeState(self.reset) builder.addDoubleProperty("p", lambda: self.Kp, self.setP) builder.addDoubleProperty("i", lambda: self.Ki, self.setI) builder.addDoubleProperty("d", lambda: self.Kd, self.setD) builder.addDoubleProperty("setpoint", lambda: self.setpoint, self.setSetpoint) def getContinuousError(self, error: float) -> float: """Wraps error around for continuous inputs. The original error is returned if continuous mode is disabled. This is an unsynchronized function. :param error: The current error of the PID controller. :return: Error for continuous inputs. """ input_range = self._input_range if self.continuous and input_range > 0: error %= input_range if error > input_range / 2: return error - input_range return error @staticmethod def _clamp(value: float, low: float, high: float) -> float: return max(low, min(value, high)) PKyM\-wpilib_controller-0.5.2.dist-info/LICENSE.txtCopyright (c) 2009-2018 FIRST All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the FIRST nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. PK!HPO'wpilib_controller-0.5.2.dist-info/WHEEL HM K-*ϳR03rOK-J,/RH,szd&Y)r$[)T&UrPK!HfA *wpilib_controller-0.5.2.dist-info/METADATAVn6S!7Nf͚ 34Yt(cD$eWl/'GJI&P 0`<;){!N=l7y-*ӪVޚd '^2mJvL4Mm'3'`jTJ4PF{m|J|r1K~7(`o}ae.ǒ[TQ, +c}0ԃ$_;#Tޘ JrL2@ Қzº"Mre٥-]nU푝4ڧm <CFNsj8% KS/|h<}J鰬bCv %K:lbsP=M.{}tBum 3J;mӅQu05UWB JTŀ{5l7.Mp3͑'|x -k}|o4ILBrPW(CQh*_ZvЌmы` gN<p_+S4%-#+:d#irQB:=܄k“Sk2ܚ*5|4ڝaY9/wReI} !_CFyRm<a%I*{WkB_*#I˝E@g-XkZBjJV F@-$7czC$y.[& !8dphW6:>},A)vnYL5+)J2{>~Z>N'ooYzp''ԸR @%˲7t@ߍGV 'vc-5A5NjU6B<2ZA"4w1%&62 X-ݧܐ]l@ǥ2K7N)A>|3T5{?&i<6n#:J%A}.r4 m}+%C4y ^sITu1u$j~ LU}6F6BEkMGT>,aE0^cllhCGsY ]9V0k`D4nє}m6+C2e9&+QA{p, DbB#}%A‘;RGaTƛhr]oߕT|TX8dd*V* >T΢rkݎ ~ŬvFXי|;A08܌YPC7#m^/Ø告2&l^q8Ta} -S!m؝:AļkuFS@1B0um\7M 6 ;w4lccEoBٯ {)J95_PK!HG(wpilib_controller-0.5.2.dist-info/RECORD9r0@g$HA@26!XلUӧrofEۈk6U4m4G[;Fօ|j6'>-F*&ʚ ě}MWo,=kIVD!RGekP5%> d*(6|$_nl>QߐrzM]1{㞅֦wb3PKN:k;33wpilib_controller.pyPKyM\-3wpilib_controller-0.5.2.dist-info/LICENSE.txtPK!HPO'9wpilib_controller-0.5.2.dist-info/WHEELPK!HfA *:wpilib_controller-0.5.2.dist-info/METADATAPK!HG(M@wpilib_controller-0.5.2.dist-info/RECORDPKA