Class PIDBase

  • All Implemented Interfaces:
    PIDInterface, PIDOutput, Sendable, AutoCloseable
    Direct Known Subclasses:
    PIDController

    public class PIDBase
    extends SendableBase
    implements PIDInterface, PIDOutput
    Class implements a PID Control Loop.

    Creates a separate thread which reads the given PIDSource and takes care of the integral calculations, as well as writing the given 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.

    • Constructor Summary

      Constructors 
      Constructor Description
      PIDBase​(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output)
      Allocate a PID object with the given constants for P, I, D, and F.
      PIDBase​(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output)
      Allocate a PID object with the given constants for P, I, and D.
    • Method Summary

      All Methods Instance Methods Concrete Methods Deprecated Methods 
      Modifier and Type Method Description
      protected void calculate()
      Read the input, calculate the output accordingly, and write to the output.
      protected double calculateFeedForward()
      Calculate the feed forward term.
      double get()
      Return the current PID result This is always centered on zero and constrained the the max and min outs.
      double getAvgError()
      Deprecated.
      Use getError(), which is now already filtered.
      protected double getContinuousError​(double error)
      Wraps error around for continuous inputs.
      double getD()
      Get the Differential coefficient.
      double getDeltaSetpoint()
      Returns the change in setpoint over time of the PIDController.
      double getError()
      Returns the current difference of the input from the setpoint.
      double getF()
      Get the Feed forward coefficient.
      double getI()
      Get the Integral coefficient.
      double getP()
      Get the Proportional coefficient.
      PIDSourceType getPIDSourceType()
      Returns the type of input the PID controller is using.
      double getSetpoint()
      Returns the current setpoint of the PIDController.
      void initSendable​(SendableBuilder builder)
      Initializes this Sendable object.
      boolean onTarget()
      Return true if the error is within the percentage of the total input range, determined by setTolerance.
      void pidWrite​(double output)
      Passes the output directly to setSetpoint().
      void reset()
      Reset the previous error, the integral term, and disable the controller.
      void setAbsoluteTolerance​(double absvalue)
      Set the absolute error which is considered tolerable for use with OnTarget.
      void setContinuous()
      Set the PID controller to consider the input to be continuous, Rather then 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.
      void setContinuous​(boolean continuous)
      Set the PID controller to consider the input to be continuous, Rather then 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.
      void setD​(double d)
      Set the Differential coefficient of the PID controller gain.
      void setF​(double f)
      Set the Feed forward coefficient of the PID controller gain.
      void setI​(double i)
      Set the Integral coefficient of the PID controller gain.
      void setInputRange​(double minimumInput, double maximumInput)
      Sets the maximum and minimum values expected from the input and setpoint.
      void setOutputRange​(double minimumOutput, double maximumOutput)
      Sets the minimum and maximum values to write.
      void setP​(double p)
      Set the Proportional coefficient of the PID controller gain.
      void setPercentTolerance​(double percentage)
      Set the percentage error which is considered tolerable for use with OnTarget.
      void setPID​(double p, double i, double d)
      Set the PID Controller gain parameters.
      void setPID​(double p, double i, double d, double f)
      Set the PID Controller gain parameters.
      void setPIDSourceType​(PIDSourceType pidSource)
      Sets what type of input the PID controller will use.
      void setSetpoint​(double setpoint)
      Set the setpoint for the PIDController.
      void setTolerance​(PIDBase.Tolerance tolerance)
      Deprecated.
      Use setPercentTolerance() instead.
      void setToleranceBuffer​(int bufLength)
      Deprecated.
      Use a LinearDigitalFilter as the input.
    • Constructor Detail

      • PIDBase

        public PIDBase​(double Kp,
                       double Ki,
                       double Kd,
                       double Kf,
                       PIDSource source,
                       PIDOutput output)
        Allocate a PID object with the given constants for P, I, D, and F.
        Parameters:
        Kp - the proportional coefficient
        Ki - the integral coefficient
        Kd - the derivative coefficient
        Kf - the feed forward term
        source - The PIDSource object that is used to get values
        output - The PIDOutput object that is set to the output percentage
      • PIDBase

        public PIDBase​(double Kp,
                       double Ki,
                       double Kd,
                       PIDSource source,
                       PIDOutput output)
        Allocate a PID object with the given constants for P, I, and D.
        Parameters:
        Kp - the proportional coefficient
        Ki - the integral coefficient
        Kd - the derivative coefficient
        source - the PIDSource object that is used to get values
        output - the PIDOutput object that is set to the output percentage
    • Method Detail

      • calculate

        protected void calculate()
        Read the input, calculate the output accordingly, and write to the output. This should only be called by the PIDTask and is created during initialization.
      • calculateFeedForward

        protected double calculateFeedForward()
        Calculate the feed forward term.

        Both of the provided feed forward calculations are velocity feed forwards. If a different feed forward calculation is desired, the user can override this function and provide his or her own. This function does no synchronization because the PIDController class only calls it in synchronized code, so be careful if calling it oneself.

        If a velocity PID controller is being used, the F term should be set to 1 over the maximum setpoint for the output. If a position PID controller is being used, the F term should be set to 1 over the maximum speed for the output measured in setpoint units per this controller's update period (see the default period in this class's constructor).

      • setPID

        public void setPID​(double p,
                           double i,
                           double d)
        Set the PID Controller gain parameters. Set the proportional, integral, and differential coefficients.
        Specified by:
        setPID in interface PIDInterface
        Parameters:
        p - Proportional coefficient
        i - Integral coefficient
        d - Differential coefficient
      • setPID

        public void setPID​(double p,
                           double i,
                           double d,
                           double f)
        Set the PID Controller gain parameters. Set the proportional, integral, and differential coefficients.
        Parameters:
        p - Proportional coefficient
        i - Integral coefficient
        d - Differential coefficient
        f - Feed forward coefficient
      • setP

        public void setP​(double p)
        Set the Proportional coefficient of the PID controller gain.
        Parameters:
        p - Proportional coefficient
      • setI

        public void setI​(double i)
        Set the Integral coefficient of the PID controller gain.
        Parameters:
        i - Integral coefficient
      • setD

        public void setD​(double d)
        Set the Differential coefficient of the PID controller gain.
        Parameters:
        d - differential coefficient
      • setF

        public void setF​(double f)
        Set the Feed forward coefficient of the PID controller gain.
        Parameters:
        f - feed forward coefficient
      • getP

        public double getP()
        Get the Proportional coefficient.
        Specified by:
        getP in interface PIDInterface
        Returns:
        proportional coefficient
      • getI

        public double getI()
        Get the Integral coefficient.
        Specified by:
        getI in interface PIDInterface
        Returns:
        integral coefficient
      • getD

        public double getD()
        Get the Differential coefficient.
        Specified by:
        getD in interface PIDInterface
        Returns:
        differential coefficient
      • getF

        public double getF()
        Get the Feed forward coefficient.
        Returns:
        feed forward coefficient
      • get

        public double get()
        Return the current PID result This is always centered on zero and constrained the the max and min outs.
        Returns:
        the latest calculated output
      • setContinuous

        public void setContinuous​(boolean continuous)
        Set the PID controller to consider the input to be continuous, Rather then 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.
        Parameters:
        continuous - Set to true turns on continuous, false turns off continuous
      • setContinuous

        public void setContinuous()
        Set the PID controller to consider the input to be continuous, Rather then 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.
      • setInputRange

        public void setInputRange​(double minimumInput,
                                  double maximumInput)
        Sets the maximum and minimum values expected from the input and setpoint.
        Parameters:
        minimumInput - the minimum value expected from the input
        maximumInput - the maximum value expected from the input
      • setOutputRange

        public void setOutputRange​(double minimumOutput,
                                   double maximumOutput)
        Sets the minimum and maximum values to write.
        Parameters:
        minimumOutput - the minimum percentage to write to the output
        maximumOutput - the maximum percentage to write to the output
      • setSetpoint

        public void setSetpoint​(double setpoint)
        Set the setpoint for the PIDController.
        Specified by:
        setSetpoint in interface PIDInterface
        Parameters:
        setpoint - the desired setpoint
      • getSetpoint

        public double getSetpoint()
        Returns the current setpoint of the PIDController.
        Specified by:
        getSetpoint in interface PIDInterface
        Returns:
        the current setpoint
      • getDeltaSetpoint

        public double getDeltaSetpoint()
        Returns the change in setpoint over time of the PIDController.
        Returns:
        the change in setpoint over time
      • getError

        public double getError()
        Returns the current difference of the input from the setpoint.
        Specified by:
        getError in interface PIDInterface
        Returns:
        the current error
      • getAvgError

        @Deprecated
        public double getAvgError()
        Deprecated.
        Use getError(), which is now already filtered.
        Returns the current difference of the error over the past few iterations. You can specify the number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is used for the onTarget() function.
        Returns:
        the current average of the error
      • setPIDSourceType

        public void setPIDSourceType​(PIDSourceType pidSource)
        Sets what type of input the PID controller will use.
        Parameters:
        pidSource - the type of input
      • getPIDSourceType

        public PIDSourceType getPIDSourceType()
        Returns the type of input the PID controller is using.
        Returns:
        the PID controller input type
      • setTolerance

        @Deprecated
        public void setTolerance​(PIDBase.Tolerance tolerance)
        Deprecated.
        Use setPercentTolerance() instead.
        Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of the range or as an absolute value. The Tolerance object encapsulates those options in an object. Use it by creating the type of tolerance that you want to use: setTolerance(new PIDController.AbsoluteTolerance(0.1))
        Parameters:
        tolerance - A tolerance object of the right type, e.g. PercentTolerance or AbsoluteTolerance
      • setAbsoluteTolerance

        public void setAbsoluteTolerance​(double absvalue)
        Set the absolute error which is considered tolerable for use with OnTarget.
        Parameters:
        absvalue - absolute error which is tolerable in the units of the input object
      • setPercentTolerance

        public void setPercentTolerance​(double percentage)
        Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 = 15 percent)
        Parameters:
        percentage - percent error which is tolerable
      • setToleranceBuffer

        @Deprecated
        public void setToleranceBuffer​(int bufLength)
        Deprecated.
        Use a LinearDigitalFilter as the input.
        Set the number of previous error samples to average for tolerancing. When determining whether a mechanism is on target, the user may want to use a rolling average of previous measurements instead of a precise position or velocity. This is useful for noisy sensors which return a few erroneous measurements when the mechanism is on target. However, the mechanism will not register as on target for at least the specified bufLength cycles.
        Parameters:
        bufLength - Number of previous cycles to average.
      • onTarget

        public boolean onTarget()
        Return true if the error is within the percentage of the total input range, determined by setTolerance. This assumes that the maximum and minimum input were set using setInput.
        Returns:
        true if the error is less than the tolerance
      • reset

        public void reset()
        Reset the previous error, the integral term, and disable the controller.
        Specified by:
        reset in interface PIDInterface
      • pidWrite

        public void pidWrite​(double output)
        Passes the output directly to setSetpoint().

        PIDControllers can be nested by passing a PIDController as another PIDController's output. In that case, the output of the parent controller becomes the input (i.e., the reference) of the child.

        It is the caller's responsibility to put the data into a valid form for setSetpoint().

        Specified by:
        pidWrite in interface PIDOutput
        Parameters:
        output - the value calculated by PIDController
      • getContinuousError

        protected double getContinuousError​(double error)
        Wraps error around for continuous inputs. The original error is returned if continuous mode is disabled. This is an unsynchronized function.
        Parameters:
        error - The current error of the PID controller.
        Returns:
        Error for continuous inputs.