Package org.chsrobotics.lib.controllers.feedback

Class PID

java.lang.Object
org.chsrobotics.lib.controllers.feedback.PID
All Implemented Interfaces:
IntrinsicLoggable
public class PID extends Object implements IntrinsicLoggable
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 non-angular PID with given gains and an infinite integration window.
    PID(double kP, double kI, double kD, double initialSetpoint, boolean angular)
    Constructs a PID with given gains and an infinite integration window.
    PID(double kP, double kI, double kD, int integrationWindow, double initialSetpoint)
    Constructs a non-angular PID with given gains and a finite integration window.
    PID(double kP, double kI, double kD, int integrationWindow, double initialSetpoint, boolean angular)
    Constructs a PID with given gains and a finite integration window.
    PID(PID.PIDConstants constants, double initialSetpoint)
    Constructs a non-angular PID with a given PIDConstants and an infinite integration window.
    PID(PID.PIDConstants constants, double initialSetpoint, boolean angular)
    Constructs a PID with a given PIDConstants and an infinite integration window.
    PID(PID.PIDConstants constants, int integrationWindow, double initialSetpoint)
    Constructs a non-angular PID with a given PIDConstants and a finite integration window.
    PID(PID.PIDConstants constants, int integrationWindow, double initialSetpoint, boolean angular)
    Constructs a PID with a given PIDConstants and a finite integration window.

  • Method Summary

    Modifier and Type
    Method
    Description
    boolean
    atSetpoint()
    Returns whether the controller has reached the setpoint with minimal velocity.
    void
    autoGenerateLogs(edu.wpi.first.util.datalog.DataLog log, String name, String subdirName, boolean publishToNT, boolean recordInLog)
    Auto-generates Loggers for important internal values.
    double
    calculate(double measurement)
    Returns an output from the controller with a given dt.
    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
    getCurrentValue()
    Returns the last output of the controller without updating with a new value.
    double
    getDContribution()
    Returns the last result of just the D term.
    double
    getIContribution()
    Returns the last result of just the I term.
    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
    getMaxAbsControlEffort()
    Returns the maximum absolute control effort allowed from the controller.
    double
    getMaxAbsDContribution()
    Returns the maximum absolute contribution allowed from the Derivative term of the controller.
    double
    getMaxAbsIContribution()
    Returns the maximum absolute contribution allowed from the Integral term of the controller.
    double
    getMaxAbsPContribution()
    Returns the maximum absolute contribution allowed from the Proportional term of the controller.
    double
    getPContribution()
    Returns the last result of just the P term.
    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.
    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
    setMaxAbsControlEffort(double newValue)
    Sets a new value to use to constrain the maximum absolute control effort from the controller.
    void
    setMaxAbsIContribution(double newValue)
    Sets a new value to use to constrain the maximum absolute contribution from the Integral term of the controller.
    void
    setMaxAbsPContribution(double newValue)
    Sets a new value to use to constrain the maximum absolute contribution from the Proportional term of the controller.
    void
    setMaxDContribution(double newValue)
    Sets a new value to use to constrain the maximum absolute contribution from the Derivative term 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.
    void
    updateLogs()
    Updates the internally generated Loggers.

    Methods inherited from class java.lang.Object

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

    Methods inherited from interface org.chsrobotics.lib.telemetry.IntrinsicLoggable

    autoGenerateLogs

  • Constructor Details

    • PID

      public PID(double kP, double kI, double kD, int integrationWindow, double initialSetpoint, boolean angular)
      Constructs a PID with given gains and a finite integration window.
      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.
      integrationWindow - The number of past values to consider for integral accumulation. If less than or equal to 0, will be an infinite window.
      initialSetpoint - The initial setpoint (or target) of the controller.
      angular - Whether the controller is controlling an angular quantaty that "wraps". Measurements and setpoints are expected to be in radians if this is true.

    • PID

      public PID(double kP, double kI, double kD, int integrationWindow, double initialSetpoint)
      Constructs a non-angular PID with given gains and a finite integration window.
      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.
      integrationWindow - The number of past values to consider for integral accumulation. If less than or equal to 0, will be an infinite window.
      initialSetpoint - The initial setpoint (or target) of the controller.

    • PID

      public PID(PID.PIDConstants constants, int integrationWindow, double initialSetpoint, boolean angular)
      Constructs a PID with a given PIDConstants and a finite integration window.
      Parameters:
      constants - The PIDConstants containing the gains for this controller.
      integrationWindow - The number of past values to consider for integral accumulation. If less than or equal to 0, will be an infinite window.
      initialSetpoint - The initial setpoint (or target) of the controller.
      angular - Whether the controller is controlling an angular quantaty that "wraps". Measurements and setpoints are expected to be in radians if this is true.

    • PID

      public PID(PID.PIDConstants constants, int integrationWindow, double initialSetpoint)
      Constructs a non-angular PID with a given PIDConstants and a finite integration window.
      Parameters:
      constants - The PIDConstants containing the gains for this controller.
      integrationWindow - The number of past values to consider for integral accumulation. If less than or equal to 0, will be an infinite window.
      initialSetpoint - The initial setpoint (or target) of the controller.

    • PID

      public PID(double kP, double kI, double kD, double initialSetpoint, boolean angular)
      Constructs a PID with given gains and an infinite integration window.
      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.
      angular - Whether the controller is controlling an angular quantaty that "wraps". Measurements and setpoints are expected to be in radians if this is true.

    • PID

      public PID(double kP, double kI, double kD, double initialSetpoint)
      Constructs a non-angular PID with given gains and an infinite integration window.
      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, boolean angular)
      Constructs a PID with a given PIDConstants and an infinite integration window.
      Parameters:
      constants - The PIDConstants containing the gains for this controller.
      initialSetpoint - The initial setpoint (or target) of the controller.
      angular - Whether the controller is controlling an angular quantaty that "wraps". Measurements and setpoints are expected to be in radians if this is true.

    • PID

      public PID(PID.PIDConstants constants, double initialSetpoint)
      Constructs a non-angular PID with a given PIDConstants and an infinite integration window.
      Parameters:
      constants - The PIDConstants containing the gains for this controller.
      initialSetpoint - The initial setpoint (or target) of the controller.

  • Method Details

    • autoGenerateLogs

      public void autoGenerateLogs(edu.wpi.first.util.datalog.DataLog log, String name, String subdirName, boolean publishToNT, boolean recordInLog)
      Description copied from interface: IntrinsicLoggable
      Auto-generates Loggers for important internal values.
      Specified by:
      autoGenerateLogs in interface IntrinsicLoggable
      Parameters:
      log - The DataLog to log values inside of, most likely from HighLevelLogger.getLog() or whatever log is being used program-wide.
      name - The name to associate with this object's logged data.
      subdirName - The string name of the existing or new NetworkTables sub-table to write to.
      publishToNT - Whether this should push logged values to NetworkTables.
      recordInLog - Whether this should store logged values in an on-robot log file.

    • 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.

    • 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 output of the controller.

    • updateLogs

      public void updateLogs()
      Description copied from interface: IntrinsicLoggable
      Updates the internally generated Loggers.
      Specified by:
      updateLogs in interface IntrinsicLoggable

    • getCurrentValue

      public double getCurrentValue()
      Returns the last output of the controller without updating with a new value.
      Returns:
      The last ouptut of the controller.

    • getPContribution

      public double getPContribution()
      Returns the last result of just the P term.
      Returns:
      The last P result. Equal to 0 if calculate() has not been called.

    • getIContribution

      public double getIContribution()
      Returns the last result of just the I term.
      Returns:
      The last I result. Equal to 0 if calculate() has not been called.

    • getDContribution

      public double getDContribution()
      Returns the last result of just the D term.
      Returns:
      The last D result. Equal to 0 if calculate() has not been called.

    • setMaxAbsControlEffort

      public void setMaxAbsControlEffort(double newValue)
      Sets a new value to use to constrain the maximum absolute control effort from the controller.
      Parameters:
      newValue - The maximum absolute control effort. If zero, no limits are applied.

    • setMaxAbsPContribution

      public void setMaxAbsPContribution(double newValue)
      Sets a new value to use to constrain the maximum absolute contribution from the Proportional term of the controller.
      Parameters:
      newValue - The maximum absolute contribution of the P term. If zero, no limits are applied.

    • setMaxAbsIContribution

      public void setMaxAbsIContribution(double newValue)
      Sets a new value to use to constrain the maximum absolute contribution from the Integral term of the controller.
      Parameters:
      newValue - The maximum absolute contribution of the I term. If zero, no limits are applied.

    • setMaxDContribution

      public void setMaxDContribution(double newValue)
      Sets a new value to use to constrain the maximum absolute contribution from the Derivative term of the controller.
      Parameters:
      newValue - The maximum absolute contribution of the D term. If zero, no limits are applied.

    • getMaxAbsControlEffort

      public double getMaxAbsControlEffort()
      Returns the maximum absolute control effort allowed from the controller.
      Returns:
      The maximum absolute P contribution. If zero, no limits are being applied.

    • getMaxAbsPContribution

      public double getMaxAbsPContribution()
      Returns the maximum absolute contribution allowed from the Proportional term of the controller.
      Returns:
      The maximum absolute P contribution. If zero, no limits are being applied.

    • getMaxAbsIContribution

      public double getMaxAbsIContribution()
      Returns the maximum absolute contribution allowed from the Integral term of the controller.
      Returns:
      The maximum absolute I contribution. If zero, no limits are being applied.

    • getMaxAbsDContribution

      public double getMaxAbsDContribution()
      Returns the maximum absolute contribution allowed from the Derivative term of the controller.
      Returns:
      The maximum absolute D contribution. If zero, no limits are being applied.

    • calculate

      public double calculate(double measurement)
      Returns an output from the controller with a given dt.

      Uses the time elapsed since last calling this method as a parameter. If this method is being called for the first time, uses the time since construction.

      Parameters:
      measurement - The value of the measured feedback.
      Returns:
      The output of the controller.

    • atSetpoint

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