From 4a07f0380fe7b305320f21b995ea9b45cd7e11d5 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 19 Nov 2017 15:58:30 -0800 Subject: [PATCH] PIDController class now uses LinearDigitalFilter for filtering velocity instead of internal queue (#38) --- wpilibc/src/main/native/cpp/PIDController.cpp | 57 +++------------ .../src/main/native/include/Filters/Filter.h | 2 +- .../src/main/native/include/PIDController.h | 10 +-- wpilibc/src/main/native/include/PIDSource.h | 2 +- .../FRCUserProgram/cpp/PIDToleranceTest.cpp | 12 ++-- .../edu/wpi/first/wpilibj/PIDController.java | 72 +++++-------------- .../wpi/first/wpilibj/PIDToleranceTest.java | 12 ++-- .../wpi/first/wpilibj/MotorEncoderTest.java | 6 +- 8 files changed, 48 insertions(+), 125 deletions(-) diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp index 9f06d2f413..de016813af 100644 --- a/wpilibc/src/main/native/cpp/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/PIDController.cpp @@ -69,7 +69,13 @@ PIDController::PIDController(double Kp, double Ki, double Kd, double Kf, m_D = Kd; m_F = Kf; - m_pidInput = source; + // Save original source + m_origSource = std::shared_ptr(source, NullDeleter()); + + // Create LinearDigitalFilter with original source as its source argument + m_filter = LinearDigitalFilter::MovingAverage(m_origSource, 1); + m_pidInput = &m_filter; + m_pidOutput = output; m_period = period; @@ -140,15 +146,6 @@ 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(); - } } } @@ -322,8 +319,6 @@ void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) { /** * Set the setpoint for the PIDController. * - * Clears the queue for GetAvgError(). - * * @param setpoint the desired setpoint */ void PIDController::SetSetpoint(double setpoint) { @@ -340,10 +335,6 @@ void PIDController::SetSetpoint(double setpoint) { } else { m_setpoint = setpoint; } - - // Clear m_buf. - m_buf = std::queue(); - m_bufTotal = 0; } if (m_setpointEntry) m_setpointEntry.SetDouble(m_setpoint); @@ -397,24 +388,6 @@ 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 - */ -double PIDController::GetAvgError() const { - double avgError = 0; - { - std::lock_guard 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. @@ -463,13 +436,10 @@ void PIDController::SetPercentTolerance(double percent) { */ void PIDController::SetToleranceBuffer(int bufLength) { std::lock_guard sync(m_mutex); - m_bufLength = bufLength; - // Cut the buffer down to size if needed. - while (m_buf.size() > static_cast(bufLength)) { - m_bufTotal -= m_buf.front(); - m_buf.pop(); - } + // Create LinearDigitalFilter with original source as its source argument + m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength); + m_pidInput = &m_filter; } /* @@ -484,12 +454,7 @@ void PIDController::SetToleranceBuffer(int bufLength) { * This will return false until at least one input value has been computed. */ bool PIDController::OnTarget() const { - { - std::lock_guard sync(m_mutex); - if (m_buf.size() == 0) return false; - } - - double error = GetAvgError(); + double error = GetError(); std::lock_guard sync(m_mutex); switch (m_toleranceType) { diff --git a/wpilibc/src/main/native/include/Filters/Filter.h b/wpilibc/src/main/native/include/Filters/Filter.h index d73c39f1a5..f4c4d133d3 100644 --- a/wpilibc/src/main/native/include/Filters/Filter.h +++ b/wpilibc/src/main/native/include/Filters/Filter.h @@ -23,7 +23,7 @@ class Filter : public PIDSource { // PIDSource interface void SetPIDSourceType(PIDSourceType pidSource) override; - PIDSourceType GetPIDSourceType() const; + PIDSourceType GetPIDSourceType() const override; double PIDGet() override = 0; /** diff --git a/wpilibc/src/main/native/include/PIDController.h b/wpilibc/src/main/native/include/PIDController.h index cb57eb7da7..8a1c753f09 100644 --- a/wpilibc/src/main/native/include/PIDController.h +++ b/wpilibc/src/main/native/include/PIDController.h @@ -9,13 +9,13 @@ #include #include -#include #include #include #include "Base.h" #include "Controller.h" +#include "Filters/LinearDigitalFilter.h" #include "LiveWindow/LiveWindow.h" #include "Notifier.h" #include "PIDInterface.h" @@ -64,7 +64,6 @@ class PIDController : public LiveWindowSendable, public PIDInterface { double GetDeltaSetpoint() const; virtual double GetError() const; - virtual double GetAvgError() const; virtual void SetPIDSourceType(PIDSourceType pidSource); virtual PIDSourceType GetPIDSourceType() const; @@ -156,11 +155,8 @@ class PIDController : public LiveWindowSendable, public PIDInterface { double m_result = 0; double m_period; - // Length of buffer for averaging for tolerances. - std::atomic m_bufLength{1}; - - std::queue m_buf; - double m_bufTotal = 0; + std::shared_ptr m_origSource; + LinearDigitalFilter m_filter{nullptr, {}, {}}; mutable wpi::mutex m_mutex; diff --git a/wpilibc/src/main/native/include/PIDSource.h b/wpilibc/src/main/native/include/PIDSource.h index fd412cc4cd..e317b797bd 100644 --- a/wpilibc/src/main/native/include/PIDSource.h +++ b/wpilibc/src/main/native/include/PIDSource.h @@ -20,7 +20,7 @@ enum class PIDSourceType { kDisplacement, kRate }; class PIDSource { public: virtual void SetPIDSourceType(PIDSourceType pidSource); - PIDSourceType GetPIDSourceType() const; + virtual PIDSourceType GetPIDSourceType() const; virtual double PIDGet() = 0; protected: diff --git a/wpilibcIntegrationTests/src/FRCUserProgram/cpp/PIDToleranceTest.cpp b/wpilibcIntegrationTests/src/FRCUserProgram/cpp/PIDToleranceTest.cpp index 01a9c111e7..708f2116cf 100644 --- a/wpilibcIntegrationTests/src/FRCUserProgram/cpp/PIDToleranceTest.cpp +++ b/wpilibcIntegrationTests/src/FRCUserProgram/cpp/PIDToleranceTest.cpp @@ -58,21 +58,21 @@ TEST_F(PIDToleranceTest, Absolute) { EXPECT_FALSE(pid->OnTarget()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetAvgError(); + << pid->GetError(); inp.val = setpoint + tolerance / 2; Wait(1.0); EXPECT_TRUE(pid->OnTarget()) << "Error was not in tolerance when it should have been. Error was " - << pid->GetAvgError(); + << pid->GetError(); inp.val = setpoint + 10 * tolerance; Wait(1.0); EXPECT_FALSE(pid->OnTarget()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetAvgError(); + << pid->GetError(); } TEST_F(PIDToleranceTest, Percent) { @@ -84,7 +84,7 @@ TEST_F(PIDToleranceTest, Percent) { EXPECT_FALSE(pid->OnTarget()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetAvgError(); + << pid->GetError(); inp.val = setpoint + (tolerance) / 200 * @@ -93,7 +93,7 @@ TEST_F(PIDToleranceTest, Percent) { EXPECT_TRUE(pid->OnTarget()) << "Error was not in tolerance when it should have been. Error was " - << pid->GetAvgError(); + << pid->GetError(); inp.val = setpoint + @@ -103,5 +103,5 @@ TEST_F(PIDToleranceTest, Percent) { EXPECT_FALSE(pid->OnTarget()) << "Error was in tolerance when it should not have been. Error was " - << pid->GetAvgError(); + << pid->GetError(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java index 7496f2be31..45fa154602 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java @@ -7,13 +7,12 @@ package edu.wpi.first.wpilibj; -import java.util.ArrayDeque; -import java.util.Queue; import java.util.TimerTask; import edu.wpi.first.networktables.EntryListenerFlags; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.filters.LinearDigitalFilter; import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; import edu.wpi.first.wpilibj.util.BoundaryException; @@ -54,14 +53,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll private double m_totalError = 0.0; // the tolerance object used to check if on target private Tolerance m_tolerance; - private int m_bufLength = 1; - private Queue m_buf; - private double m_bufTotal = 0.0; private double m_setpoint = 0.0; private double m_prevSetpoint = 0.0; private double m_error = 0.0; private double m_result = 0.0; private double m_period = kDefaultPeriod; + + PIDSource m_origSource; + LinearDigitalFilter m_filter; + protected PIDSource m_pidInput; protected PIDOutput m_pidOutput; java.util.Timer m_controlLoop; @@ -96,8 +96,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll @Override public boolean onTarget() { - return isAvgErrorValid() && Math.abs(getAvgError()) < m_percentage / 100 * (m_maximumInput - - m_minimumInput); + return Math.abs(getError()) < m_percentage / 100 * (m_maximumInput - m_minimumInput); } } @@ -110,7 +109,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll @Override public boolean onTarget() { - return isAvgErrorValid() && Math.abs(getAvgError()) < m_value; + return Math.abs(getError()) < m_value; } } @@ -157,7 +156,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll m_D = Kd; m_F = Kf; - m_pidInput = source; + // Save original source + m_origSource = source; + + // Create LinearDigitalFilter with original source as its source argument + m_filter = LinearDigitalFilter.movingAverage(m_origSource, 1); + m_pidInput = m_filter; + m_pidOutput = output; m_period = period; @@ -166,8 +171,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll instances++; HLUsageReporting.reportPIDController(instances); m_tolerance = new NullTolerance(); - - m_buf = new ArrayDeque(m_bufLength + 1); } /** @@ -282,14 +285,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll pidOutput = m_pidOutput; result = m_result; - - // Update the buffer. - m_buf.add(m_error); - m_bufTotal += m_error; - // Remove old elements when the buffer is full. - if (m_buf.size() > m_bufLength) { - m_bufTotal -= m_buf.remove(); - } } pidOutput.pidWrite(result); @@ -471,7 +466,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } /** - * Set the setpoint for the PIDController Clears the queue for GetAvgError(). + * Set the setpoint for the PIDController. * * @param setpoint the desired setpoint */ @@ -488,9 +483,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll m_setpoint = setpoint; } - m_buf.clear(); - m_bufTotal = 0; - if (m_setpointEntry != null) { m_setpointEntry.setDouble(m_setpoint); } @@ -541,32 +533,6 @@ 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; - } - - /** - * Returns whether or not any values have been collected. If no values have been collected, - * getAvgError is 0, which is invalid. - * - * @return True if {@link #getAvgError()} is currently valid. - */ - private synchronized boolean isAvgErrorValid() { - return m_buf.size() != 0; - } - /** * 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 @@ -609,12 +575,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll * @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.remove(); - } + m_filter = LinearDigitalFilter.movingAverage(m_origSource, bufLength); + m_pidInput = m_filter; } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java index 00c84f870f..400400f84c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java @@ -77,15 +77,15 @@ public class PIDToleranceTest { m_pid.enable(); Timer.delay(1); assertFalse("Error was in tolerance when it should not have been. Error was " - + m_pid.getAvgError(), m_pid.onTarget()); + + m_pid.getError(), m_pid.onTarget()); m_inp.m_val = m_setPoint + m_tolerance / 2; Timer.delay(1.0); assertTrue("Error was not in tolerance when it should have been. Error was " - + m_pid.getAvgError(), m_pid.onTarget()); + + m_pid.getError(), m_pid.onTarget()); m_inp.m_val = m_setPoint + 10 * m_tolerance; Timer.delay(1.0); assertFalse("Error was in tolerance when it should not have been. Error was " - + m_pid.getAvgError(), m_pid.onTarget()); + + m_pid.getError(), m_pid.onTarget()); } @Test @@ -94,16 +94,16 @@ public class PIDToleranceTest { m_pid.setSetpoint(m_setPoint); m_pid.enable(); assertFalse("Error was in tolerance when it should not have been. Error was " - + m_pid.getAvgError(), m_pid.onTarget()); + + m_pid.getError(), m_pid.onTarget()); //half of percent tolerance away from setPoint m_inp.m_val = m_setPoint + m_tolerance / 200 * m_range; Timer.delay(1.0); assertTrue("Error was not in tolerance when it should have been. Error was " - + m_pid.getAvgError(), m_pid.onTarget()); + + m_pid.getError(), m_pid.onTarget()); //double percent tolerance away from setPoint m_inp.m_val = m_setPoint + m_tolerance / 50 * m_range; Timer.delay(1.0); assertFalse("Error was in tolerance when it should not have been. Error was " - + m_pid.getAvgError(), m_pid.onTarget()); + + m_pid.getError(), m_pid.onTarget()); } } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java index 1c8d9ffec3..be1cb9c3ac 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -185,8 +185,8 @@ public class MotorEncoderTest extends AbstractComsSetup { pid.disable(); assertTrue( - "PID loop did not reach setpoint within 10 seconds. The average error was: " + pid - .getAvgError() + "The current error was" + pid.getError(), pid.onTarget()); + "PID loop did not reach setpoint within 10 seconds. The current error was" + pid + .getError(), pid.onTarget()); pid.free(); } @@ -206,7 +206,7 @@ public class MotorEncoderTest extends AbstractComsSetup { pid.disable(); assertTrue( - "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getAvgError(), + "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(), pid.onTarget()); pid.free();