Package edu.wpi.first.wpilibj.command
Class PIDSubsystem
- java.lang.Object
-
- edu.wpi.first.wpilibj.command.Subsystem
-
- edu.wpi.first.wpilibj.command.PIDSubsystem
-
- All Implemented Interfaces:
Sendable
,AutoCloseable
public abstract class PIDSubsystem extends Subsystem
This class is designed to handle the case where there is aSubsystem
which uses a singlePIDController
almost constantly (for instance, an elevator which attempts to stay at a constant height).It provides some convenience methods to run an internal
PIDController
. It also allows access to the internalPIDController
in order to give total control to the programmer.
-
-
Constructor Summary
Constructors Constructor Description PIDSubsystem(double p, double i, double d)
Instantiates aPIDSubsystem
that will use the given p, i and d values.PIDSubsystem(double p, double i, double d, double period)
Instantiates aPIDSubsystem
that will use the given p, i and d values.PIDSubsystem(double p, double i, double d, double f, double period)
Instantiates aPIDSubsystem
that will use the given p, i, d, and f values.PIDSubsystem(String name, double p, double i, double d)
Instantiates aPIDSubsystem
that will use the given p, i and d values.PIDSubsystem(String name, double p, double i, double d, double f)
Instantiates aPIDSubsystem
that will use the given p, i, d, and f values.PIDSubsystem(String name, double p, double i, double d, double f, double period)
Instantiates aPIDSubsystem
that will use the given p, i, d, and f values.
-
Method Summary
All Methods Instance Methods Abstract Methods Concrete Methods Modifier and Type Method Description void
disable()
Disables the internalPIDController
.void
enable()
Enables the internalPIDController
.PIDController
getPIDController()
Returns thePIDController
used by thisPIDSubsystem
.double
getPosition()
Returns the current position.double
getSetpoint()
Returns the setpoint.boolean
onTarget()
Return true if the error is within the percentage of the total input range, determined by setTolerance.protected abstract double
returnPIDInput()
Returns the input for the pid loop.void
setAbsoluteTolerance(double t)
Set the absolute error which is considered tolerable for use with OnTarget.void
setInputRange(double minimumInput, double maximumInput)
Sets the maximum and minimum values expected from the input.void
setOutputRange(double minimumOutput, double maximumOutput)
Sets the maximum and minimum values to write.void
setPercentTolerance(double p)
Set the percentage error which is considered tolerable for use with OnTarget.void
setSetpoint(double setpoint)
Sets the setpoint to the given value.void
setSetpointRelative(double deltaSetpoint)
Adds the given value to the setpoint.protected abstract void
usePIDOutput(double output)
Uses the value that the pid loop calculated.-
Methods inherited from class edu.wpi.first.wpilibj.command.Subsystem
addChild, addChild, close, getCurrentCommand, getCurrentCommandName, getDefaultCommand, getDefaultCommandName, getName, getSubsystem, initDefaultCommand, initSendable, periodic, setDefaultCommand, setName, setSubsystem, toString
-
-
-
-
Constructor Detail
-
PIDSubsystem
public PIDSubsystem(String name, double p, double i, double d)
Instantiates aPIDSubsystem
that will use the given p, i and d values.- Parameters:
name
- the namep
- the proportional valuei
- the integral valued
- the derivative value
-
PIDSubsystem
public PIDSubsystem(String name, double p, double i, double d, double f)
Instantiates aPIDSubsystem
that will use the given p, i, d, and f values.- Parameters:
name
- the namep
- the proportional valuei
- the integral valued
- the derivative valuef
- the feed forward value
-
PIDSubsystem
public PIDSubsystem(String name, double p, double i, double d, double f, double period)
Instantiates aPIDSubsystem
that will use the given p, i, d, and f values. It will also space the time between PID loop calculations to be equal to the given period.- Parameters:
name
- the namep
- the proportional valuei
- the integral valued
- the derivative valuef
- the feed forward valueperiod
- the time (in seconds) between calculations
-
PIDSubsystem
public PIDSubsystem(double p, double i, double d)
Instantiates aPIDSubsystem
that will use the given p, i and d values. It will use the class name as its name.- Parameters:
p
- the proportional valuei
- the integral valued
- the derivative value
-
PIDSubsystem
public PIDSubsystem(double p, double i, double d, double f, double period)
Instantiates aPIDSubsystem
that will use the given p, i, d, and f values. It will use the class name as its name. It will also space the time between PID loop calculations to be equal to the given period.- Parameters:
p
- the proportional valuei
- the integral valued
- the derivative valuef
- the feed forward coefficientperiod
- the time (in seconds) between calculations
-
PIDSubsystem
public PIDSubsystem(double p, double i, double d, double period)
Instantiates aPIDSubsystem
that will use the given p, i and d values. It will use the class name as its name. It will also space the time between PID loop calculations to be equal to the given period.- Parameters:
p
- the proportional valuei
- the integral valued
- the derivative valueperiod
- the time (in seconds) between calculations
-
-
Method Detail
-
getPIDController
public PIDController getPIDController()
Returns thePIDController
used by thisPIDSubsystem
. Use this if you would like to fine tune the pid loop.- Returns:
- the
PIDController
used by thisPIDSubsystem
-
setSetpointRelative
public void setSetpointRelative(double deltaSetpoint)
Adds the given value to the setpoint. IfsetInputRange(...)
was used, then the bounds will still be honored by this method.- Parameters:
deltaSetpoint
- the change in the setpoint
-
setSetpoint
public void setSetpoint(double setpoint)
Sets the setpoint to the given value. IfsetInputRange(...)
was called, then the given setpoint will be trimmed to fit within the range.- Parameters:
setpoint
- the new setpoint
-
getSetpoint
public double getSetpoint()
Returns the setpoint.- Returns:
- the setpoint
-
getPosition
public double getPosition()
Returns the current position.- Returns:
- the current position
-
setInputRange
public void setInputRange(double minimumInput, double maximumInput)
Sets the maximum and minimum values expected from the input.- Parameters:
minimumInput
- the minimum value expected from the inputmaximumInput
- the maximum value expected from the output
-
setOutputRange
public void setOutputRange(double minimumOutput, double maximumOutput)
Sets the maximum and minimum values to write.- Parameters:
minimumOutput
- the minimum value to write to the outputmaximumOutput
- the maximum value to write to the output
-
setAbsoluteTolerance
public void setAbsoluteTolerance(double t)
Set the absolute error which is considered tolerable for use with OnTarget. The value is in the same range as the PIDInput values.- Parameters:
t
- the absolute tolerance
-
setPercentTolerance
public void setPercentTolerance(double p)
Set the percentage error which is considered tolerable for use with OnTarget. (Value of 15.0 == 15 percent).- Parameters:
p
- the percent tolerance
-
onTarget
public boolean onTarget()
Return true if the error is within the percentage of the total input range, determined by setTolerance. This assumes that the maximum and minimum input were set using setInput.- Returns:
- true if the error is less than the tolerance
-
returnPIDInput
protected abstract double returnPIDInput()
Returns the input for the pid loop.It returns the input for the pid loop, so if this Subsystem was based off of a gyro, then it should return the angle of the gyro.
All subclasses of
PIDSubsystem
must override this method.- Returns:
- the value the pid loop should use as input
-
usePIDOutput
protected abstract void usePIDOutput(double output)
Uses the value that the pid loop calculated. The calculated value is the "output" parameter. This method is a good time to set motor values, maybe something along the lines ofdriveline.tankDrive(output, -output)
.All subclasses of
PIDSubsystem
must override this method.- Parameters:
output
- the value the pid loop calculated
-
enable
public void enable()
Enables the internalPIDController
.
-
disable
public void disable()
Disables the internalPIDController
.
-
-