Package edu.wpi.first.wpilibj.controller
Class PIDController
- java.lang.Object
-
- edu.wpi.first.wpilibj.controller.PIDController
-
- All Implemented Interfaces:
Sendable,AutoCloseable
public class PIDController extends Object implements Sendable, AutoCloseable
Implements a PID control loop.
-
-
Constructor Summary
Constructors Constructor Description PIDController(double Kp, double Ki, double Kd)Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of 0.02 seconds.PIDController(double Kp, double Ki, double Kd, double period)Allocates a PIDController with the given constants for Kp, Ki, and Kd.
-
Method Summary
All Methods Instance Methods Concrete Methods Modifier and Type Method Description booleanatSetpoint()Returns true if the error is within the percentage of the total input range, determined by SetTolerance.doublecalculate(double measurement)Returns the next output of the PID controller.doublecalculate(double measurement, double setpoint)Returns the next output of the PID controller.voidclose()voiddisableContinuousInput()Disables continuous input.voidenableContinuousInput(double minimumInput, double maximumInput)Enables continuous input.protected doublegetContinuousError(double error)Wraps error around for continuous inputs.doublegetD()Get the Differential coefficient.doublegetI()Get the Integral coefficient.doublegetP()Get the Proportional coefficient.doublegetPeriod()Returns the period of this controller.doublegetPositionError()Returns the difference between the setpoint and the measurement.doublegetSetpoint()Returns the current setpoint of the PIDController.doublegetVelocityError()Returns the velocity error.voidinitSendable(SendableBuilder builder)Initializes thisSendableobject.voidreset()Resets the previous error and the integral term.voidsetD(double Kd)Sets the Differential coefficient of the PID controller gain.voidsetI(double Ki)Sets the Integral coefficient of the PID controller gain.voidsetIntegratorRange(double minimumIntegral, double maximumIntegral)Sets the minimum and maximum values for the integrator.voidsetP(double Kp)Sets the Proportional coefficient of the PID controller gain.voidsetPID(double Kp, double Ki, double Kd)Sets the PID Controller gain parameters.voidsetSetpoint(double setpoint)Sets the setpoint for the PIDController.voidsetTolerance(double positionTolerance)Sets the error which is considered tolerable for use with atSetpoint().voidsetTolerance(double positionTolerance, double velocityTolerance)Sets the error which is considered tolerable for use with atSetpoint().-
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
-
Methods inherited from interface edu.wpi.first.wpilibj.Sendable
addChild, getName, getSubsystem, setName, setName, setName, setName, setSubsystem
-
-
-
-
Constructor Detail
-
PIDController
public PIDController(double Kp, double Ki, double Kd)Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of 0.02 seconds.- Parameters:
Kp- The proportional coefficient.Ki- The integral coefficient.Kd- The derivative coefficient.
-
PIDController
public PIDController(double Kp, double Ki, double Kd, double period)Allocates a PIDController with the given constants for Kp, Ki, and Kd.- Parameters:
Kp- The proportional coefficient.Ki- The integral coefficient.Kd- The derivative coefficient.period- The period between controller updates in seconds.
-
-
Method Detail
-
close
public void close()
- Specified by:
closein interfaceAutoCloseable
-
setPID
public void setPID(double Kp, double Ki, double Kd)Sets the PID Controller gain parameters.Set the proportional, integral, and differential coefficients.
- Parameters:
Kp- The proportional coefficient.Ki- The integral coefficient.Kd- The derivative coefficient.
-
setP
public void setP(double Kp)
Sets the Proportional coefficient of the PID controller gain.- Parameters:
Kp- proportional coefficient
-
setI
public void setI(double Ki)
Sets the Integral coefficient of the PID controller gain.- Parameters:
Ki- integral coefficient
-
setD
public void setD(double Kd)
Sets the Differential coefficient of the PID controller gain.- Parameters:
Kd- differential coefficient
-
getP
public double getP()
Get the Proportional coefficient.- Returns:
- proportional coefficient
-
getI
public double getI()
Get the Integral coefficient.- Returns:
- integral coefficient
-
getD
public double getD()
Get the Differential coefficient.- Returns:
- differential coefficient
-
getPeriod
public double getPeriod()
Returns the period of this controller.- Returns:
- the period of the controller.
-
setSetpoint
public void setSetpoint(double setpoint)
Sets the setpoint for the PIDController.- Parameters:
setpoint- The desired setpoint.
-
getSetpoint
public double getSetpoint()
Returns the current setpoint of the PIDController.- Returns:
- The current setpoint.
-
atSetpoint
public boolean atSetpoint()
Returns true if the error is within the percentage of the total input range, determined by SetTolerance. This asssumes that the maximum and minimum input were set using SetInput.This will return false until at least one input value has been computed.
- Returns:
- Whether the error is within the acceptable bounds.
-
enableContinuousInput
public void enableContinuousInput(double minimumInput, double maximumInput)Enables continuous input.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:
minimumInput- The minimum value expected from the input.maximumInput- The maximum value expected from the input.
-
disableContinuousInput
public void disableContinuousInput()
Disables continuous input.
-
setIntegratorRange
public void setIntegratorRange(double minimumIntegral, double maximumIntegral)Sets the minimum and maximum values for the integrator.When the cap is reached, the integrator value is added to the controller output rather than the integrator value times the integral gain.
- Parameters:
minimumIntegral- The minimum value of the integrator.maximumIntegral- The maximum value of the integrator.
-
setTolerance
public void setTolerance(double positionTolerance)
Sets the error which is considered tolerable for use with atSetpoint().- Parameters:
positionTolerance- Position error which is tolerable.
-
setTolerance
public void setTolerance(double positionTolerance, double velocityTolerance)Sets the error which is considered tolerable for use with atSetpoint().- Parameters:
positionTolerance- Position error which is tolerable.velocityTolerance- Velocity error which is tolerable.
-
getPositionError
public double getPositionError()
Returns the difference between the setpoint and the measurement.- Returns:
- The error.
-
getVelocityError
public double getVelocityError()
Returns the velocity error.
-
calculate
public double calculate(double measurement, double setpoint)Returns the next output of the PID controller.- Parameters:
measurement- The current measurement of the process variable.setpoint- The new setpoint of the controller.
-
calculate
public double calculate(double measurement)
Returns the next output of the PID controller.- Parameters:
measurement- The current measurement of the process variable.
-
reset
public void reset()
Resets the previous error and the integral term.
-
initSendable
public void initSendable(SendableBuilder builder)
Description copied from interface:SendableInitializes thisSendableobject.- Specified by:
initSendablein interfaceSendable- Parameters:
builder- sendable builder
-
getContinuousError
protected double getContinuousError(double error)
Wraps error around for continuous inputs. The original error is returned if continuous mode is disabled.- Parameters:
error- The current error of the PID controller.- Returns:
- Error for continuous inputs.
-
-