diff --git a/wpilibc/wpilibC++/include/PIDController.h b/wpilibc/wpilibC++/include/PIDController.h index 91bf1cc28b..2994216d73 100644 --- a/wpilibc/wpilibC++/include/PIDController.h +++ b/wpilibc/wpilibC++/include/PIDController.h @@ -16,6 +16,9 @@ #include +#include +#include + class PIDOutput; /** @@ -53,6 +56,7 @@ class PIDController : public LiveWindowSendable, virtual double GetSetpoint() const override; virtual float GetError() const; + virtual float GetAvgError() const; virtual void SetPIDSourceType(PIDSourceType pidSource); virtual PIDSourceType GetPIDSourceType() const; @@ -60,6 +64,7 @@ class PIDController : public LiveWindowSendable, virtual void SetTolerance(float percent); virtual void SetAbsoluteTolerance(float absValue); virtual void SetPercentTolerance(float percentValue); + virtual void SetToleranceBuffer(unsigned buf = 1); virtual bool OnTarget() const; virtual void Enable() override; @@ -96,13 +101,19 @@ class PIDController : public LiveWindowSendable, kPercentTolerance, kNoTolerance } m_toleranceType = kNoTolerance; - float m_tolerance = 0.05; // the percetage or absolute error that is considered on - // target + + // the percetage or absolute error that is considered on target. + float m_tolerance = 0.05; float m_setpoint = 0; float m_error; float m_result = 0; float m_period; + // Length of buffer for averaging for tolerances. + std::atomic m_bufLength{1}; + std::queue m_buf; + double m_bufTotal = 0; + mutable priority_mutex m_mutex; std::unique_ptr m_controlLoop; diff --git a/wpilibc/wpilibC++Devices/src/PIDController.cpp b/wpilibc/wpilibC++Devices/src/PIDController.cpp index 25cae3b5f1..6fb55f1484 100644 --- a/wpilibc/wpilibC++Devices/src/PIDController.cpp +++ b/wpilibc/wpilibC++Devices/src/PIDController.cpp @@ -169,6 +169,15 @@ void PIDController::Calculate() { result = m_result; pidOutput->PIDWrite(result); + + // Update the buffer. + m_buf.push(m_error); + m_bufTotal += m_error; + // Remove old elements when buffer is full. + if (m_buf.size() > m_bufLength) { + m_bufTotal -= m_buf.front(); + m_buf.pop(); + } } } } @@ -310,6 +319,7 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) { /** * Set the setpoint for the PIDController + * Clears the queue for GetAvgError(). * @param setpoint the desired setpoint */ void PIDController::SetSetpoint(float setpoint) { @@ -325,6 +335,9 @@ void PIDController::SetSetpoint(float setpoint) { } else { m_setpoint = setpoint; } + + // Clear m_buf. + m_buf = std::queue(); } if (m_table != nullptr) { @@ -368,6 +381,22 @@ PIDSourceType PIDController::GetPIDSourceType() const { return m_pidInput->GetPIDSourceType(); } +/** + * 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 + */ +float PIDController::GetAvgError() const { + float avgError = 0; + { + std::unique_lock sync(m_mutex); + // Don't divide by zero. + if (m_buf.size()) avgError = m_bufTotal / m_buf.size(); + } + return avgError; +} + /* * Set the percentage error which is considered tolerable for use with * OnTarget. @@ -407,6 +436,25 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) { } } +/* + * 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. + */ +void PIDController::SetToleranceBuffer(unsigned bufLength) { + m_bufLength = bufLength; + + // Cut the buffer down to size if needed. + while (m_buf.size() > bufLength) { + m_bufTotal -= m_buf.front(); + m_buf.pop(); + } +} + /* * 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 @@ -417,7 +465,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) { * time. */ bool PIDController::OnTarget() const { - double error = GetError(); + double error = GetAvgError(); std::unique_lock sync(m_mutex); switch (m_toleranceType) { diff --git a/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp index f0e21efed3..1175c2dc4b 100644 --- a/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp +++ b/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp @@ -150,18 +150,19 @@ TEST_P(MotorEncoderTest, VelocityPIDController) { Reset(); m_encoder->SetPIDSourceType(PIDSourceType::kRate); - PIDController pid(0.002f, 0.0f, 0.0001f, m_encoder, m_speedController); - pid.SetAbsoluteTolerance(20.0f); + PIDController pid(1e-5, 0.0f, 3e-5, 8e-5, m_encoder, m_speedController); + pid.SetAbsoluteTolerance(50.0f); + pid.SetToleranceBuffer(10); pid.SetOutputRange(-0.3f, 0.3f); - pid.SetSetpoint(30); + pid.SetSetpoint(2000); /* 10 seconds should be plenty time to get to the setpoint */ pid.Enable(); Wait(10.0); - RecordProperty("PIDError", pid.GetError()); + RecordProperty("PIDError", pid.GetAvgError()); - EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds."; + EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 2000 << " Error was: " << pid.GetError(); pid.Disable(); } diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java index f02a397d6a..8658110ffb 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java @@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj; import java.util.TimerTask; +import java.util.LinkedList; import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; import edu.wpi.first.wpilibj.tables.ITable; @@ -39,6 +40,9 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll // integral calc private Tolerance m_tolerance; // the tolerance object used to check if on // target + private int m_bufLength = 0; + private LinkedList m_buf; + private double m_bufTotal = 0.0; private double m_setpoint = 0.0; private double m_error = 0.0; private double m_result = 0.0; @@ -79,7 +83,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll @Override public boolean onTarget() { - return (Math.abs(getError()) < percentage / 100 * (m_maximumInput - m_minimumInput)); + return (Math.abs(getAvgError()) < percentage / 100 * (m_maximumInput - m_minimumInput)); } } @@ -92,7 +96,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll @Override public boolean onTarget() { - return Math.abs(getError()) < value; + return Math.abs(getAvgError()) < value; } } @@ -153,6 +157,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll instances++; HLUsageReporting.reportPIDController(instances); m_tolerance = new NullTolerance(); + + m_buf = new LinkedList(); } /** @@ -298,6 +304,14 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } pidOutput = m_pidOutput; result = m_result; + + // Update the buffer. + m_buf.push(m_error); + m_bufTotal += m_error; + // Remove old elements when the buffer is full. + if (m_buf.size() > m_bufLength) { + m_bufTotal -= m_buf.pop(); + } } pidOutput.pidWrite(result); @@ -445,6 +459,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll /** * Set the setpoint for the PIDController + * Clears the queue for GetAvgError(). *$ * @param setpoint the desired setpoint */ @@ -461,6 +476,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll m_setpoint = setpoint; } + m_buf.clear(); + if (table != null) table.putNumber("setpoint", m_setpoint); } @@ -502,6 +519,21 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll return m_pidInput.getPIDSourceType(); } + /** + * 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. + *$ + * @return the current average of the error + */ + public synchronized double getAvgError() { + double avgError = 0; + // Don't divide by zero. + if (m_buf.size() != 0) avgError = m_bufTotal / m_buf.size(); + return avgError; + } + /** * Set the percentage error which is considered tolerable for use with * OnTarget. (Input of 15.0 = 15 percent) @@ -549,6 +581,24 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll m_tolerance = new PercentageTolerance(percentage); } + /** + * 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. + */ + public synchronized void setToleranceBuffer(int bufLength) { + m_bufLength = bufLength; + + // Cut the existing buffer down to size if needed. + while (m_buf.size() > bufLength) { + m_bufTotal -= m_buf.pop(); + } + } + /** * 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 diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java index 08d781d4af..5758c694a5 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -188,17 +188,19 @@ public class MotorEncoderTest extends AbstractComsSetup { @Test public void testVelocityPIDController() { me.getEncoder().setPIDSourceType(PIDSourceType.kRate); - PIDController pid = new PIDController(0.002, 0, 0.0001, me.getEncoder(), me.getMotor()); - pid.setAbsoluteTolerance(50); - pid.setOutputRange(-0.2, 0.2); - pid.setSetpoint(30); + PIDController pid = + new PIDController(1e-5, 0.0, 3e-5, 8e-5, me.getEncoder(), me.getMotor()); + pid.setAbsoluteTolerance(200); + pid.setToleranceBuffer(50); + pid.setOutputRange(-0.3, 0.3); + pid.setSetpoint(2000); pid.enable(); Timer.delay(10.0); pid.disable(); assertTrue( - "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(), + "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getAvgError(), pid.onTarget()); pid.free();