Package edu.wpi.first.wpilibj.command
Class PIDCommand
- java.lang.Object
-
- edu.wpi.first.wpilibj.command.Command
-
- edu.wpi.first.wpilibj.command.PIDCommand
-
- All Implemented Interfaces:
Sendable
,AutoCloseable
public abstract class PIDCommand extends Command
This class defines aCommand
which interacts heavily with a PID loop.It provides some convenience methods to run an internal
PIDController
. It will also start and stop saidPIDController
when thePIDCommand
is first initialized and ended/interrupted.
-
-
Constructor Summary
Constructors Constructor Description PIDCommand(double p, double i, double d)
Instantiates aPIDCommand
that will use the given p, i and d values.PIDCommand(double p, double i, double d, double period)
Instantiates aPIDCommand
that will use the given p, i and d values.PIDCommand(double p, double i, double d, double period, Subsystem subsystem)
Instantiates aPIDCommand
that will use the given p, i and d values.PIDCommand(double p, double i, double d, Subsystem subsystem)
Instantiates aPIDCommand
that will use the given p, i and d values.PIDCommand(String name, double p, double i, double d)
Instantiates aPIDCommand
that will use the given p, i and d values.PIDCommand(String name, double p, double i, double d, double period)
Instantiates aPIDCommand
that will use the given p, i and d values.PIDCommand(String name, double p, double i, double d, double period, Subsystem subsystem)
Instantiates aPIDCommand
that will use the given p, i and d values.PIDCommand(String name, double p, double i, double d, Subsystem subsystem)
Instantiates aPIDCommand
that will use the given p, i and d values.
-
Method Summary
All Methods Instance Methods Abstract Methods Concrete Methods Modifier and Type Method Description protected PIDController
getPIDController()
Returns thePIDController
used by thisPIDCommand
.protected double
getPosition()
Returns the current position.protected double
getSetpoint()
Returns the setpoint.void
initSendable(SendableBuilder builder)
Initializes thisSendable
object.protected abstract double
returnPIDInput()
Returns the input for the pid loop.protected void
setInputRange(double minimumInput, double maximumInput)
Sets the maximum and minimum values expected from the input and setpoint.protected 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.Command
cancel, clearRequirements, close, doesRequire, end, execute, getGroup, getName, getSubsystem, initialize, interrupted, isCanceled, isCompleted, isFinished, isInterruptible, isRunning, isTimedOut, requires, setInterruptible, setName, setRunWhenDisabled, setSubsystem, setTimeout, start, timeSinceInitialized, toString, willRunWhenDisabled
-
-
-
-
Constructor Detail
-
PIDCommand
public PIDCommand(String name, double p, double i, double d)
Instantiates aPIDCommand
that will use the given p, i and d values.- Parameters:
name
- the name of the commandp
- the proportional valuei
- the integral valued
- the derivative value
-
PIDCommand
public PIDCommand(String name, double p, double i, double d, double period)
Instantiates aPIDCommand
that will use the given p, i and d 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 valueperiod
- the time (in seconds) between calculations
-
PIDCommand
public PIDCommand(double p, double i, double d)
Instantiates aPIDCommand
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
-
PIDCommand
public PIDCommand(double p, double i, double d, double period)
Instantiates aPIDCommand
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
-
PIDCommand
public PIDCommand(String name, double p, double i, double d, Subsystem subsystem)
Instantiates aPIDCommand
that will use the given p, i and d values.- Parameters:
name
- the name of the commandp
- the proportional valuei
- the integral valued
- the derivative valuesubsystem
- the subsystem that this command requires
-
PIDCommand
public PIDCommand(String name, double p, double i, double d, double period, Subsystem subsystem)
Instantiates aPIDCommand
that will use the given p, i and d 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 valueperiod
- the time (in seconds) between calculationssubsystem
- the subsystem that this command requires
-
PIDCommand
public PIDCommand(double p, double i, double d, Subsystem subsystem)
Instantiates aPIDCommand
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 valuesubsystem
- the subsystem that this command requires
-
PIDCommand
public PIDCommand(double p, double i, double d, double period, Subsystem subsystem)
Instantiates aPIDCommand
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 calculationssubsystem
- the subsystem that this command requires
-
-
Method Detail
-
getPIDController
protected PIDController getPIDController()
Returns thePIDController
used by thisPIDCommand
. Use this if you would like to fine tune the pid loop.- Returns:
- the
PIDController
used by thisPIDCommand
-
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
protected 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
protected double getSetpoint()
Returns the setpoint.- Returns:
- the setpoint
-
getPosition
protected double getPosition()
Returns the current position.- Returns:
- the current position
-
setInputRange
protected void setInputRange(double minimumInput, double maximumInput)
Sets the maximum and minimum values expected from the input and setpoint.- Parameters:
minimumInput
- the minimum value expected from the input and setpointmaximumInput
- the maximum value expected from the input and setpoint
-
returnPIDInput
protected abstract double returnPIDInput()
Returns the input for the pid loop.It returns the input for the pid loop, so if this command was based off of a gyro, then it should return the angle of the gyro.
All subclasses of
PIDCommand
must override this method.This method will be called in a different thread then the
Scheduler
thread.- 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
PIDCommand
must override this method.This method will be called in a different thread then the
Scheduler
thread.- Parameters:
output
- the value the pid loop calculated
-
initSendable
public void initSendable(SendableBuilder builder)
Description copied from interface:Sendable
Initializes thisSendable
object.- Specified by:
initSendable
in interfaceSendable
- Overrides:
initSendable
in classCommand
- Parameters:
builder
- sendable builder
-
-