Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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

23
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
View File

@@ -16,12 +16,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* through the sensor. Many sensors have multiple axis and can be treated as multiple devices. Each
* is calibrated by finding the center value over a period of time.
*/
public class AnalogAccelerometer implements PIDSource, Sendable, AutoCloseable {
public class AnalogAccelerometer implements Sendable, AutoCloseable {
private AnalogInput m_analogChannel;
private double m_voltsPerG = 1.0;
private double m_zeroGVoltage = 2.5;
private final boolean m_allocatedChannel;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/** Common initialization. */
private void initAccelerometer() {
@@ -108,26 +107,6 @@ public class AnalogAccelerometer implements PIDSource, Sendable, AutoCloseable {
m_zeroGVoltage = zero;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the Acceleration for the PID Source parent.
*
* @return The current acceleration in Gs.
*/
@Override
public double pidGet() {
return getAcceleration();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Accelerometer");

23
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
View File

@@ -25,13 +25,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* accumulated effectively increasing the resolution, while the averaged samples are divided by the
* number of samples to retain the resolution, but get more stable values.
*/
public class AnalogInput implements PIDSource, Sendable, AutoCloseable {
public class AnalogInput implements Sendable, AutoCloseable {
private static final int kAccumulatorSlot = 1;
int m_port; // explicit no modifier, private and package accessible.
private int m_channel;
private static final int[] kAccumulatorChannels = {0, 1};
private long m_accumulatorOffset;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Construct an analog channel.
@@ -322,26 +321,6 @@ public class AnalogInput implements PIDSource, Sendable, AutoCloseable {
return AnalogJNI.getAnalogSampleRate();
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the average voltage for use with PIDController.
*
* @return the average voltage
*/
@Override
public double pidGet() {
return getAverageVoltage();
}
/**
* Indicates this input is used by a simulated device.
*

24
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
View File

@@ -18,7 +18,6 @@ public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseab
private boolean m_initAnalogInput;
private double m_fullRange;
private double m_offset;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* AnalogPotentiometer constructor.
@@ -129,29 +128,6 @@ public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseab
+ m_offset;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current reading.
*/
@Override
public double pidGet() {
return get();
}
@Override
public void initSendable(SendableBuilder builder) {
if (m_analogInput != null) {

36
wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
View File

@@ -26,7 +26,7 @@ import java.nio.ByteOrder;
* <p>All counters will immediately start counting - reset() them if you need them to be zeroed
* before use.
*/
public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable {
public class Counter implements CounterBase, Sendable, AutoCloseable {
/** Mode determines how and what the counter counts. */
public enum Mode {
/** mode: two pulse. */
@@ -51,7 +51,6 @@ public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable
private boolean m_allocatedDownSource;
private int m_counter; // /< The FPGA counter object.
private int m_index; // /< The index of this counter.
private PIDSourceType m_pidSource;
private double m_distancePerPulse; // distance of travel for each tick
/** Create an instance of a counter with the given mode. */
@@ -507,39 +506,6 @@ public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable
m_distancePerPulse = distancePerPulse;
}
/**
* Set which parameter of the encoder you are using as a process control variable. The counter
* class supports the rate and distance parameters.
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
requireNonNullParam(pidSource, "pidSource", "setPIDSourceType");
if (pidSource != PIDSourceType.kDisplacement && pidSource != PIDSourceType.kRate) {
throw new IllegalArgumentException("PID Source parameter was not valid type: " + pidSource);
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
@Override
public double pidGet() {
switch (m_pidSource) {
case kDisplacement:
return getDistance();
case kRate:
return getRate();
default:
return 0.0;
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Counter");

38
wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
View File

@@ -27,7 +27,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <p>All encoders will immediately start counting - reset() them if you need them to be zeroed
* before use.
*/
public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable {
public class Encoder implements CounterBase, Sendable, AutoCloseable {
public enum IndexingType {
kResetWhileHigh(0),
kResetWhileLow(1),
@@ -51,7 +51,6 @@ public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable
private boolean m_allocatedA;
private boolean m_allocatedB;
private boolean m_allocatedI;
private PIDSourceType m_pidSource;
private int m_encoder; // the HAL encoder object
@@ -73,8 +72,6 @@ public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable
reverseDirection,
type.value);
m_pidSource = PIDSourceType.kDisplacement;
int fpgaIndex = getFPGAIndex();
HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1);
SendableRegistry.addLW(this, "Encoder", fpgaIndex);
@@ -481,39 +478,6 @@ public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable
return EncoderJNI.getEncoderSamplesToAverage(m_encoder);
}
/**
* Set which parameter of the encoder you are using as a process control variable. The encoder
* class supports the rate and distance parameters.
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current value of the selected source parameter.
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kDisplacement:
return getDistance();
case kRate:
return getRate();
default:
return 0.0;
}
}
/**
* Set the index source for the encoder. When this source is activated, the encoder count
* automatically resets.

38
wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
View File

@@ -8,43 +8,7 @@ import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
/** GyroBase is the common base class for Gyro implementations such as AnalogGyro. */
public abstract class GyroBase implements Gyro, PIDSource, Sendable {
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Set which parameter of the gyro you are using as a process control variable. The Gyro class
* supports the rate and displacement parameters
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the output of the gyro for use with PIDControllers. May be the angle or rate depending on
* the set PIDSourceType
*
* @return the output according to the gyro
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kRate:
return getRate();
case kDisplacement:
return getAngle();
default:
return 0.0;
}
}
public abstract class GyroBase implements Gyro, Sendable {
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
View File

@@ -89,16 +89,6 @@ public class NidecBrushless extends MotorSafety
return m_isInverted;
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
@Override
public void pidWrite(double output) {
set(output);
}
/**
* Stop the motor. This is called by the MotorSafety object when it has a timeout for this PWM and
* needs to stop it from running. Calling set() will re-enable the motor.

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

@@ -1,819 +0,0 @@
// 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
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
View File

@@ -1,172 +0,0 @@
// 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
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
View File

@@ -1,25 +0,0 @@
// 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
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
View File

@@ -1,21 +0,0 @@
// 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
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
View File

@@ -1,34 +0,0 @@
// 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
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
View File

@@ -1,12 +0,0 @@
// 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
}

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java
View File

@@ -92,16 +92,6 @@ public abstract class PWMSpeedController extends MotorSafety
return m_pwm.getChannel();
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
@Override
public void pidWrite(double output) {
set(output);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Speed Controller");

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
View File

@@ -5,8 +5,7 @@
package edu.wpi.first.wpilibj;
/** Interface for speed controlling devices. */
@SuppressWarnings("removal")
public interface SpeedController extends PIDOutput {
public interface SpeedController {
/**
* Common interface for setting the speed of a speed controller.
*

5
wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
View File

@@ -87,11 +87,6 @@ public class SpeedControllerGroup implements SpeedController, Sendable, AutoClos
}
}
@Override
public void pidWrite(double output) {
set(output);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Speed Controller");

95
wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
View File

@@ -26,15 +26,7 @@ import java.util.List;
* echo is received. The time that the line is high determines the round trip distance (time of
* flight).
*/
public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
/** The units to return when PIDGet is called. */
public enum Unit {
/** Use inches for PIDGet. */
kInches,
/** Use millimeters for PIDGet. */
kMillimeters
}
public class Ultrasonic implements Sendable, AutoCloseable {
// Time (sec) for the ping trigger pulse.
private static final double kPingTime = 10 * 1e-6;
private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
@@ -44,14 +36,12 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
private static volatile boolean m_automaticEnabled;
private DigitalInput m_echoChannel;
private DigitalOutput m_pingChannel;
private boolean m_allocatedChannels;
private final boolean m_allocatedChannels;
private boolean m_enabled;
private Counter m_counter;
// task doing the round-robin automatic sensing
private static Thread m_task;
private Unit m_units;
private static int m_instances;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
private SimDevice m_simDevice;
private SimBoolean m_simRangeValid;
@@ -128,31 +118,16 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
* sending the ping.
* @param echoChannel The digital input channel that receives the echo. The length of time that
* the echo is high represents the round trip time of the ping, and the distance.
* @param units The units returned in either kInches or kMilliMeters
*/
public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
public Ultrasonic(final int pingChannel, final int echoChannel) {
m_pingChannel = new DigitalOutput(pingChannel);
m_echoChannel = new DigitalInput(echoChannel);
SendableRegistry.addChild(this, m_pingChannel);
SendableRegistry.addChild(this, m_echoChannel);
m_allocatedChannels = true;
m_units = units;
initialize();
}
/**
* Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
* and Vex ultrasonic sensors. Default unit is inches.
*
* @param pingChannel The digital output channel that sends the pulse to initiate the sensor
* sending the ping.
* @param echoChannel The digital input channel that receives the echo. The length of time that
* the echo is high represents the round trip time of the ping, and the distance.
*/
public Ultrasonic(final int pingChannel, final int echoChannel) {
this(pingChannel, echoChannel, Unit.kInches);
}
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
* DigitalOutput for the ping channel.
@@ -160,31 +135,17 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
* @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
* 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to determine the range.
* @param units The units returned in either kInches or kMilliMeters
*/
public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel, Unit units) {
public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
requireNonNull(pingChannel, "Provided ping channel was null");
requireNonNull(echoChannel, "Provided echo channel was null");
m_allocatedChannels = false;
m_pingChannel = pingChannel;
m_echoChannel = echoChannel;
m_units = units;
initialize();
}
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
* DigitalOutput for the ping channel. Default unit is inches.
*
* @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
* 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to determine the range.
*/
public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
this(pingChannel, echoChannel, Unit.kInches);
}
/**
* Destructor for the ultrasonic sensor. Delete the instance of the ultrasonic sensor by freeing
* the allocated digital channels. If the system was in automatic mode (round robin), then it is
@@ -326,54 +287,6 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
return getRangeInches() * 25.4;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the range in the current DistanceUnit for the PIDSource base object.
*
* @return The range in DistanceUnit
*/
@Override
public double pidGet() {
switch (m_units) {
case kInches:
return getRangeInches();
case kMillimeters:
return getRangeMM();
default:
return 0.0;
}
}
/**
* Set the current DistanceUnit that should be used for the PIDSource base object.
*
* @param units The DistanceUnit that should be used.
*/
public void setDistanceUnits(Unit units) {
m_units = units;
}
/**
* Get the current DistanceUnit that is used for the PIDSource base object.
*
* @return The type of DistanceUnit that is being used.
*/
public Unit getDistanceUnits() {
return m_units;
}
/**
* Is the ultrasonic enabled.
*

54
wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
View File

@@ -1,54 +0,0 @@
// 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.filters;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* Superclass for filters.
*
* @deprecated This class is no longer used.
*/
@Deprecated(since = "2020", forRemoval = true)
public abstract class Filter implements PIDSource {
private final PIDSource m_source;
public Filter(PIDSource source) {
m_source = source;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_source.setPIDSourceType(pidSource);
}
@Override
public PIDSourceType getPIDSourceType() {
return m_source.getPIDSourceType();
}
@Override
public abstract double pidGet();
/**
* Returns the current filter estimate without also inserting new data as pidGet() would do.
*
* @return The current filter estimate
*/
public abstract double get();
/** Reset the filter state. */
public abstract void reset();
/**
* Calls PIDGet() of source.
*
* @return Current value of source
*/
protected double pidGetSource() {
return m_source.pidGet();
}
}

187
wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
View File

@@ -1,187 +0,0 @@
// 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.filters;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpiutil.CircularBuffer;
import java.util.Arrays;
/**
* This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
* Static factory methods are provided to create commonly used types of filters.
*
* <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
* a2*y[n-2] + ... + aQ*y[n-Q])
*
* <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
* the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
* "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
* front of the feedback term! This is a common convention in signal processing.
*
* <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
* undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
* noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
* impact of these high frequency components. Likewise, a "high pass" filter gets rid of slow-moving
* signal components, letting you detect large changes more easily.
*
* <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
* the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
* wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
* strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
* PID controller out of this class!
*
* <p>For more on filters, I highly recommend the following articles: http://en.wikipedia
* .org/wiki/Linear_filter http://en.wikipedia.org/wiki/Iir_filter http://en.wikipedia
* .org/wiki/Fir_filter
*
* <p>Note 1: PIDGet() should be called by the user on a known, regular period. You can set up a
* Notifier to do this (look at the WPILib PIDController class), or do it "inline" with code in a
* periodic function.
*
* <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
* that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
* then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
* to make sure PIDGet() gets called at the desired, constant frequency!
*
* @deprecated Use LinearFilter class instead.
*/
@Deprecated
public class LinearDigitalFilter extends Filter {
private static int instances;
private final CircularBuffer m_inputs;
private final CircularBuffer m_outputs;
private final double[] m_inputGains;
private final double[] m_outputGains;
/**
* Create a linear FIR or IIR filter.
*
* @param source The PIDSource object that is used to get values
* @param ffGains The "feed forward" or FIR gains
* @param fbGains The "feed back" or IIR gains
*/
public LinearDigitalFilter(PIDSource source, double[] ffGains, double[] fbGains) {
super(source);
m_inputs = new CircularBuffer(ffGains.length);
m_outputs = new CircularBuffer(fbGains.length);
m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
instances++;
HAL.report(tResourceType.kResourceType_LinearFilter, instances);
}
/**
* Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
* gain = e^(-dt / T), T is the time constant in seconds.
*
* <p>This filter is stable for time constants greater than zero.
*
* @param source The PIDSource object that is used to get values
* @param timeConstant The discrete-time time constant in seconds
* @param period The period in seconds between samples taken by the user
*/
public static LinearDigitalFilter singlePoleIIR(
PIDSource source, double timeConstant, double period) {
double gain = Math.exp(-period / timeConstant);
double[] ffGains = {1.0 - gain};
double[] fbGains = {-gain};
return new LinearDigitalFilter(source, ffGains, fbGains);
}
/**
* Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
* gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
*
* <p>This filter is stable for time constants greater than zero.
*
* @param source The PIDSource object that is used to get values
* @param timeConstant The discrete-time time constant in seconds
* @param period The period in seconds between samples taken by the user
*/
public static LinearDigitalFilter highPass(PIDSource source, double timeConstant, double period) {
double gain = Math.exp(-period / timeConstant);
double[] ffGains = {gain, -gain};
double[] fbGains = {-gain};
return new LinearDigitalFilter(source, ffGains, fbGains);
}
/**
* Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
* x[0]).
*
* <p>This filter is always stable.
*
* @param source The PIDSource object that is used to get values
* @param taps The number of samples to average over. Higher = smoother but slower
* @throws IllegalArgumentException if number of taps is less than 1
*/
public static LinearDigitalFilter movingAverage(PIDSource source, int taps) {
if (taps <= 0) {
throw new IllegalArgumentException("Number of taps was not at least 1");
}
double[] ffGains = new double[taps];
for (int i = 0; i < ffGains.length; i++) {
ffGains[i] = 1.0 / taps;
}
double[] fbGains = new double[0];
return new LinearDigitalFilter(source, ffGains, fbGains);
}
@Override
public double get() {
double retVal = 0.0;
// Calculate the new value
for (int i = 0; i < m_inputGains.length; i++) {
retVal += m_inputs.get(i) * m_inputGains[i];
}
for (int i = 0; i < m_outputGains.length; i++) {
retVal -= m_outputs.get(i) * m_outputGains[i];
}
return retVal;
}
@Override
public void reset() {
m_inputs.clear();
m_outputs.clear();
}
/**
* Calculates the next value of the filter.
*
* @return The filtered value at this step
*/
@Override
public double pidGet() {
double retVal = 0.0;
// Rotate the inputs
m_inputs.addFirst(pidGetSource());
// Calculate the new value
for (int i = 0; i < m_inputGains.length; i++) {
retVal += m_inputs.get(i) * m_inputGains[i];
}
for (int i = 0; i < m_outputGains.length; i++) {
retVal -= m_outputs.get(i) * m_outputGains[i];
}
// Rotate the outputs
m_outputs.addFirst(retVal);
return retVal;
}
}

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
View File

@@ -4,9 +4,7 @@
package edu.wpi.first.wpilibj.interfaces;
import edu.wpi.first.wpilibj.PIDSource;
/** Interface for a Potentiometer. */
public interface Potentiometer extends PIDSource {
public interface Potentiometer {
double get();
}