Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Remove PIDController, PIDOutput, PIDSource

Browse Source 
Move them to the old commands vendordep so that PIDCommand and PIDSubsystem
continue to work.

This also removes Filter and LinearDigitalFilter.
This commit is contained in:
Peter Johnson
2021-03-12 15:41:52 -08:00
parent 3abe0b9d49
commit 6b168ab0c8
69 changed files with 30 additions and 1248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

819
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDBase.java Normal file
View File

@@ -0,0 +1,819 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.util.BoundaryException;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import java.util.concurrent.locks.ReentrantLock;
/**
* Class implements a PID Control Loop.
*
* <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
* calculations, as well as writing the given PIDOutput.
*
* <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
* and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
* given set of PID constants.
*
* @deprecated All APIs which use this have been deprecated.
*/
@Deprecated(since = "2020", forRemoval = true)
@SuppressWarnings("PMD.TooManyFields")
public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
public static final double kDefaultPeriod = 0.05;
private static int instances;
// Factor for "proportional" control
@SuppressWarnings("MemberName")
private double m_P;
// Factor for "integral" control
@SuppressWarnings("MemberName")
private double m_I;
// Factor for "derivative" control
@SuppressWarnings("MemberName")
private double m_D;
// Factor for "feed forward" control
@SuppressWarnings("MemberName")
private double m_F;
// |maximum output|
private double m_maximumOutput = 1.0;
// |minimum output|
private double m_minimumOutput = -1.0;
// Maximum input - limit setpoint to this
private double m_maximumInput;
// Minimum input - limit setpoint to this
private double m_minimumInput;
// Input range - difference between maximum and minimum
private double m_inputRange;
// Do the endpoints wrap around? (e.g., absolute encoder)
private boolean m_continuous;
// Is the PID controller enabled
protected boolean m_enabled;
// The prior error (used to compute velocity)
private double m_prevError;
// The sum of the errors for use in the integral calc
private double m_totalError;
// The tolerance object used to check if on target
private Tolerance m_tolerance;
private double m_setpoint;
private double m_prevSetpoint;
@SuppressWarnings("PMD.UnusedPrivateField")
private double m_error;
private double m_result;
private LinearFilter m_filter;
protected ReentrantLock m_thisMutex = new ReentrantLock();
// Ensures when disable() is called, pidWrite() won't run if calculate()
// is already running at that time.
protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
protected PIDSource m_pidInput;
protected PIDOutput m_pidOutput;
protected Timer m_setpointTimer;
/**
* Tolerance is the type of tolerance used to specify if the PID controller is on target.
*
* <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
* specify types of tolerance specifications to use.
*/
public interface Tolerance {
boolean onTarget();
}
/** Used internally for when Tolerance hasn't been set. */
public static class NullTolerance implements Tolerance {
@Override
public boolean onTarget() {
throw new IllegalStateException("No tolerance value set when calling onTarget().");
}
}
public class PercentageTolerance implements Tolerance {
private final double m_percentage;
PercentageTolerance(double value) {
m_percentage = value;
}
@Override
public boolean onTarget() {
return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
}
}
public class AbsoluteTolerance implements Tolerance {
private final double m_value;
AbsoluteTolerance(double value) {
m_value = value;
}
@Override
public boolean onTarget() {
return Math.abs(getError()) < m_value;
}
}
/**
* Allocate a PID object with the given constants for P, I, D, and F.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
requireNonNullParam(source, "PIDSource", "PIDBase");
requireNonNullParam(output, "output", "PIDBase");
m_setpointTimer = new Timer();
m_setpointTimer.start();
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_pidInput = source;
m_filter = LinearFilter.movingAverage(1);
m_pidOutput = output;
instances++;
HAL.report(tResourceType.kResourceType_PIDController, instances);
m_tolerance = new NullTolerance();
SendableRegistry.add(this, "PIDController", instances);
}
/**
* Allocate a PID object with the given constants for P, I, and D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source the PIDSource object that is used to get values
* @param output the PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, 0.0, source, output);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Read the input, calculate the output accordingly, and write to the output. This should only be
* called by the PIDTask and is created during initialization.
*/
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.NPathComplexity"})
protected void calculate() {
if (m_pidInput == null || m_pidOutput == null) {
return;
}
boolean enabled;
m_thisMutex.lock();
try {
enabled = m_enabled;
} finally {
m_thisMutex.unlock();
}
if (enabled) {
double input;
// Storage for function inputs
PIDSourceType pidSourceType;
double P;
double I;
double D;
double feedForward = calculateFeedForward();
double minimumOutput;
double maximumOutput;
// Storage for function input-outputs
double prevError;
double error;
double totalError;
m_thisMutex.lock();
try {
input = m_filter.calculate(m_pidInput.pidGet());
pidSourceType = m_pidInput.getPIDSourceType();
P = m_P;
I = m_I;
D = m_D;
minimumOutput = m_minimumOutput;
maximumOutput = m_maximumOutput;
prevError = m_prevError;
error = getContinuousError(m_setpoint - input);
totalError = m_totalError;
} finally {
m_thisMutex.unlock();
}
// Storage for function outputs
double result;
if (pidSourceType.equals(PIDSourceType.kRate)) {
if (P != 0) {
totalError = clamp(totalError + error, minimumOutput / P, maximumOutput / P);
}
result = P * totalError + D * error + feedForward;
} else {
if (I != 0) {
totalError = clamp(totalError + error, minimumOutput / I, maximumOutput / I);
}
result = P * error + I * totalError + D * (error - prevError) + feedForward;
}
result = clamp(result, minimumOutput, maximumOutput);
// Ensures m_enabled check and pidWrite() call occur atomically
m_pidWriteMutex.lock();
try {
m_thisMutex.lock();
try {
if (m_enabled) {
// Don't block other PIDController operations on pidWrite()
m_thisMutex.unlock();
m_pidOutput.pidWrite(result);
}
} finally {
if (m_thisMutex.isHeldByCurrentThread()) {
m_thisMutex.unlock();
}
}
} finally {
m_pidWriteMutex.unlock();
}
m_thisMutex.lock();
try {
m_prevError = error;
m_error = error;
m_totalError = totalError;
m_result = result;
} finally {
m_thisMutex.unlock();
}
}
}
/**
* Calculate the feed forward term.
*
* <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
* feed forward calculation is desired, the user can override this function and provide his or her
* own. This function does no synchronization because the PIDController class only calls it in
* synchronized code, so be careful if calling it oneself.
*
* <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
* setpoint for the output. If a position PID controller is being used, the F term should be set
* to 1 over the maximum speed for the output measured in setpoint units per this controller's
* update period (see the default period in this class's constructor).
*/
protected double calculateFeedForward() {
if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
return m_F * getSetpoint();
} else {
double temp = m_F * getDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.reset();
return temp;
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and differential
* coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
@Override
@SuppressWarnings("ParameterName")
public void setPID(double p, double i, double d) {
m_thisMutex.lock();
try {
m_P = p;
m_I = i;
m_D = d;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and differential
* coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
* @param f Feed forward coefficient
*/
@SuppressWarnings("ParameterName")
public void setPID(double p, double i, double d, double f) {
m_thisMutex.lock();
try {
m_P = p;
m_I = i;
m_D = d;
m_F = f;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Proportional coefficient of the PID controller gain.
*
* @param p Proportional coefficient
*/
@SuppressWarnings("ParameterName")
public void setP(double p) {
m_thisMutex.lock();
try {
m_P = p;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Integral coefficient of the PID controller gain.
*
* @param i Integral coefficient
*/
@SuppressWarnings("ParameterName")
public void setI(double i) {
m_thisMutex.lock();
try {
m_I = i;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Differential coefficient of the PID controller gain.
*
* @param d differential coefficient
*/
@SuppressWarnings("ParameterName")
public void setD(double d) {
m_thisMutex.lock();
try {
m_D = d;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Feed forward coefficient of the PID controller gain.
*
* @param f feed forward coefficient
*/
@SuppressWarnings("ParameterName")
public void setF(double f) {
m_thisMutex.lock();
try {
m_F = f;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Proportional coefficient.
*
* @return proportional coefficient
*/
@Override
public double getP() {
m_thisMutex.lock();
try {
return m_P;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Integral coefficient.
*
* @return integral coefficient
*/
@Override
public double getI() {
m_thisMutex.lock();
try {
return m_I;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Differential coefficient.
*
* @return differential coefficient
*/
@Override
public double getD() {
m_thisMutex.lock();
try {
return m_D;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Feed forward coefficient.
*
* @return feed forward coefficient
*/
public double getF() {
m_thisMutex.lock();
try {
return m_F;
} finally {
m_thisMutex.unlock();
}
}
/**
* Return the current PID result This is always centered on zero and constrained the the max and
* min outs.
*
* @return the latest calculated output
*/
public double get() {
m_thisMutex.lock();
try {
return m_result;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID controller to consider the input to be continuous, 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.
*
* @param continuous Set to true turns on continuous, false turns off continuous
*/
public void setContinuous(boolean continuous) {
if (continuous && m_inputRange <= 0) {
throw new IllegalStateException("No input range set when calling setContinuous().");
}
m_thisMutex.lock();
try {
m_continuous = continuous;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID controller to consider the input to be continuous, 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.
*/
public void setContinuous() {
setContinuous(true);
}
/**
* Sets the maximum and minimum values expected from the input and setpoint.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the input
*/
public void setInputRange(double minimumInput, double maximumInput) {
m_thisMutex.lock();
try {
if (minimumInput > maximumInput) {
throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
} finally {
m_thisMutex.unlock();
}
setSetpoint(m_setpoint);
}
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput the minimum percentage to write to the output
* @param maximumOutput the maximum percentage to write to the output
*/
public void setOutputRange(double minimumOutput, double maximumOutput) {
m_thisMutex.lock();
try {
if (minimumOutput > maximumOutput) {
throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the setpoint for the PIDController.
*
* @param setpoint the desired setpoint
*/
@Override
public void setSetpoint(double setpoint) {
m_thisMutex.lock();
try {
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput) {
m_setpoint = m_maximumInput;
} else if (setpoint < m_minimumInput) {
m_setpoint = m_minimumInput;
} else {
m_setpoint = setpoint;
}
} else {
m_setpoint = setpoint;
}
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current setpoint of the PIDController.
*
* @return the current setpoint
*/
@Override
public double getSetpoint() {
m_thisMutex.lock();
try {
return m_setpoint;
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the change in setpoint over time of the PIDController.
*
* @return the change in setpoint over time
*/
public double getDeltaSetpoint() {
m_thisMutex.lock();
try {
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current difference of the input from the setpoint.
*
* @return the current error
*/
@Override
public double getError() {
m_thisMutex.lock();
try {
return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet()));
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current difference of the error over the past few iterations. You can specify the
* number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
* used for the onTarget() function.
*
* @deprecated Use getError(), which is now already filtered.
* @return the current average of the error
*/
@Deprecated
public double getAvgError() {
m_thisMutex.lock();
try {
return getError();
} finally {
m_thisMutex.unlock();
}
}
/**
* Sets what type of input the PID controller will use.
*
* @param pidSource the type of input
*/
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidInput.setPIDSourceType(pidSource);
}
/**
* Returns the type of input the PID controller is using.
*
* @return the PID controller input type
*/
public PIDSourceType getPIDSourceType() {
return m_pidInput.getPIDSourceType();
}
/**
* Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
* the range or as an absolute value. The Tolerance object encapsulates those options in an
* object. Use it by creating the type of tolerance that you want to use: setTolerance(new
* PIDController.AbsoluteTolerance(0.1))
*
* @deprecated Use setPercentTolerance() instead.
* @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
* AbsoluteTolerance
*/
@Deprecated
public void setTolerance(Tolerance tolerance) {
m_tolerance = tolerance;
}
/**
* Set the absolute error which is considered tolerable for use with OnTarget.
*
* @param absvalue absolute error which is tolerable in the units of the input object
*/
public void setAbsoluteTolerance(double absvalue) {
m_thisMutex.lock();
try {
m_tolerance = new AbsoluteTolerance(absvalue);
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
* 15 percent)
*
* @param percentage percent error which is tolerable
*/
public void setPercentTolerance(double percentage) {
m_thisMutex.lock();
try {
m_tolerance = new PercentageTolerance(percentage);
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the number of previous error samples to average for tolerancing. When determining whether a
* mechanism is on target, the user may want to use a rolling average of previous measurements
* instead of a precise position or velocity. This is useful for noisy sensors which return a few
* erroneous measurements when the mechanism is on target. However, the mechanism will not
* register as on target for at least the specified bufLength cycles.
*
* @deprecated Use a LinearFilter as the input.
* @param bufLength Number of previous cycles to average.
*/
@Deprecated
public void setToleranceBuffer(int bufLength) {
m_thisMutex.lock();
try {
m_filter = LinearFilter.movingAverage(bufLength);
} finally {
m_thisMutex.unlock();
}
}
/**
* 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.
*
* @return true if the error is less than the tolerance
*/
public boolean onTarget() {
m_thisMutex.lock();
try {
return m_tolerance.onTarget();
} finally {
m_thisMutex.unlock();
}
}
/** Reset the previous error, the integral term, and disable the controller. */
@Override
public void reset() {
m_thisMutex.lock();
try {
m_prevError = 0;
m_totalError = 0;
m_result = 0;
} finally {
m_thisMutex.unlock();
}
}
/**
* Passes the output directly to setSetpoint().
*
* <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
* In that case, the output of the parent controller becomes the input (i.e., the reference) of
* the child.
*
* <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
*/
@Override
public void pidWrite(double output) {
setSetpoint(output);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("PIDController");
builder.setSafeState(this::reset);
builder.addDoubleProperty("p", this::getP, this::setP);
builder.addDoubleProperty("i", this::getI, this::setI);
builder.addDoubleProperty("d", this::getD, this::setD);
builder.addDoubleProperty("f", this::getF, this::setF);
builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
}
/**
* Wraps error around for continuous inputs. The original error is returned if continuous mode is
* disabled. This is an unsynchronized function.
*
* @param error The current error of the PID controller.
* @return Error for continuous inputs.
*/
protected double getContinuousError(double error) {
if (m_continuous && m_inputRange > 0) {
error %= m_inputRange;
if (Math.abs(error) > m_inputRange / 2) {
if (error > 0) {
return error - m_inputRange;
} else {
return error + m_inputRange;
}
}
}
return error;
}
private static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
}

172
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDController.java Normal file
View File

@@ -0,0 +1,172 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
/**
* Class implements a PID Control Loop.
*
* <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
* calculations, as well as writing the given PIDOutput.
*
* <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
* and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
* given set of PID constants.
*
* @deprecated Use {@link edu.wpi.first.wpilibj.controller.PIDController} instead.
*/
@Deprecated(since = "2020", forRemoval = true)
public class PIDController extends PIDBase implements Controller {
Notifier m_controlLoop = new Notifier(this::calculate);
/**
* Allocate a PID object with the given constants for P, I, D, and F.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
* @param period the loop time for doing calculations in seconds. This particularly affects
* calculations of the integral and differential terms. The default is 0.05 (50ms).
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp,
double Ki,
double Kd,
double Kf,
PIDSource source,
PIDOutput output,
double period) {
super(Kp, Ki, Kd, Kf, source, output);
m_controlLoop.startPeriodic(period);
}
/**
* Allocate a PID object with the given constants for P, I, D and period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source the PIDSource object that is used to get values
* @param output the PIDOutput object that is set to the output percentage
* @param period the loop time for doing calculations in seconds. This particularly affects
* calculations of the integral and differential terms. The default is 0.05 (50ms).
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
this(Kp, Ki, Kd, 0.0, source, output, period);
}
/**
* Allocate a PID object with the given constants for P, I, D, using a 50ms period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, source, output, kDefaultPeriod);
}
/**
* Allocate a PID object with the given constants for P, I, D, using a 50ms period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
}
@Override
public void close() {
m_controlLoop.close();
m_thisMutex.lock();
try {
m_pidOutput = null;
m_pidInput = null;
m_controlLoop = null;
} finally {
m_thisMutex.unlock();
}
}
/** Begin running the PIDController. */
@Override
public void enable() {
m_thisMutex.lock();
try {
m_enabled = true;
} finally {
m_thisMutex.unlock();
}
}
/** Stop running the PIDController, this sets the output to zero before stopping. */
@Override
public void disable() {
// Ensures m_enabled check and pidWrite() call occur atomically
m_pidWriteMutex.lock();
try {
m_thisMutex.lock();
try {
m_enabled = false;
} finally {
m_thisMutex.unlock();
}
m_pidOutput.pidWrite(0);
} finally {
m_pidWriteMutex.unlock();
}
}
/** Set the enabled state of the PIDController. */
public void setEnabled(boolean enable) {
if (enable) {
enable();
} else {
disable();
}
}
/** Return true if PIDController is enabled. */
public boolean isEnabled() {
m_thisMutex.lock();
try {
return m_enabled;
} finally {
m_thisMutex.unlock();
}
}
/** Reset the previous error, the integral term, and disable the controller. */
@Override
public void reset() {
disable();
super.reset();
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
}
}

25
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
@Deprecated(since = "2020", forRemoval = true)
@SuppressWarnings("SummaryJavadoc")
public interface PIDInterface {
void setPID(double p, double i, double d);
double getP();
double getI();
double getD();
void setSetpoint(double setpoint);
double getSetpoint();
double getError();
void reset();
}

21
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java Normal file
View File

@@ -0,0 +1,21 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
/**
* This interface allows PIDController to write it's results to its output.
*
* @deprecated Use DoubleConsumer and new PIDController class.
*/
@FunctionalInterface
@Deprecated(since = "2020", forRemoval = true)
public interface PIDOutput {
/**
* Set the output to the value calculated by PIDController.
*
* @param output the value calculated by PIDController
*/
void pidWrite(double output);
}

34
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSource.java Normal file
View File

@@ -0,0 +1,34 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
/**
* This interface allows for PIDController to automatically read from this object.
*
* @deprecated Use DoubleSupplier and new PIDController class.
*/
@Deprecated(since = "2020", forRemoval = true)
public interface PIDSource {
/**
* Set which parameter of the device you are using as a process control variable.
*
* @param pidSource An enum to select the parameter.
*/
void setPIDSourceType(PIDSourceType pidSource);
/**
* Get which parameter of the device you are using as a process control variable.
*
* @return the currently selected PID source parameter
*/
PIDSourceType getPIDSourceType();
/**
* Get the result to use in PIDController.
*
* @return the result to use in PIDController
*/
double pidGet();
}

12
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java Normal file
View File

@@ -0,0 +1,12 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
/** A description for the type of output value to provide to a PIDController. */
@Deprecated(since = "2020", forRemoval = true)
public enum PIDSourceType {
kDisplacement,
kRate
}

358
wpilibOldCommands/src/main/native/cpp/PIDBase.cpp Normal file
View File

@@ -0,0 +1,358 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/PIDBase.h"
#include <algorithm>
#include <cmath>
#include <hal/FRCUsageReporting.h>
#include "frc/PIDOutput.h"
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
template <class T>
constexpr const T& clamp(const T& value, const T& low, const T& high) {
return std::max(low, std::min(value, high));
}
PIDBase::PIDBase(double Kp, double Ki, double Kd, PIDSource& source,
PIDOutput& output)
: PIDBase(Kp, Ki, Kd, 0.0, source, output) {}
PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
PIDOutput& output) {
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_pidInput = &source;
m_filter = LinearFilter<double>::MovingAverage(1);
m_pidOutput = &output;
m_setpointTimer.Start();
static int instances = 0;
instances++;
HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
SendableRegistry::GetInstance().Add(this, "PIDController", instances);
}
double PIDBase::Get() const {
std::scoped_lock lock(m_thisMutex);
return m_result;
}
void PIDBase::SetContinuous(bool continuous) {
std::scoped_lock lock(m_thisMutex);
m_continuous = continuous;
}
void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
{
std::scoped_lock lock(m_thisMutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
}
SetSetpoint(m_setpoint);
}
void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
std::scoped_lock lock(m_thisMutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
void PIDBase::SetPID(double p, double i, double d) {
{
std::scoped_lock lock(m_thisMutex);
m_P = p;
m_I = i;
m_D = d;
}
}
void PIDBase::SetPID(double p, double i, double d, double f) {
std::scoped_lock lock(m_thisMutex);
m_P = p;
m_I = i;
m_D = d;
m_F = f;
}
void PIDBase::SetP(double p) {
std::scoped_lock lock(m_thisMutex);
m_P = p;
}
void PIDBase::SetI(double i) {
std::scoped_lock lock(m_thisMutex);
m_I = i;
}
void PIDBase::SetD(double d) {
std::scoped_lock lock(m_thisMutex);
m_D = d;
}
void PIDBase::SetF(double f) {
std::scoped_lock lock(m_thisMutex);
m_F = f;
}
double PIDBase::GetP() const {
std::scoped_lock lock(m_thisMutex);
return m_P;
}
double PIDBase::GetI() const {
std::scoped_lock lock(m_thisMutex);
return m_I;
}
double PIDBase::GetD() const {
std::scoped_lock lock(m_thisMutex);
return m_D;
}
double PIDBase::GetF() const {
std::scoped_lock lock(m_thisMutex);
return m_F;
}
void PIDBase::SetSetpoint(double setpoint) {
{
std::scoped_lock lock(m_thisMutex);
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput) {
m_setpoint = m_maximumInput;
} else if (setpoint < m_minimumInput) {
m_setpoint = m_minimumInput;
} else {
m_setpoint = setpoint;
}
} else {
m_setpoint = setpoint;
}
}
}
double PIDBase::GetSetpoint() const {
std::scoped_lock lock(m_thisMutex);
return m_setpoint;
}
double PIDBase::GetDeltaSetpoint() const {
std::scoped_lock lock(m_thisMutex);
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
}
double PIDBase::GetError() const {
double setpoint = GetSetpoint();
{
std::scoped_lock lock(m_thisMutex);
return GetContinuousError(setpoint - m_pidInput->PIDGet());
}
}
double PIDBase::GetAvgError() const {
return GetError();
}
void PIDBase::SetPIDSourceType(PIDSourceType pidSource) {
m_pidInput->SetPIDSourceType(pidSource);
}
PIDSourceType PIDBase::GetPIDSourceType() const {
return m_pidInput->GetPIDSourceType();
}
void PIDBase::SetTolerance(double percent) {
std::scoped_lock lock(m_thisMutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
void PIDBase::SetAbsoluteTolerance(double absTolerance) {
std::scoped_lock lock(m_thisMutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
void PIDBase::SetPercentTolerance(double percent) {
std::scoped_lock lock(m_thisMutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
void PIDBase::SetToleranceBuffer(int bufLength) {
std::scoped_lock lock(m_thisMutex);
m_filter = LinearFilter<double>::MovingAverage(bufLength);
}
bool PIDBase::OnTarget() const {
double error = GetError();
std::scoped_lock lock(m_thisMutex);
switch (m_toleranceType) {
case kPercentTolerance:
return std::fabs(error) < m_tolerance / 100 * m_inputRange;
break;
case kAbsoluteTolerance:
return std::fabs(error) < m_tolerance;
break;
case kNoTolerance:
// TODO: this case needs an error
return false;
}
return false;
}
void PIDBase::Reset() {
std::scoped_lock lock(m_thisMutex);
m_prevError = 0;
m_totalError = 0;
m_result = 0;
}
void PIDBase::PIDWrite(double output) {
SetSetpoint(output);
}
void PIDBase::InitSendable(SendableBuilder& builder) {
builder.SetSmartDashboardType("PIDController");
builder.SetSafeState([=]() { Reset(); });
builder.AddDoubleProperty(
"p", [=]() { return GetP(); }, [=](double value) { SetP(value); });
builder.AddDoubleProperty(
"i", [=]() { return GetI(); }, [=](double value) { SetI(value); });
builder.AddDoubleProperty(
"d", [=]() { return GetD(); }, [=](double value) { SetD(value); });
builder.AddDoubleProperty(
"f", [=]() { return GetF(); }, [=](double value) { SetF(value); });
builder.AddDoubleProperty(
"setpoint", [=]() { return GetSetpoint(); },
[=](double value) { SetSetpoint(value); });
}
void PIDBase::Calculate() {
if (m_pidInput == nullptr || m_pidOutput == nullptr) {
return;
}
bool enabled;
{
std::scoped_lock lock(m_thisMutex);
enabled = m_enabled;
}
if (enabled) {
double input;
// Storage for function inputs
PIDSourceType pidSourceType;
double P;
double I;
double D;
double feedForward = CalculateFeedForward();
double minimumOutput;
double maximumOutput;
// Storage for function input-outputs
double prevError;
double error;
double totalError;
{
std::scoped_lock lock(m_thisMutex);
input = m_filter.Calculate(m_pidInput->PIDGet());
pidSourceType = m_pidInput->GetPIDSourceType();
P = m_P;
I = m_I;
D = m_D;
minimumOutput = m_minimumOutput;
maximumOutput = m_maximumOutput;
prevError = m_prevError;
error = GetContinuousError(m_setpoint - input);
totalError = m_totalError;
}
// Storage for function outputs
double result;
if (pidSourceType == PIDSourceType::kRate) {
if (P != 0) {
totalError =
clamp(totalError + error, minimumOutput / P, maximumOutput / P);
}
result = D * error + P * totalError + feedForward;
} else {
if (I != 0) {
totalError =
clamp(totalError + error, minimumOutput / I, maximumOutput / I);
}
result =
P * error + I * totalError + D * (error - prevError) + feedForward;
}
result = clamp(result, minimumOutput, maximumOutput);
{
// Ensures m_enabled check and PIDWrite() call occur atomically
std::scoped_lock pidWriteLock(m_pidWriteMutex);
std::unique_lock mainLock(m_thisMutex);
if (m_enabled) {
// Don't block other PIDBase operations on PIDWrite()
mainLock.unlock();
m_pidOutput->PIDWrite(result);
}
}
std::scoped_lock lock(m_thisMutex);
m_prevError = m_error;
m_error = error;
m_totalError = totalError;
m_result = result;
}
}
double PIDBase::CalculateFeedForward() {
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
return m_F * GetSetpoint();
} else {
double temp = m_F * GetDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.Reset();
return temp;
}
}
double PIDBase::GetContinuousError(double error) const {
if (m_continuous && m_inputRange != 0) {
error = std::fmod(error, m_inputRange);
if (std::fabs(error) > m_inputRange / 2) {
if (error > 0) {
return error - m_inputRange;
} else {
return error + m_inputRange;
}
}
}
return error;
}

83
wpilibOldCommands/src/main/native/cpp/PIDController.cpp Normal file
View File

@@ -0,0 +1,83 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/PIDController.h"
#include "frc/Notifier.h"
#include "frc/PIDOutput.h"
#include "frc/smartdashboard/SendableBuilder.h"
using namespace frc;
PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
PIDOutput* output, double period)
: PIDController(Kp, Ki, Kd, 0.0, *source, *output, period) {}
PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
PIDSource* source, PIDOutput* output,
double period)
: PIDController(Kp, Ki, Kd, Kf, *source, *output, period) {}
PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source,
PIDOutput& output, double period)
: PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
PIDSource& source, PIDOutput& output,
double period)
: PIDBase(Kp, Ki, Kd, Kf, source, output) {
m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_controlLoop->StartPeriodic(units::second_t(period));
}
PIDController::~PIDController() {
// Forcefully stopping the notifier so the callback can successfully run.
m_controlLoop->Stop();
}
void PIDController::Enable() {
{
std::scoped_lock lock(m_thisMutex);
m_enabled = true;
}
}
void PIDController::Disable() {
{
// Ensures m_enabled modification and PIDWrite() call occur atomically
std::scoped_lock pidWriteLock(m_pidWriteMutex);
{
std::scoped_lock mainLock(m_thisMutex);
m_enabled = false;
}
m_pidOutput->PIDWrite(0);
}
}
void PIDController::SetEnabled(bool enable) {
if (enable) {
Enable();
} else {
Disable();
}
}
bool PIDController::IsEnabled() const {
std::scoped_lock lock(m_thisMutex);
return m_enabled;
}
void PIDController::Reset() {
Disable();
PIDBase::Reset();
}
void PIDController::InitSendable(SendableBuilder& builder) {
PIDBase::InitSendable(builder);
builder.AddBooleanProperty(
"enabled", [=]() { return IsEnabled(); },
[=](bool value) { SetEnabled(value); });
}

15
wpilibOldCommands/src/main/native/cpp/PIDSource.cpp Normal file
View File

@@ -0,0 +1,15 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/PIDSource.h"
using namespace frc;
void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
PIDSourceType PIDSource::GetPIDSourceType() const {
return m_pidSource;
}

407
wpilibOldCommands/src/main/native/include/frc/PIDBase.h Normal file
View File

@@ -0,0 +1,407 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <string>
#include <wpi/deprecated.h>
#include <wpi/mutex.h>
#include "frc/Base.h"
#include "frc/LinearFilter.h"
#include "frc/PIDInterface.h"
#include "frc/PIDOutput.h"
#include "frc/PIDSource.h"
#include "frc/Timer.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
class SendableBuilder;
/**
* Class implements a PID Control Loop.
*
* Creates a separate thread which reads the given PIDSource and takes care of
* the integral calculations, as well as writing the given PIDOutput.
*
* This feedback controller runs in discrete time, so time deltas are not used
* in the integral and derivative calculations. Therefore, the sample rate
* affects the controller's behavior for a given set of PID constants.
*
* @deprecated All APIs which use this have been deprecated.
*/
class PIDBase : public PIDInterface,
public PIDOutput,
public Sendable,
public SendableHelper<PIDBase> {
public:
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
*/
WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
*/
WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDBase(double p, double i, double d, double f, PIDSource& source,
PIDOutput& output);
~PIDBase() override = default;
/**
* Return the current PID result.
*
* This is always centered on zero and constrained the the max and min outs.
*
* @return the latest calculated output
*/
virtual double Get() const;
/**
* Set the PID controller to consider the input to be continuous,
*
* 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.
*
* @param continuous true turns on continuous, false turns off continuous
*/
virtual void SetContinuous(bool continuous = true);
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
virtual void SetInputRange(double minimumInput, double maximumInput);
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput the minimum value to write to the output
* @param maximumOutput the maximum value to write to the output
*/
virtual void SetOutputRange(double minimumOutput, double maximumOutput);
/**
* Set the PID Controller gain parameters.
*
* Set the proportional, integral, and differential coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
void SetPID(double p, double i, double d) override;
/**
* Set the PID Controller gain parameters.
*
* Set the proportional, integral, and differential coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
* @param f Feed forward coefficient
*/
virtual void SetPID(double p, double i, double d, double f);
/**
* Set the Proportional coefficient of the PID controller gain.
*
* @param p proportional coefficient
*/
void SetP(double p);
/**
* Set the Integral coefficient of the PID controller gain.
*
* @param i integral coefficient
*/
void SetI(double i);
/**
* Set the Differential coefficient of the PID controller gain.
*
* @param d differential coefficient
*/
void SetD(double d);
/**
* Get the Feed forward coefficient of the PID controller gain.
*
* @param f Feed forward coefficient
*/
void SetF(double f);
/**
* Get the Proportional coefficient.
*
* @return proportional coefficient
*/
double GetP() const override;
/**
* Get the Integral coefficient.
*
* @return integral coefficient
*/
double GetI() const override;
/**
* Get the Differential coefficient.
*
* @return differential coefficient
*/
double GetD() const override;
/**
* Get the Feed forward coefficient.
*
* @return Feed forward coefficient
*/
virtual double GetF() const;
/**
* Set the setpoint for the PIDBase.
*
* @param setpoint the desired setpoint
*/
void SetSetpoint(double setpoint) override;
/**
* Returns the current setpoint of the PIDBase.
*
* @return the current setpoint
*/
double GetSetpoint() const override;
/**
* Returns the change in setpoint over time of the PIDBase.
*
* @return the change in setpoint over time
*/
double GetDeltaSetpoint() const;
/**
* Returns the current difference of the input from the setpoint.
*
* @return the current error
*/
virtual double GetError() const;
/**
* Returns the current average of the error over the past few iterations.
*
* You can specify the number of iterations to average with
* SetToleranceBuffer() (defaults to 1). This is the same value that is used
* for OnTarget().
*
* @return the average error
*/
WPI_DEPRECATED("Use a LinearFilter as the input and GetError().")
virtual double GetAvgError() const;
/**
* Sets what type of input the PID controller will use.
*/
virtual void SetPIDSourceType(PIDSourceType pidSource);
/**
* Returns the type of input the PID controller is using.
*
* @return the PID controller input type
*/
virtual PIDSourceType GetPIDSourceType() const;
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget.
*
* @param percentage error which is tolerable
*/
WPI_DEPRECATED("Use SetPercentTolerance() instead.")
virtual void SetTolerance(double percent);
/**
* Set the absolute error which is considered tolerable for use with
* OnTarget.
*
* @param percentage error which is tolerable
*/
virtual void SetAbsoluteTolerance(double absValue);
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget.
*
* @param percentage error which is tolerable
*/
virtual void SetPercentTolerance(double percentValue);
/**
* Set the number of previous error samples to average for tolerancing. When
* determining whether a mechanism is on target, the user may want to use a
* rolling average of previous measurements instead of a precise position or
* velocity. This is useful for noisy sensors which return a few erroneous
* measurements when the mechanism is on target. However, the mechanism will
* not register as on target for at least the specified bufLength cycles.
*
* @param bufLength Number of previous cycles to average. Defaults to 1.
*/
WPI_DEPRECATED("Use a LinearDigitalFilter as the input.")
virtual void SetToleranceBuffer(int buf = 1);
/**
* Return 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.
*
* Currently this just reports on target as the actual value passes through
* the setpoint. Ideally it should be based on being within the tolerance for
* some period of time.
*
* This will return false until at least one input value has been computed.
*/
virtual bool OnTarget() const;
/**
* Reset the previous error, the integral term, and disable the controller.
*/
void Reset() override;
/**
* Passes the output directly to SetSetpoint().
*
* PIDControllers can be nested by passing a PIDController as another
* PIDController's output. In that case, the output of the parent controller
* becomes the input (i.e., the reference) of the child.
*
* It is the caller's responsibility to put the data into a valid form for
* SetSetpoint().
*/
void PIDWrite(double output) override;
void InitSendable(SendableBuilder& builder) override;
protected:
// Is the pid controller enabled
bool m_enabled = false;
mutable wpi::mutex m_thisMutex;
// Ensures when Disable() is called, PIDWrite() won't run if Calculate()
// is already running at that time.
mutable wpi::mutex m_pidWriteMutex;
PIDSource* m_pidInput;
PIDOutput* m_pidOutput;
Timer m_setpointTimer;
/**
* Read the input, calculate the output accordingly, and write to the output.
* This should only be called by the Notifier.
*/
virtual void Calculate();
/**
* Calculate the feed forward term.
*
* Both of the provided feed forward calculations are velocity feed forwards.
* If a different feed forward calculation is desired, the user can override
* this function and provide his or her own. This function does no
* synchronization because the PIDBase class only calls it in synchronized
* code, so be careful if calling it oneself.
*
* If a velocity PID controller is being used, the F term should be set to 1
* over the maximum setpoint for the output. If a position PID controller is
* being used, the F term should be set to 1 over the maximum speed for the
* output measured in setpoint units per this controller's update period (see
* the default period in this class's constructor).
*/
virtual double CalculateFeedForward();
/**
* Wraps error around for continuous inputs. The original error is returned if
* continuous mode is disabled. This is an unsynchronized function.
*
* @param error The current error of the PID controller.
* @return Error for continuous inputs.
*/
double GetContinuousError(double error) const;
private:
// Factor for "proportional" control
double m_P;
// Factor for "integral" control
double m_I;
// Factor for "derivative" control
double m_D;
// Factor for "feed forward" control
double m_F;
// |maximum output|
double m_maximumOutput = 1.0;
// |minimum output|
double m_minimumOutput = -1.0;
// Maximum input - limit setpoint to this
double m_maximumInput = 0;
// Minimum input - limit setpoint to this
double m_minimumInput = 0;
// input range - difference between maximum and minimum
double m_inputRange = 0;
// Do the endpoints wrap around? eg. Absolute encoder
bool m_continuous = false;
// The prior error (used to compute velocity)
double m_prevError = 0;
// The sum of the errors for use in the integral calc
double m_totalError = 0;
enum {
kAbsoluteTolerance,
kPercentTolerance,
kNoTolerance
} m_toleranceType = kNoTolerance;
// The percetage or absolute error that is considered on target.
double m_tolerance = 0.05;
double m_setpoint = 0;
double m_prevSetpoint = 0;
double m_error = 0;
double m_result = 0;
LinearFilter<double> m_filter{{}, {}};
};
} // namespace frc

136
wpilibOldCommands/src/main/native/include/frc/PIDController.h Normal file
View File

@@ -0,0 +1,136 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <string>
#include <wpi/deprecated.h>
#include <wpi/mutex.h>
#include "frc/Base.h"
#include "frc/Controller.h"
#include "frc/Notifier.h"
#include "frc/PIDBase.h"
#include "frc/PIDSource.h"
#include "frc/Timer.h"
namespace frc {
class PIDOutput;
/**
* Class implements a PID Control Loop.
*
* Creates a separate thread which reads the given PIDSource and takes care of
* the integral calculations, as well as writing the given PIDOutput.
*
* This feedback controller runs in discrete time, so time deltas are not used
* in the integral and derivative calculations. Therefore, the sample rate
* affects the controller's behavior for a given set of PID constants.
*
* @deprecated Use frc2::PIDController class instead.
*/
class PIDController : public PIDBase, public Controller {
public:
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, PIDSource* source,
PIDOutput* output, double period = 0.05);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, double f, PIDSource* source,
PIDOutput* output, double period = 0.05);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, PIDSource& source,
PIDOutput& output, double period = 0.05);
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations in seconds. This
* particularly affects calculations of the integral and
* differential terms. The default is 0.05 (50ms).
*/
WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, double f, PIDSource& source,
PIDOutput& output, double period = 0.05);
~PIDController() override;
/**
* Begin running the PIDController.
*/
void Enable() override;
/**
* Stop running the PIDController, this sets the output to zero before
* stopping.
*/
void Disable() override;
/**
* Set the enabled state of the PIDController.
*/
void SetEnabled(bool enable);
/**
* Return true if PIDController is enabled.
*/
bool IsEnabled() const;
/**
* Reset the previous error, the integral term, and disable the controller.
*/
void Reset() override;
void InitSendable(SendableBuilder& builder) override;
private:
std::unique_ptr<Notifier> m_controlLoop;
};
} // namespace frc

34
wpilibOldCommands/src/main/native/include/frc/PIDInterface.h Normal file
View File

@@ -0,0 +1,34 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/deprecated.h>
namespace frc {
/**
* Interface for PID Control Loop.
*
* @deprecated All APIs which use this have been deprecated.
*/
class PIDInterface {
public:
WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDInterface() = default;
PIDInterface(PIDInterface&&) = default;
PIDInterface& operator=(PIDInterface&&) = default;
virtual void SetPID(double p, double i, double d) = 0;
virtual double GetP() const = 0;
virtual double GetI() const = 0;
virtual double GetD() const = 0;
virtual void SetSetpoint(double setpoint) = 0;
virtual double GetSetpoint() const = 0;
virtual void Reset() = 0;
};
} // namespace frc

22
wpilibOldCommands/src/main/native/include/frc/PIDOutput.h Normal file
View File

@@ -0,0 +1,22 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/Base.h"
namespace frc {
/**
* PIDOutput interface is a generic output for the PID class.
*
* PWMs use this class. Users implement this interface to allow for a
* PIDController to read directly from the inputs.
*/
class PIDOutput {
public:
virtual void PIDWrite(double output) = 0;
};
} // namespace frc

36
wpilibOldCommands/src/main/native/include/frc/PIDSource.h Normal file
View File

@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
namespace frc {
enum class PIDSourceType { kDisplacement, kRate };
/**
* PIDSource interface is a generic sensor source for the PID class.
*
* All sensors that can be used with the PID class will implement the PIDSource
* that returns a standard value that will be used in the PID code.
*/
class PIDSource {
public:
virtual ~PIDSource() = default;
/**
* Set which parameter you are using as a process control variable.
*
* @param pidSource An enum to select the parameter.
*/
virtual void SetPIDSourceType(PIDSourceType pidSource);
virtual PIDSourceType GetPIDSourceType() const;
virtual double PIDGet() = 0;
protected:
PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
};
} // namespace frc