Package trclib.robotcore

Class TrcPidController

java.lang.Object
trclib.robotcore.TrcPidController
Direct Known Subclasses:
TrcCascadePidController
public class TrcPidController extends Object
This class implements a PID controller. A PID controller takes a target set point and an input from a feedback device to calculate the output power of an effector usually a motor or a set of motors.

  • Field Details

    • DEF_SETTLING_TIME

      public static final double DEF_SETTLING_TIME
      See Also:

    • DEF_STALL_DETECTION_DELAY

      private static final double DEF_STALL_DETECTION_DELAY
      See Also:

    • DEF_STALL_DETECTION_TIMEOUT

      private static final double DEF_STALL_DETECTION_TIMEOUT
      See Also:

    • DEF_STALL_ERR_RATE_THRESHOLD

      private static final double DEF_STALL_ERR_RATE_THRESHOLD
      See Also:

    • dashboard

      private final TrcDashboard dashboard

    • tracer

      public final TrcDbgTrace tracer

    • instanceName

      private final String instanceName

    • pidInput

      private final TrcPidController.PidInput pidInput

    • inverted

      private boolean inverted

    • absSetPoint

      private boolean absSetPoint

    • noOscillation

      private boolean noOscillation

    • minOutput

      private double minOutput

    • maxOutput

      private double maxOutput

    • outputLimit

      private double outputLimit

    • rampRate

      private Double rampRate

    • outputLimitStack

      private final Stack<Double> outputLimitStack

    • pidCtrlState

      private final TrcPidController.PidCtrlState pidCtrlState

  • Constructor Details

    • TrcPidController

      public TrcPidController(String instanceName, TrcPidController.PidCoefficients pidCoeffs, TrcPidController.PidInput pidInput)
      Constructor: Create an instance of the object.
      Parameters:
      instanceName - specifies the instance name.
      pidCoeffs - specifies the PID coefficients.
      pidInput - specifies the method to call to get PID sensor input.

  • Method Details

    • toString

      public String toString()
      This method returns the instance name.
      Overrides:
      toString in class Object
      Returns:
      instance name.

    • setTraceLevel

      public void setTraceLevel(TrcDbgTrace.MsgLevel msgLevel, boolean verbosePidInfo, TrcRobotBattery battery)
      This method sets the message trace level for the tracer.
      Parameters:
      msgLevel - specifies the message level.
      verbosePidInfo - specifies true to trace verbose PID info, false otherwise.
      battery - specifies the battery object to get battery info for the message.

    • setTraceLevel

      public void setTraceLevel(TrcDbgTrace.MsgLevel msgLevel, boolean verbosePidInfo)
      This method sets the message trace level for the tracer.
      Parameters:
      msgLevel - specifies the message level.
      verbosePidInfo - specifies true to trace verbose PID info, false otherwise.

    • setTraceLevel

      public void setTraceLevel(TrcDbgTrace.MsgLevel msgLevel)
      This method sets the message trace level for the tracer.
      Parameters:
      msgLevel - specifies the message level.

    • getPidInput

      public TrcPidController.PidInput getPidInput()
      This method returns the PidInput interface.
      Returns:
      PidInput interface.

    • setInverted

      public void setInverted(boolean inverted)
      This method inverts the sign of the calculated error. Normally, the calculated error starts with a large positive number and goes down. However, in some sensors such as the ultrasonic sensor, the target is a small number and the error starts with a negative value and increases. In order to calculate a correct output which will go towards the target, the error sign must be inverted.
      Parameters:
      inverted - specifies true to invert the sign of the calculated error, false otherwise.

    • setAbsoluteSetPoint

      public void setAbsoluteSetPoint(boolean absolute)
      This method sets the set point mode to be absolute. PID controller always calculates the output with an absolute set point comparing to a sensor value representing an absolute input. But by default, it will treat the set point as a value relative to its current input. So it will add the relative set point value to the current input as the absolute set point in its calculation. This method allows the caller to treat the set point as absolute set point.
      Parameters:
      absolute - specifies true if set point is absolute, false otherwise.

    • hasAbsoluteSetPoint

      public boolean hasAbsoluteSetPoint()
      This method returns true if setpoints are absolute, false otherwise.
      Returns:
      true if setpoints are absolute, false otherwise.

    • setNoOscillation

      public void setNoOscillation(boolean noOscillation)
      This method enables/disables NoOscillation mode. In PID control, if the PID constants are not tuned quite correctly, it may cause oscillation that could waste a lot of time. In some scenarios, passing the target beyond the tolerance may be acceptable. This method allows the PID controller to declare "On Target" even though it passes the target beyond tolerance so it doesn't oscillate.
      Parameters:
      noOscillation - specifies true to enable no oscillation, false to disable.

    • setStallDetectionEnabled

      public void setStallDetectionEnabled(double stallDetectionDelay, double stallDetectionTimeout, double stallErrorRateThreshold)
      This method enables/disables stall detection.
      Parameters:
      stallDetectionDelay - specifies stall detection start delay in seconds, zero to disable stall detection.
      stallDetectionTimeout - specifies stall timeout in seconds which is the minimum elapsed time for the motor to be motionless to be considered stalled.
      stallErrorRateThreshold - specifies the error rate threshold below which it will consider stalling.

    • setStallDetectionEnabled

      public void setStallDetectionEnabled(boolean enabled)
      This method enables/disables stall detection.
      Parameters:
      enabled - specifies true to enable stall detection, false to disable.

    • startStallDetection

      public void startStallDetection()
      This method starts stall detection.

    • endStallDetection

      public void endStallDetection()
      This method ends stall detection.

    • getPidCoefficients

      public TrcPidController.PidCoefficients getPidCoefficients()
      This method returns the current PID coefficients.
      Returns:
      current PID coefficients.

    • setPidCoefficients

      public void setPidCoefficients(TrcPidController.PidCoefficients pidCoeffs)
      This method sets new PID coefficients.
      Parameters:
      pidCoeffs - specifies new PID coefficients.

    • setRampRate

      public void setRampRate(Double rampRate)
      This method sets the ramp rate of the PID controller output. It is sometimes useful to limit the acceleration of the output of the PID controller. For example, the strafing PID controller on a mecanum drive base may benefit from a lower acceleration to minimize wheel slippage.
      Parameters:
      rampRate - specifies the ramp rate in percent power per second, null to disable.

    • setOutputRange

      public void setOutputRange(double minOutput, double maxOutput)
      This method sets a range limit on the calculated output. It is very useful to limit the output range to less than full power for scenarios such as using mecanum wheels on a drive train to prevent wheel slipping or slow down a PID drive in order to detect a line etc.
      Parameters:
      minOutput - specifies the PID output lower range limit.
      maxOutput - specifies the PID output higher range limit.

    • setOutputLimit

      public void setOutputLimit(double limit)
      This method sets the output to the range -limit to +limit. It calls setOutputRange. If the caller wants to limit the output power symmetrically, this is the method to call, not setOutputRange.
      Parameters:
      limit - specifies the output limit as a positive number.

    • getOutputLimit

      public double getOutputLimit()
      This method returns the last set output limit. It is sometimes useful to temporarily change the output range of the PID controller for an operation and restore it afterwards. This method allows the caller to save the last set output limit and restore it later on.
      Returns:
      last set output limit.

    • saveAndSetOutputLimit

      public double saveAndSetOutputLimit(double limit)
      This method saves the current output limit of the PID controller and sets it to the given new limit. This is useful if the caller wants to temporarily set a limit for an operation and restore it afterwards. Note: this is implemented with a stack so it is assuming the saving and restoring calls are nested in nature. If this is called in a multi-threading environment where saving and restoring can be interleaved by different threads, unexpected result may happen. It is recommended to avoid this type of scenario if possible.
      Parameters:
      limit - specifies the new output limit.
      Returns:
      return the previous output limit.

    • restoreOutputLimit

      public double restoreOutputLimit()
      This method restores the last saved output limit and return its value. If there was no previous call to saveAndSetOutputLimit, the current output limit is returned and the limit is not changed.
      Returns:
      last saved output limit.

    • getTarget

      public double getTarget()
      This method returns the current set point value.
      Returns:
      current set point.

    • setTarget

      public void setTarget(double target, TrcWarpSpace warpSpace, boolean resetError)
      This methods sets the target set point.
      Parameters:
      target - specifies the target set point.
      warpSpace - specifies the warp space object if the target is in one, null if not.
      resetError - specifies true to reset error state, false otherwise. It is important to preserve error state if we are changing target before the PID operation is completed.

    • setTarget

      public void setTarget(double target, TrcWarpSpace warpSpace)
      This methods sets the target set point.
      Parameters:
      target - specifies the target set point.
      warpSpace - specifies the warp space object if the target is in one, null if not.

    • setTarget

      public void setTarget(double target, boolean resetError)
      This methods sets the target set point.
      Parameters:
      target - specifies the target set point.
      resetError - specifies true to reset error state, false otherwise. It is important to preserve error state if we are changing target before the PID operation is completed.

    • setTarget

      public void setTarget(double target)
      This methods sets the target set point.
      Parameters:
      target - specifies the target set point.

    • getError

      public double getError()
      This method returns the error of a previous output calculation.
      Returns:
      previous error.

    • reset

      public void reset()
      This method resets the PID controller clearing the set point, error, total error and output.

    • isStalled

      public boolean isStalled()
      This method detects if PID is stalled.
      Returns:
      true if PID is stalled, false otherwise.

    • isOnTarget

      public boolean isOnTarget(double tolerance, double settlingTime)
      This method determines if we have reached the set point target. It is considered on target if the previous error is smaller than the tolerance and there is no movement for at least settling time. If NoOscillation mode is set, it is considered on target if we are within tolerance or pass target regardless of setting time.
      Parameters:
      tolerance - specifies the PID error tolerance.
      settlingTime - specifies the minimum on target settling time.
      Returns:
      true if we reached target, false otherwise.

    • isOnTarget

      public boolean isOnTarget(double tolerance)
      This method determines if we have reached the set point target. It is considered on target if the previous error is smaller than the tolerance and there is no movement for at least settling time. If NoOscillation mode is set, it is considered on target if we are within tolerance or pass target regardless of setting time.
      Parameters:
      tolerance - specifies the tolerance.
      Returns:
      true if we reached target, false otherwise.

    • getCurrentInput

      public double getCurrentInput()
      This method returns the current PID input value.
      Returns:
      current PID input value.

    • getOutput

      public double getOutput(double input, double setPoint)
      This method calculates the PID output applying the PID equation to the given set point target and current input value.
      Returns:
      PID output value.

    • getOutput

      public double getOutput()
      This method calculates the PID output applying the PID equation to the given set point target and current input value.
      Returns:
      PID output value.

    • displayPidInfo

      public void displayPidInfo(int lineNum)
      This method displays the PID information on the dashboard for debugging and tuning purpose. Note that the PID info occupies two dashboard lines.
      Parameters:
      lineNum - specifies the starting line number of the dashboard to display the info.

    • printPidInfo

      public void printPidInfo(TrcDbgTrace msgTracer, boolean verbose, TrcRobotBattery battery)
      This method prints the PID information to the tracer console. If no tracer is provided, it will attempt to use the debug tracer in this module but if the debug tracer is not enabled, no output will be produced.
      Parameters:
      msgTracer - specifies the tracer object to print the PID info to.
      verbose - specifies true to print verbose info, false to print summary info.
      battery - specifies the battery object to get battery info, can be null if not provided.

    • printPidInfo

      public void printPidInfo(TrcDbgTrace tracer, boolean verbose)
      This method prints the PID information to the tracer console. If no tracer is provided, it will attempt to use the debug tracer in this module but if the debug tracer is not enabled, no output will be produced.
      Parameters:
      tracer - specifies the tracer object to print the PID info to.
      verbose - specifies true to print verbose info, false to print summary info.

    • printPidInfo

      public void printPidInfo(TrcDbgTrace tracer)
      This method prints the PID information to the tracer console. If no tracer is provided, it will attempt to use the debug tracer in this module but if the debug tracer is not enabled, no output will be produced.
      Parameters:
      tracer - specifies the tracer object to print the PID info to.

    • printPidInfo

      public void printPidInfo()
      This method prints the PID information to the default debug tracer.