Package org.chsrobotics.lib.controllers

Class MotionProfilePID

  • public class MotionProfilePID
extends java.lang.Object
    A PID controller with built-in motion profile following. The setpoint of the controller at a time is the position value of the motion profile at that time.

    For more information on PID controllers and motion profiles, see PID, MotionProfile, and AsymmetricTrapezoidProfile.

    • Constructor Summary

      Constructors 
      Constructor Description
      MotionProfilePID​(double kP, double kI, double kD, MotionProfile profile)
      Constructs a MotionProfilePID out of kP, kI, and kD gains, and a MotionProfile to follow.
      MotionProfilePID​(PID.PIDConstants constants, MotionProfile profile)
      Constructs a MotionProfilePID out of provided PIDConstants and a MotionProfile to follow.

    • Method Summary

      All Methods Instance Methods Concrete Methods 
      Modifier and Type Method Description
      double calculate​(double measurement, double reference)
      Returns an output from the controller, provided a feedback measurement and place into the MotionProfile to sample.
      double calculate​(double measurement, double reference, double dt)
      Returns an output from the controller, provided a feedback measurement, place into the MotionProfile, and delta-time.
      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.
      MotionProfile.State getSetpoint()
      Returns the current setpoint (target) of the controller, as a State with both position and velocity.
      MotionProfile.State getSetpoint​(double time)
      Returns the current setpoint (target) of the controller, as a State with both position and velocity.
      double getSetpointPositionTolerance()
      Returns the maximum allowed position error for isAtSetpoint() to return true.
      boolean isAtSetpoint()
      Returns whether the controller has reached the position of the setpoint.
      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 setSetpointTolerance​(double positionTolerance)
      Sets the maximum position allowed for isFinished() to return true.

      • Methods inherited from class java.lang.Object

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

    • Constructor Detail

      • MotionProfilePID

        public MotionProfilePID​(PID.PIDConstants constants,
                        MotionProfile profile)
        Constructs a MotionProfilePID out of provided PIDConstants and a MotionProfile to follow.
        Parameters:
        constants - The gains of the PID controller.
        profile - The motion profile for the setpoint of the controller to follow.

      • MotionProfilePID

        public MotionProfilePID​(double kP,
                        double kI,
                        double kD,
                        MotionProfile profile)
        Constructs a MotionProfilePID out of kP, kI, and kD gains, and a MotionProfile to follow.
        Parameters:
        kP - The Proportional gain of the controller.
        kI - The Integral gain of the controller.
        kD - The Derivative gain of the controller.
        profile - The motion profile for the setpoint of the controller to follow.

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

      • getConstants

        public PID.PIDConstants getConstants()
        Returns the current gains of the controller.
        Returns:
        A PIDConstants containing the gains of 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.

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

      • getSetpoint

        public MotionProfile.State getSetpoint​(double time)
        Returns the current setpoint (target) of the controller, as a State with both position and velocity.
        Parameters:
        time - The time into the profile to sample, above zero in seconds.
        Returns:
        A State representing the motion profile at that time.

      • getSetpoint

        public MotionProfile.State getSetpoint()
        Returns the current setpoint (target) of the controller, as a State with both position and velocity.

        This uses the last value of reference given to this class's calculate() method.

        Returns:
        A State representing the motion profile at that time.

      • 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)
        Sets the maximum position allowed for isFinished() to return true.
        Parameters:
        positionTolerance - The maximum allowed position error, as a proportion of the setpoint.

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

      • 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 reference)
        Returns an output from the controller, provided a feedback measurement and place into the MotionProfile to sample.

        For this to work, the value of reference must increment by the same amount of times between calls of this.

        Parameters:
        measurement - The value of the measured feedback.
        reference - The time, in seconds greater than zero, to sample into the profile.
        Returns:
        The sum of the P, I, and D terms.

      • calculate

        public double calculate​(double measurement,
                        double reference,
                        double dt)
        Returns an output from the controller, provided a feedback measurement, place into the MotionProfile, and delta-time.
        Parameters:
        measurement - The value of the measured feedback.
        reference - The time, in seconds greater than zero, to sample into the profile.
        dt - The elapsed time since the last call of any of this class's calculate() methods.
        Returns:
        The sum of the P, I, and D terms.

      • isAtSetpoint

        public boolean isAtSetpoint()
        Returns whether the controller has reached the position of the setpoint.
        Returns:
        Whether the controller is within the minimum position error.