Package org.chsrobotics.lib.controllers

Class PID

  • public class PID
extends java.lang.Object
    Implementation of a simple Proportional-Integral-Derivative feedback controller.

    The PID controller is a function with the goal of bringing the difference (or error) between a setpoint (target) and an actual measurement to zero. It does this using feedback control, where the past state(s) of a controlled system are leveraged to find the current output.

    Since the values of the gains are tuned, they and the output are dimensionless. However, the gains need to be optimized to output what would be sensible values of other units, such as motor controller voltage or duty cycle.

    The PID controller has three internal components: fittingly, P, I, and D.

    P, or Proportional, simply consists of multiplication of the error by a constant. This term does the bulk of lifting in the controller and is usually primarily responsible for the bulk of the output. In a theoretical world without static friction and motor input limits, it's enough to get to the setpoint.

    I, or Integral, consists of the multiplication of the *sum* of past error by a constant. This past sum should be reset at sensible times to keep it from eccessively accumulating. The Integral term, as it sums up error over time, is able to turn a small amount of constant error the P term can't solve (steady-state error) into enough of a control input to get to the setpoint.

    D, Derivative and the final term, consists of the multiplication of the *rate of change* of the error by a constant. This is useful to prevent the P term of the controller from entering a series of oscillations where it goes back and forth with high amplitude around the setpoint.

    Methods for tuning the constants of the PID Controller are a matter of opinion, however, the original author of this doc recommends to start with all constants at 0. Tweak the P term slightly until you arrive at a situation where the P term rapidly gets to the setpoint without overshooting. Any steady-state error that is left can be sparingly corrected with the I term. The D term should be used when the P controller needed for good performance near the setpoint causes the controller to overshoot the setpoint.

    When docs in this class refer to "position" or "velocity", "position" refers to the quantity of the thing being controlled, "velocity" to the rate of change of that thing. So it's possible to make a velocity PID controller, or something else with a controlled quantity other than position.

    • Nested Class Summary

      Nested Classes 
      Modifier and Type Class Description
      static class  PID.PIDConstants
      Data class for holding the gains to a PID controller.

    • Constructor Summary

      Constructors 
      Constructor Description
      PID​(double kP, double kI, double kD, double initialSetpoint)
      Constructs a PID with given gains.
      PID​(PID.PIDConstants constants, double initialSetpoint)
      Constructs a PID with a given PIDConstants.

    • Method Summary

      All Methods Instance Methods Concrete Methods 
      Modifier and Type Method Description
      double calculate​(double measurement)
      Returns an output from the controller with the default robot loop time.
      double calculate​(double measurement, double dt)
      Returns an output from the controller with a given dt.
      PID.PIDConstants getConstants()
      Returns the current gains of the controller.
      double getCurrentState()
      Returns the last reported measurement given to the controller.
      double getIntegralAccumulation()
      Returns the accumulated past error in the integral term.
      double getkD()
      Returns the current derivative gain of the controller.
      double getkI()
      Returns the current integral gain of the controller.
      double getkP()
      Returns the current proportional gain of the controller.
      double getSetpoint()
      Returns the current setpoint (target) of the controller.
      double getSetpointPositionTolerance()
      Returns the maximum allowed position error for isAtSetpoint() to return true.
      double getSetpointVelocityTolerance()
      Returns the maximum allowed velocity error for isAtSetpoint() to return true.
      boolean isAtSetpoint()
      Returns whether the controller has reached the setpoint with minimal velocity.
      void reset()
      Resets all references to past states in the controller, effectively restarting it.
      void resetIntegralAccumulation()
      Resets accumulation of past error in the integral term.
      void resetPreviousMeasurement()
      Resets the previous measurement used for velocity approximation for the derivative term.
      void setConstants​(double kP, double kI, double kD)
      Sets the gains of the controller using provided gains.
      void setConstants​(PID.PIDConstants constants)
      Sets the gains of the controller using a provided PIDConstants.
      void setkD​(double kD)
      Sets the derivative gain of the controller.
      void setkI​(double kI)
      Sets the integral gain of the controller.
      void setkP​(double kP)
      Sets the proportional gain of the controller.
      void setSetpoint​(double value)
      Sets the setpoint (target) of the controller.
      void setSetpointTolerance​(double positionTolerance, double velocityTolerance)
      Sets the maximum error of both position and velocity allowed for isAtSetpoint() to return true.

      • Methods inherited from class java.lang.Object

        clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

    • Constructor Detail

      • PID

        public PID​(double kP,
           double kI,
           double kD,
           double initialSetpoint)
        Constructs a PID with given gains.
        Parameters:
        kP - The initial proportional gain of the controller.
        kI - The initial integral gain of the controller.
        kD - The initial derivative gain of the controller.
        initialSetpoint - The initial setpoint (or target) of the controller.

      • PID

        public PID​(PID.PIDConstants constants,
           double initialSetpoint)
        Constructs a PID with a given PIDConstants.
        Parameters:
        constants - The PIDConstants containing the gains for this controller.
        initialSetpoint - The initial setpoint (or target) of the controller.

    • Method Detail

      • setConstants

        public void setConstants​(double kP,
                         double kI,
                         double kD)
        Sets the gains of the controller using provided gains.
        Parameters:
        kP - The proportional gain for the controller.
        kI - The integral gain for the controller.
        kD - The derivative gain for the controller.

      • setConstants

        public void setConstants​(PID.PIDConstants constants)
        Sets the gains of the controller using a provided PIDConstants.
        Parameters:
        constants - The PIDConstants containing the gains for the controller.

      • getConstants

        public PID.PIDConstants getConstants()
        Returns the current gains of the controller.
        Returns:
        A PIDConstants containing the gains of the controller.

      • getkP

        public double getkP()
        Returns the current proportional gain of the controller.
        Returns:
        The current kP.

      • setkP

        public void setkP​(double kP)
        Sets the proportional gain of the controller.
        Parameters:
        kP - The new kP for the controller.

      • getkI

        public double getkI()
        Returns the current integral gain of the controller.
        Returns:
        The current kI.

      • setkI

        public void setkI​(double kI)
        Sets the integral gain of the controller.
        Parameters:
        kI - The new kI for the controller.

      • getkD

        public double getkD()
        Returns the current derivative gain of the controller.
        Returns:
        The current kD.

      • setkD

        public void setkD​(double kD)
        Sets the derivative gain of the controller.
        Parameters:
        kD - The new kD for the controller.

      • setSetpoint

        public void setSetpoint​(double value)
        Sets the setpoint (target) of the controller.
        Parameters:
        value - The new target of the controller.

      • getSetpoint

        public double getSetpoint()
        Returns the current setpoint (target) of the controller.
        Returns:
        The current setpoint.

      • getIntegralAccumulation

        public double getIntegralAccumulation()
        Returns the accumulated past error in the integral term. Not equal to the output of the I term: this is not multiplied by the gain.
        Returns:
        The integral of error with respect to time from the last reset to now.

      • resetIntegralAccumulation

        public void resetIntegralAccumulation()
        Resets accumulation of past error in the integral term.

      • resetPreviousMeasurement

        public void resetPreviousMeasurement()
        Resets the previous measurement used for velocity approximation for the derivative term.

      • reset

        public void reset()
        Resets all references to past states in the controller, effectively restarting it.

      • setSetpointTolerance

        public void setSetpointTolerance​(double positionTolerance,
                                 double velocityTolerance)
        Sets the maximum error of both position and velocity allowed for isAtSetpoint() to return true.
        Parameters:
        positionTolerance - The maximum allowed position error, as a proportion of the setpoint.
        velocityTolerance - The maximum allowed absolute velocity per second, as a proportion of the setpoint / second.

      • getSetpointPositionTolerance

        public double getSetpointPositionTolerance()
        Returns the maximum allowed position error for isAtSetpoint() to return true.
        Returns:
        The maximum allowed position error, as a proportion of the setpoint.

      • getSetpointVelocityTolerance

        public double getSetpointVelocityTolerance()
        Returns the maximum allowed velocity error for isAtSetpoint() to return true.
        Returns:
        The maximum allowed absolute velocity per second, as a proportion of the setpoint.

      • getCurrentState

        public double getCurrentState()
        Returns the last reported measurement given to the controller.
        Returns:
        The last reported measurement (0 if none have been given).

      • calculate

        public double calculate​(double measurement,
                        double dt)
        Returns an output from the controller with a given dt.
        Parameters:
        measurement - The value of the measured feedback.
        dt - The time, in seconds, since the last update of this controller.
        Returns:
        The sum of the P, I, and D terms of the controller.

      • calculate

        public double calculate​(double measurement)
        Returns an output from the controller with the default robot loop time.

        This must be called at a rate of once every robot loop to be consistent.

        Parameters:
        measurement - The value of the measured feedback.
        Returns:
        The sum of the P, I, and D terms of the controller.

      • isAtSetpoint

        public boolean isAtSetpoint()
        Returns whether the controller has reached the setpoint with minimal velocity.
        Returns:
        Whether the controller is within the minimum position and velocity errors.