Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove PIDControllerRunner and mutex from new PIDController (#1795)

Browse Source 
Teams that wish to use it asynchronously may still do so - they simply need to handle the thread safety themselves (it is not that difficult, and can be done more cleanly in the calling code anyway).
This commit is contained in:
Oblarg
2019-08-04 03:01:11 -04:00
committed by Peter Johnson
parent 98d0706de8
commit c9873e81b2
11 changed files with 165 additions and 706 deletions
Download Patch File Download Diff File Expand all files Collapse all files

266
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -7,8 +7,6 @@
package edu.wpi.first.wpilibj.controller;
import java.util.concurrent.locks.ReentrantLock;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.SendableBase;
@@ -19,8 +17,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
*/
@SuppressWarnings("PMD.TooManyFields")
public class PIDController extends SendableBase {
protected final ReentrantLock m_thisMutex = new ReentrantLock();
private static int instances;
// Factor for "proportional" control
@@ -124,14 +120,9 @@ public class PIDController extends SendableBase {
*/
@SuppressWarnings("ParameterName")
public void setPID(double Kp, double Ki, double Kd) {
m_thisMutex.lock();
try {
m_Kp = Kp;
m_Ki = Ki;
m_Kd = Kd;
} finally {
m_thisMutex.unlock();
}
m_Kp = Kp;
m_Ki = Ki;
m_Kd = Kd;
}
/**
@@ -141,12 +132,7 @@ public class PIDController extends SendableBase {
*/
@SuppressWarnings("ParameterName")
public void setP(double Kp) {
m_thisMutex.lock();
try {
m_Kp = Kp;
} finally {
m_thisMutex.unlock();
}
m_Kp = Kp;
}
/**
@@ -156,12 +142,7 @@ public class PIDController extends SendableBase {
*/
@SuppressWarnings("ParameterName")
public void setI(double Ki) {
m_thisMutex.lock();
try {
m_Ki = Ki;
} finally {
m_thisMutex.unlock();
}
m_Ki = Ki;
}
/**
@@ -171,12 +152,7 @@ public class PIDController extends SendableBase {
*/
@SuppressWarnings("ParameterName")
public void setD(double Kd) {
m_thisMutex.lock();
try {
m_Kd = Kd;
} finally {
m_thisMutex.unlock();
}
m_Kd = Kd;
}
/**
@@ -185,12 +161,7 @@ public class PIDController extends SendableBase {
* @return proportional coefficient
*/
public double getP() {
m_thisMutex.lock();
try {
return m_Kp;
} finally {
m_thisMutex.unlock();
}
return m_Kp;
}
/**
@@ -199,12 +170,7 @@ public class PIDController extends SendableBase {
* @return integral coefficient
*/
public double getI() {
m_thisMutex.lock();
try {
return m_Ki;
} finally {
m_thisMutex.unlock();
}
return m_Ki;
}
/**
@@ -213,12 +179,7 @@ public class PIDController extends SendableBase {
* @return differential coefficient
*/
public double getD() {
m_thisMutex.lock();
try {
return m_Kd;
} finally {
m_thisMutex.unlock();
}
return m_Kd;
}
/**
@@ -238,12 +199,7 @@ public class PIDController extends SendableBase {
* @return The latest calculated output.
*/
public double getOutput() {
m_thisMutex.lock();
try {
return m_output;
} finally {
m_thisMutex.unlock();
}
return m_output;
}
/**
@@ -252,15 +208,10 @@ public class PIDController extends SendableBase {
* @param setpoint The desired setpoint.
*/
public void setSetpoint(double setpoint) {
m_thisMutex.lock();
try {
if (m_maximumInput > m_minimumInput) {
m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput);
} else {
m_setpoint = setpoint;
}
} finally {
m_thisMutex.unlock();
if (m_maximumInput > m_minimumInput) {
m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput);
} else {
m_setpoint = setpoint;
}
}
@@ -270,18 +221,12 @@ public class PIDController extends SendableBase {
* @return The current setpoint.
*/
public double getSetpoint() {
m_thisMutex.lock();
try {
return m_setpoint;
} finally {
m_thisMutex.unlock();
}
return m_setpoint;
}
/**
* Returns true if the error is within the percentage of the total input range,
* determined by SetTolerance. This asssumes that the maximum and minimum
* input were set using SetInput.
* Returns true if the error is within the percentage of the total input range, determined by
* SetTolerance. This asssumes that the maximum and minimum input were set using SetInput.
*
* <p>This will return false until at least one input value has been computed.
*
@@ -294,27 +239,21 @@ public class PIDController extends SendableBase {
/**
* Returns true if the error and change in error are below the specified tolerances.
*
* @param tolerance The maximum allowable error.
* @param tolerance The maximum allowable error.
* @param deltaTolerance The maximum allowable change in error from the previous iteration.
* @param toleranceType Whether the given tolerance values are absolute, or percentages of the
* total input range.
* @param toleranceType Whether the given tolerance values are absolute, or percentages of the
* total input range.
* @return Whether the error is within the acceptable bounds.
*/
public boolean atSetpoint(double tolerance, double deltaTolerance, Tolerance toleranceType) {
double error = getError();
double deltaError = (error - m_prevError) / getPeriod();
m_thisMutex.lock();
try {
double deltaError = (error - m_prevError) / getPeriod();
if (toleranceType == Tolerance.kPercent) {
return Math.abs(error) < tolerance / 100 * m_inputRange
&& Math.abs(deltaError) < deltaTolerance / 100 * m_inputRange;
} else {
return Math.abs(error) < tolerance
&& Math.abs(deltaError) < deltaTolerance;
}
} finally {
m_thisMutex.unlock();
if (toleranceType == Tolerance.kPercent) {
return Math.abs(error) < tolerance / 100 * m_inputRange
&& Math.abs(deltaError) < deltaTolerance / 100 * m_inputRange;
} else {
return Math.abs(error) < tolerance && Math.abs(deltaError) < deltaTolerance;
}
}
@@ -337,12 +276,7 @@ public class PIDController extends SendableBase {
* @param continuous true turns on continuous, false turns off continuous
*/
public void setContinuous(boolean continuous) {
m_thisMutex.lock();
try {
m_continuous = continuous;
} finally {
m_thisMutex.unlock();
}
m_continuous = continuous;
}
/**
@@ -352,18 +286,13 @@ public class PIDController extends SendableBase {
* @param maximumInput The maximum value expected from the input.
*/
public void setInputRange(double minimumInput, double maximumInput) {
m_thisMutex.lock();
try {
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
// Clamp setpoint to new input
if (m_maximumInput > m_minimumInput) {
m_setpoint = clamp(m_setpoint, m_minimumInput, m_maximumInput);
}
} finally {
m_thisMutex.unlock();
// Clamp setpoint to new input
if (m_maximumInput > m_minimumInput) {
m_setpoint = clamp(m_setpoint, m_minimumInput, m_maximumInput);
}
}
@@ -374,13 +303,8 @@ public class PIDController extends SendableBase {
* @param maximumOutput the maximum value to write to the output
*/
public void setOutputRange(double minimumOutput, double maximumOutput) {
m_thisMutex.lock();
try {
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
} finally {
m_thisMutex.unlock();
}
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
/**
@@ -399,14 +323,9 @@ public class PIDController extends SendableBase {
* @param deltaTolerance Change in absolute error per second which is tolerable.
*/
public void setAbsoluteTolerance(double tolerance, double deltaTolerance) {
m_thisMutex.lock();
try {
m_toleranceType = Tolerance.kAbsolute;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
} finally {
m_thisMutex.unlock();
}
m_toleranceType = Tolerance.kAbsolute;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
}
/**
@@ -425,14 +344,9 @@ public class PIDController extends SendableBase {
* @param deltaTolerance Change in percent error per second which is tolerable.
*/
public void setPercentTolerance(double tolerance, double deltaTolerance) {
m_thisMutex.lock();
try {
m_toleranceType = Tolerance.kPercent;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
} finally {
m_thisMutex.unlock();
}
m_toleranceType = Tolerance.kPercent;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
}
/**
@@ -441,12 +355,7 @@ public class PIDController extends SendableBase {
* @return The error.
*/
public double getError() {
m_thisMutex.lock();
try {
return getContinuousError(m_currError);
} finally {
m_thisMutex.unlock();
}
return getContinuousError(m_currError);
}
/**
@@ -454,13 +363,19 @@ public class PIDController extends SendableBase {
*/
public double getDeltaError() {
double error = getError();
return (error - m_prevError) / getPeriod();
}
m_thisMutex.lock();
try {
return (error - m_prevError) / getPeriod();
} finally {
m_thisMutex.unlock();
}
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
* @param setpoint The new setpoint of the controller.
*/
public double calculate(double measurement, double setpoint) {
// Set setpoint to provided value
setSetpoint(setpoint);
return calculate(measurement);
}
/**
@@ -469,48 +384,28 @@ public class PIDController extends SendableBase {
* @param measurement The current measurement of the process variable.
*/
public double calculate(double measurement) {
m_thisMutex.lock();
try {
return calculateUnsafe(measurement);
} finally {
m_thisMutex.unlock();
}
}
m_prevError = m_currError;
m_currError = getContinuousError(m_setpoint - measurement);
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
* @param setpoint The new setpoint of the controller.
*/
public double calculate(double measurement, double setpoint) {
m_thisMutex.lock();
try {
// Set setpoint to provided value
if (m_maximumInput > m_minimumInput) {
m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput);
} else {
m_setpoint = setpoint;
}
return calculateUnsafe(measurement);
} finally {
m_thisMutex.unlock();
if (m_Ki != 0) {
m_totalError = clamp(m_totalError + m_currError * getPeriod(), m_minimumOutput / m_Ki,
m_maximumOutput / m_Ki);
}
m_output = clamp(
m_Kp * m_currError + m_Ki * m_totalError + m_Kd * (m_currError - m_prevError) / getPeriod(),
m_minimumOutput, m_maximumOutput);
return m_output;
}
/**
* Resets the previous error and the integral term. Also disables the controller.
*/
public void reset() {
m_thisMutex.lock();
try {
m_prevError = 0;
m_totalError = 0;
m_output = 0;
} finally {
m_thisMutex.unlock();
}
m_prevError = 0;
m_totalError = 0;
m_output = 0;
}
@Override
@@ -524,7 +419,7 @@ public class PIDController extends SendableBase {
/**
* Wraps error around for continuous inputs. The original error is returned if continuous mode is
* disabled. This is an unsynchronized function.
* disabled.
*
* @param error The current error of the PID controller.
* @return Error for continuous inputs.
@@ -540,32 +435,9 @@ public class PIDController extends SendableBase {
}
}
}
return error;
}
/**
* Returns the next output of the PID controller.
*
* <p>Unlike the public functions above, this function doesn't lock the mutex.
*
* @param measurement The current measurement of the process variable.
*/
private double calculateUnsafe(double measurement) {
m_prevError = m_currError;
m_currError = getContinuousError(m_setpoint - measurement);
if (m_Ki != 0) {
m_totalError = clamp(m_totalError + m_currError * getPeriod(), m_minimumOutput / m_Ki,
m_maximumOutput / m_Ki);
}
m_output = clamp(m_Kp * m_currError + m_Ki * m_totalError
+ m_Kd * (m_currError - m_prevError) / getPeriod(), m_minimumOutput, m_maximumOutput);
return m_output;
}
private static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}

134
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java
View File

@@ -1,134 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.controller;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.DoubleConsumer;
import java.util.function.DoubleSupplier;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.SendableBase;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
public class PIDControllerRunner extends SendableBase {
private final PIDController m_controller;
private final DoubleConsumer m_controllerOutput;
private final DoubleSupplier m_measurementSource;
private boolean m_enabled;
private final ReentrantLock m_thisMutex = new ReentrantLock();
// Ensures when disable() is called, m_controllerOutput() won't run if
// Controller.update() is already running at that time.
private final ReentrantLock m_outputMutex = new ReentrantLock();
// This is declared after all other member variables so that during
// PIDControllerRunner destruction, the Notifier is stopped before any member
// variables its callable uses are destructed. This avoids use-after-free
// bugs like crashes when locking is attempted on deallocated mutexes.
private final Notifier m_notifier = new Notifier(this::run);
/**
* Allocates a PIDControllerRunner.
*
* @param controller The controller on which to call update().
* @param measurementSource The function that supplies the current process variable measurement.
* @param controllerOutput The function which updates the plant using the controller output
* passed as the argument.
*/
public PIDControllerRunner(PIDController controller, DoubleSupplier measurementSource,
DoubleConsumer controllerOutput) {
m_controller = controller;
m_controllerOutput = controllerOutput;
m_measurementSource = measurementSource;
m_notifier.startPeriodic(m_controller.getPeriod());
}
/**
* Begins running the controller.
*/
public void enable() {
m_thisMutex.lock();
try {
m_enabled = true;
} finally {
m_thisMutex.unlock();
}
}
/**
* Stops running the controller.
*
* <p>This sets the output to zero before stopping.
*/
public void disable() {
// Ensures m_enabled modification and m_controllerOutput() call occur
// atomically
m_outputMutex.lock();
try {
m_thisMutex.lock();
try {
m_enabled = false;
} finally {
m_thisMutex.unlock();
}
m_controllerOutput.accept(0.0);
} finally {
m_outputMutex.unlock();
}
}
/**
* Returns whether controller is running.
*/
public boolean isEnabled() {
m_thisMutex.lock();
try {
return m_enabled;
} finally {
m_thisMutex.unlock();
}
}
private void run() {
// Ensures m_enabled check and m_controllerOutput() call occur atomically
m_outputMutex.lock();
try {
m_thisMutex.lock();
try {
if (m_enabled) {
// Don't block other PIDControllerRunner operations on output
m_thisMutex.unlock();
m_controllerOutput.accept(m_controller.calculate(m_measurementSource.getAsDouble()));
}
} finally {
if (m_thisMutex.isHeldByCurrentThread()) {
m_thisMutex.unlock();
}
}
} finally {
m_outputMutex.unlock();
}
}
@Override
public void initSendable(SendableBuilder builder) {
m_controller.initSendable(builder);
builder.setSafeState(this::disable);
builder.addBooleanProperty("enabled", this::isEnabled, enabled -> {
if (enabled) {
enable();
} else {
disable();
}
});
}
}