From 30e936837ce0fe4fb64b229c9496f8fdf8844485 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 28 Jun 2019 13:35:57 -0700 Subject: [PATCH] Clean up LinearDigitalFilter class (#782) * Renamed LinearDigitalFilter to LinearFilter * Filter base class removed since it wasn't useful * C++: std::shared_ptr<> replaced with double parameter --- cmake/modules/CompileWarnings.cmake | 2 +- wpilibc/src/main/native/cpp/LinearFilter.cpp | 63 +++++++ wpilibc/src/main/native/cpp/PIDBase.cpp | 19 +- .../main/native/include/frc/LinearFilter.h | 140 +++++++++++++++ wpilibc/src/main/native/include/frc/PIDBase.h | 9 +- .../main/native/include/frc/PIDController.h | 3 +- .../include/frc/filters/LinearDigitalFilter.h | 5 +- .../src/test/native/cpp/FilterNoiseTest.cpp | 137 --------------- .../src/test/native/cpp/FilterOutputTest.cpp | 157 ----------------- .../test/native/cpp/LinearFilterNoiseTest.cpp | 92 ++++++++++ .../native/cpp/LinearFilterOutputTest.cpp | 132 ++++++++++++++ .../edu/wpi/first/wpilibj/LinearFilter.java | 162 ++++++++++++++++++ .../java/edu/wpi/first/wpilibj/PIDBase.java | 23 +-- .../wpilibj/filters/LinearDigitalFilter.java | 5 +- .../first/wpilibj/LinearFilterNoiseTest.java | 65 +++++++ .../first/wpilibj/LinearFilterOutputTest.java | 85 +++++++++ .../wpi/first/wpilibj/FilterNoiseTest.java | 97 ----------- .../wpi/first/wpilibj/FilterOutputTest.java | 96 ----------- .../wpi/first/wpilibj/WpiLibJTestSuite.java | 7 +- .../wpilibj/fixtures/FilterNoiseFixture.java | 159 ----------------- .../wpilibj/fixtures/FilterOutputFixture.java | 159 ----------------- .../edu/wpi/first/wpilibj/test/TestBench.java | 114 +----------- 22 files changed, 771 insertions(+), 960 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/LinearFilter.cpp create mode 100644 wpilibc/src/main/native/include/frc/LinearFilter.h delete mode 100644 wpilibc/src/test/native/cpp/FilterNoiseTest.cpp delete mode 100644 wpilibc/src/test/native/cpp/FilterOutputTest.cpp create mode 100644 wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp create mode 100644 wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterNoiseTest.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterOutputTest.java delete mode 100644 wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java delete mode 100644 wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterOutputTest.java delete mode 100644 wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterNoiseFixture.java delete mode 100644 wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterOutputFixture.java diff --git a/cmake/modules/CompileWarnings.cmake b/cmake/modules/CompileWarnings.cmake index b2a3172001..0eb9733c1a 100644 --- a/cmake/modules/CompileWarnings.cmake +++ b/cmake/modules/CompileWarnings.cmake @@ -1,6 +1,6 @@ macro(wpilib_target_warnings target) if(NOT MSVC) - target_compile_options(${target} PRIVATE -Wall -pedantic -Wextra -Werror -Wno-unused-parameter) + target_compile_options(${target} PRIVATE -Wall -pedantic -Wextra -Werror -Wno-unused-parameter -Wno-error=deprecated-declarations) else() target_compile_options(${target} PRIVATE /wd4244 /wd4267 /wd4146 /WX /wd4996) endif() diff --git a/wpilibc/src/main/native/cpp/LinearFilter.cpp b/wpilibc/src/main/native/cpp/LinearFilter.cpp new file mode 100644 index 0000000000..5e7957e2de --- /dev/null +++ b/wpilibc/src/main/native/cpp/LinearFilter.cpp @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/LinearFilter.h" + +#include +#include + +using namespace frc; + +LinearFilter::LinearFilter(wpi::ArrayRef ffGains, + wpi::ArrayRef fbGains) + : m_inputs(ffGains.size()), + m_outputs(fbGains.size()), + m_inputGains(ffGains), + m_outputGains(fbGains) {} + +LinearFilter LinearFilter::SinglePoleIIR(double timeConstant, double period) { + double gain = std::exp(-period / timeConstant); + return LinearFilter(1.0 - gain, -gain); +} + +LinearFilter LinearFilter::HighPass(double timeConstant, double period) { + double gain = std::exp(-period / timeConstant); + const double ffGains[] = {gain, -gain}; + return LinearFilter(ffGains, -gain); +} + +LinearFilter LinearFilter::MovingAverage(int taps) { + assert(taps > 0); + + std::vector gains(taps, 1.0 / taps); + return LinearFilter(gains, {}); +} + +void LinearFilter::Reset() { + m_inputs.reset(); + m_outputs.reset(); +} + +double LinearFilter::Calculate(double input) { + double retVal = 0.0; + + // Rotate the inputs + m_inputs.push_front(input); + + // Calculate the new value + for (size_t i = 0; i < m_inputGains.size(); i++) { + retVal += m_inputs[i] * m_inputGains[i]; + } + for (size_t i = 0; i < m_outputGains.size(); i++) { + retVal -= m_outputs[i] * m_outputGains[i]; + } + + // Rotate the outputs + m_outputs.push_front(retVal); + + return retVal; +} diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp index 872a45a6e3..90e81a5ca5 100644 --- a/wpilibc/src/main/native/cpp/PIDBase.cpp +++ b/wpilibc/src/main/native/cpp/PIDBase.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-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. */ @@ -34,12 +34,8 @@ PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source, m_D = Kd; m_F = Kf; - // 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_pidInput = &source; + m_filter = LinearFilter::MovingAverage(1); m_pidOutput = &output; @@ -200,10 +196,7 @@ void PIDBase::SetPercentTolerance(double percent) { void PIDBase::SetToleranceBuffer(int bufLength) { std::lock_guard lock(m_thisMutex); - - // Create LinearDigitalFilter with original source as its source argument - m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength); - m_pidInput = &m_filter; + m_filter = LinearFilter::MovingAverage(bufLength); } bool PIDBase::OnTarget() const { @@ -249,7 +242,7 @@ void PIDBase::InitSendable(SendableBuilder& builder) { } void PIDBase::Calculate() { - if (m_origSource == nullptr || m_pidOutput == nullptr) return; + if (m_pidInput == nullptr || m_pidOutput == nullptr) return; bool enabled; { @@ -277,7 +270,7 @@ void PIDBase::Calculate() { { std::lock_guard lock(m_thisMutex); - input = m_pidInput->PIDGet(); + input = m_filter.Calculate(m_pidInput->PIDGet()); pidSourceType = m_pidInput->GetPIDSourceType(); P = m_P; diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h new file mode 100644 index 0000000000..4f2bb9531d --- /dev/null +++ b/wpilibc/src/main/native/include/frc/LinearFilter.h @@ -0,0 +1,140 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include "frc/circular_buffer.h" + +namespace frc { + +/** + * 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. + * + * 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]) + * + * 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. + * + * 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. + * + * 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! + * + * For more on filters, we highly recommend the following articles:
+ * https://en.wikipedia.org/wiki/Linear_filter
+ * https://en.wikipedia.org/wiki/Iir_filter
+ * https://en.wikipedia.org/wiki/Fir_filter
+ * + * Note 1: Calculate() should be called by the user on a known, regular period. + * You can use a Notifier for this or do it "inline" with code in a + * periodic function. + * + * 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 Calculate() gets called at the desired, constant frequency! + */ +class LinearFilter { + public: + /** + * Create a linear FIR or IIR filter. + * + * @param ffGains The "feed forward" or FIR gains. + * @param fbGains The "feed back" or IIR gains. + */ + LinearFilter(wpi::ArrayRef ffGains, wpi::ArrayRef fbGains); + + LinearFilter(LinearFilter&&) = default; + LinearFilter& operator=(LinearFilter&&) = default; + + // Static methods to create commonly used filters + /** + * 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 + * + * This filter is stable for time constants greater than zero. + * + * @param timeConstant The discrete-time time constant in seconds. + * @param period The period in seconds between samples taken by the + * user. + */ + static LinearFilter SinglePoleIIR(double timeConstant, double period); + + /** + * 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 + * + * This filter is stable for time constants greater than zero. + * + * @param timeConstant The discrete-time time constant in seconds. + * @param period The period in seconds between samples taken by the + * user. + */ + static LinearFilter HighPass(double timeConstant, double period); + + /** + * Creates a K-tap FIR moving average filter of the form:
+ * y[n] = 1/k * (x[k] + x[k-1] + … + x[0]) + * + * This filter is always stable. + * + * @param taps The number of samples to average over. Higher = smoother but + * slower + */ + static LinearFilter MovingAverage(int taps); + + /** + * Reset the filter state. + */ + void Reset(); + + /** + * Calculates the next value of the filter. + * + * @param input Current input value. + * + * @return The filtered value at this step + */ + double Calculate(double input); + + private: + circular_buffer m_inputs{0}; + circular_buffer m_outputs{0}; + std::vector m_inputGains; + std::vector m_outputGains; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h index f29b56e464..63e7b02087 100644 --- a/wpilibc/src/main/native/include/frc/PIDBase.h +++ b/wpilibc/src/main/native/include/frc/PIDBase.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-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. */ @@ -14,11 +14,11 @@ #include #include "frc/Base.h" +#include "frc/LinearFilter.h" #include "frc/PIDInterface.h" #include "frc/PIDOutput.h" #include "frc/PIDSource.h" #include "frc/Timer.h" -#include "frc/filters/LinearDigitalFilter.h" #include "frc/smartdashboard/SendableBase.h" namespace frc { @@ -215,7 +215,7 @@ class PIDBase : public SendableBase, public PIDInterface, public PIDOutput { * * @return the average error */ - WPI_DEPRECATED("Use a LinearDigitalFilter as the input and GetError().") + WPI_DEPRECATED("Use a LinearFilter as the input and GetError().") virtual double GetAvgError() const; /** @@ -397,8 +397,7 @@ class PIDBase : public SendableBase, public PIDInterface, public PIDOutput { double m_error = 0; double m_result = 0; - std::shared_ptr m_origSource; - LinearDigitalFilter m_filter{nullptr, {}, {}}; + LinearFilter m_filter{{}, {}}; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/PIDController.h b/wpilibc/src/main/native/include/frc/PIDController.h index e9eea8b767..027032d151 100644 --- a/wpilibc/src/main/native/include/frc/PIDController.h +++ b/wpilibc/src/main/native/include/frc/PIDController.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-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. */ @@ -19,7 +19,6 @@ #include "frc/PIDBase.h" #include "frc/PIDSource.h" #include "frc/Timer.h" -#include "frc/filters/LinearDigitalFilter.h" namespace frc { diff --git a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h index 472d53b2f3..274d72a456 100644 --- a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h +++ b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2015-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. */ @@ -11,6 +11,7 @@ #include #include +#include #include "frc/circular_buffer.h" #include "frc/filters/Filter.h" @@ -76,6 +77,7 @@ class LinearDigitalFilter : public Filter { * @param ffGains The "feed forward" or FIR gains * @param fbGains The "feed back" or IIR gains */ + WPI_DEPRECATED("Use LinearFilter class instead.") LinearDigitalFilter(PIDSource& source, wpi::ArrayRef ffGains, wpi::ArrayRef fbGains); @@ -86,6 +88,7 @@ class LinearDigitalFilter : public Filter { * @param ffGains The "feed forward" or FIR gains * @param fbGains The "feed back" or IIR gains */ + WPI_DEPRECATED("Use LinearFilter class instead.") LinearDigitalFilter(std::shared_ptr source, wpi::ArrayRef ffGains, wpi::ArrayRef fbGains); diff --git a/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp deleted file mode 100644 index 86bb42c55d..0000000000 --- a/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-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. */ -/*----------------------------------------------------------------------------*/ - -#include "frc/filters/LinearDigitalFilter.h" // NOLINT(build/include_order) - -#include -#include -#include -#include -#include - -#include "frc/Base.h" -#include "gtest/gtest.h" - -/* Filter constants */ -static constexpr double kFilterStep = 0.005; -static constexpr double kFilterTime = 2.0; -static constexpr double kSinglePoleIIRTimeConstant = 0.015915; -static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003; -static constexpr double kHighPassTimeConstant = 0.006631; -static constexpr double kHighPassExpectedOutput = 10.074717; -static constexpr int32_t kMovAvgTaps = 6; -static constexpr double kMovAvgExpectedOutput = -10.191644; -static constexpr double kPi = 3.14159265358979323846; - -using namespace frc; - -enum FilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG }; - -std::ostream& operator<<(std::ostream& os, const FilterNoiseTestType& type) { - switch (type) { - case TEST_SINGLE_POLE_IIR: - os << "LinearDigitalFilter SinglePoleIIR"; - break; - case TEST_MOVAVG: - os << "LinearDigitalFilter MovingAverage"; - break; - } - - return os; -} - -constexpr double kStdDev = 10.0; - -/** - * Adds Gaussian white noise to a function returning data. The noise will have - * the standard deviation provided in the constructor. - */ -class NoiseGenerator : public PIDSource { - public: - NoiseGenerator(double (*dataFunc)(double), double stdDev) - : m_distr(0.0, stdDev) { - m_dataFunc = dataFunc; - } - - void SetPIDSourceType(PIDSourceType pidSource) override {} - - double Get() { return m_dataFunc(m_count) + m_noise; } - - double PIDGet() override { - m_noise = m_distr(m_gen); - m_count += kFilterStep; - return m_dataFunc(m_count) + m_noise; - } - - void Reset() { m_count = -kFilterStep; } - - private: - std::function m_dataFunc; - double m_noise = 0.0; - - // Make sure first call to PIDGet() uses m_count == 0 - double m_count = -kFilterStep; - - std::random_device m_rd; - std::mt19937 m_gen{m_rd()}; - std::normal_distribution m_distr; -}; - -/** - * A fixture that includes a noise generator wrapped in a filter - */ -class FilterNoiseTest : public testing::TestWithParam { - protected: - std::unique_ptr m_filter; - std::shared_ptr m_noise; - - static double GetData(double t) { return 100.0 * std::sin(2.0 * kPi * t); } - - void SetUp() override { - m_noise = std::make_shared(GetData, kStdDev); - - switch (GetParam()) { - case TEST_SINGLE_POLE_IIR: { - m_filter = std::make_unique( - LinearDigitalFilter::SinglePoleIIR( - m_noise, kSinglePoleIIRTimeConstant, kFilterStep)); - break; - } - - case TEST_MOVAVG: { - m_filter = std::make_unique( - LinearDigitalFilter::MovingAverage(m_noise, kMovAvgTaps)); - break; - } - } - } -}; - -/** - * Test if the filter reduces the noise produced by a signal generator - */ -TEST_P(FilterNoiseTest, NoiseReduce) { - double theoryData = 0.0; - double noiseGenError = 0.0; - double filterError = 0.0; - - m_noise->Reset(); - for (double t = 0; t < kFilterTime; t += kFilterStep) { - theoryData = GetData(t); - filterError += std::abs(m_filter->PIDGet() - theoryData); - noiseGenError += std::abs(m_noise->Get() - theoryData); - } - - RecordProperty("FilterError", filterError); - - // The filter should have produced values closer to the theory - EXPECT_GT(noiseGenError, filterError) - << "Filter should have reduced noise accumulation but failed"; -} - -INSTANTIATE_TEST_SUITE_P(Test, FilterNoiseTest, - testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG)); diff --git a/wpilibc/src/test/native/cpp/FilterOutputTest.cpp b/wpilibc/src/test/native/cpp/FilterOutputTest.cpp deleted file mode 100644 index 4cb0973d27..0000000000 --- a/wpilibc/src/test/native/cpp/FilterOutputTest.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-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. */ -/*----------------------------------------------------------------------------*/ - -#include "frc/filters/LinearDigitalFilter.h" // NOLINT(build/include_order) - -#include -#include -#include -#include -#include - -#include "frc/Base.h" -#include "gtest/gtest.h" - -/* Filter constants */ -static constexpr double kFilterStep = 0.005; -static constexpr double kFilterTime = 2.0; -static constexpr double kSinglePoleIIRTimeConstant = 0.015915; -static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003; -static constexpr double kHighPassTimeConstant = 0.006631; -static constexpr double kHighPassExpectedOutput = 10.074717; -static constexpr int32_t kMovAvgTaps = 6; -static constexpr double kMovAvgExpectedOutput = -10.191644; -static constexpr double kPi = 3.14159265358979323846; - -using namespace frc; - -enum FilterOutputTestType { - TEST_SINGLE_POLE_IIR, - TEST_HIGH_PASS, - TEST_MOVAVG, - TEST_PULSE -}; - -std::ostream& operator<<(std::ostream& os, const FilterOutputTestType& type) { - switch (type) { - case TEST_SINGLE_POLE_IIR: - os << "LinearDigitalFilter SinglePoleIIR"; - break; - case TEST_HIGH_PASS: - os << "LinearDigitalFilter HighPass"; - break; - case TEST_MOVAVG: - os << "LinearDigitalFilter MovingAverage"; - break; - case TEST_PULSE: - os << "LinearDigitalFilter Pulse"; - break; - } - - return os; -} - -class DataWrapper : public PIDSource { - public: - explicit DataWrapper(double (*dataFunc)(double)) { m_dataFunc = dataFunc; } - - virtual void SetPIDSourceType(PIDSourceType pidSource) {} - - virtual double PIDGet() { - m_count += kFilterStep; - return m_dataFunc(m_count); - } - - void Reset() { m_count = -kFilterStep; } - - private: - std::function m_dataFunc; - - // Make sure first call to PIDGet() uses m_count == 0 - double m_count = -kFilterStep; -}; - -/** - * A fixture that includes a consistent data source wrapped in a filter - */ -class FilterOutputTest : public testing::TestWithParam { - protected: - std::unique_ptr m_filter; - std::shared_ptr m_data; - double m_expectedOutput = 0.0; - - static double GetData(double t) { - return 100.0 * std::sin(2.0 * kPi * t) + 20.0 * std::cos(50.0 * kPi * t); - } - - static double GetPulseData(double t) { - if (std::abs(t - 1.0) < 0.001) { - return 1.0; - } else { - return 0.0; - } - } - - void SetUp() override { - switch (GetParam()) { - case TEST_SINGLE_POLE_IIR: { - m_data = std::make_shared(GetData); - m_filter = std::make_unique( - LinearDigitalFilter::SinglePoleIIR( - m_data, kSinglePoleIIRTimeConstant, kFilterStep)); - m_expectedOutput = kSinglePoleIIRExpectedOutput; - break; - } - - case TEST_HIGH_PASS: { - m_data = std::make_shared(GetData); - m_filter = - std::make_unique(LinearDigitalFilter::HighPass( - m_data, kHighPassTimeConstant, kFilterStep)); - m_expectedOutput = kHighPassExpectedOutput; - break; - } - - case TEST_MOVAVG: { - m_data = std::make_shared(GetData); - m_filter = std::make_unique( - LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps)); - m_expectedOutput = kMovAvgExpectedOutput; - break; - } - - case TEST_PULSE: { - m_data = std::make_shared(GetPulseData); - m_filter = std::make_unique( - LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps)); - m_expectedOutput = 0.0; - break; - } - } - } -}; - -/** - * Test if the linear digital filters produce consistent output - */ -TEST_P(FilterOutputTest, FilterOutput) { - m_data->Reset(); - - double filterOutput = 0.0; - for (double t = 0.0; t < kFilterTime; t += kFilterStep) { - filterOutput = m_filter->PIDGet(); - } - - RecordProperty("FilterOutput", filterOutput); - - EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput) - << "Filter output didn't match expected value"; -} - -INSTANTIATE_TEST_SUITE_P(Test, FilterOutputTest, - testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS, - TEST_MOVAVG, TEST_PULSE)); diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp new file mode 100644 index 0000000000..8ebf1f293a --- /dev/null +++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/LinearFilter.h" // NOLINT(build/include_order) + +#include +#include +#include + +#include "gtest/gtest.h" + +// Filter constants +static constexpr double kFilterStep = 0.005; +static constexpr double kFilterTime = 2.0; +static constexpr double kSinglePoleIIRTimeConstant = 0.015915; +static constexpr int32_t kMovAvgTaps = 6; + +enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG }; + +std::ostream& operator<<(std::ostream& os, + const LinearFilterNoiseTestType& type) { + switch (type) { + case TEST_SINGLE_POLE_IIR: + os << "LinearFilter SinglePoleIIR"; + break; + case TEST_MOVAVG: + os << "LinearFilter MovingAverage"; + break; + } + + return os; +} + +static double GetData(double t) { + constexpr double kPi = 3.14159265358979323846; + return 100.0 * std::sin(2.0 * kPi * t); +} + +class LinearFilterNoiseTest + : public testing::TestWithParam { + protected: + std::unique_ptr m_filter; + + void SetUp() override { + switch (GetParam()) { + case TEST_SINGLE_POLE_IIR: { + m_filter = std::make_unique( + frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant, + kFilterStep)); + break; + } + + case TEST_MOVAVG: { + m_filter = std::make_unique( + frc::LinearFilter::MovingAverage(kMovAvgTaps)); + break; + } + } + } +}; + +/** + * Test if the filter reduces the noise produced by a signal generator + */ +TEST_P(LinearFilterNoiseTest, NoiseReduce) { + double noiseGenError = 0.0; + double filterError = 0.0; + + std::random_device rd; + std::mt19937 gen{rd()}; + std::normal_distribution distr{0.0, 10.0}; + + for (double t = 0; t < kFilterTime; t += kFilterStep) { + double theory = GetData(t); + double noise = distr(gen); + filterError += std::abs(m_filter->Calculate(theory + noise) - theory); + noiseGenError += std::abs(noise - theory); + } + + RecordProperty("FilterError", filterError); + + // The filter should have produced values closer to the theory + EXPECT_GT(noiseGenError, filterError) + << "Filter should have reduced noise accumulation but failed"; +} + +INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest, + testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG)); diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp new file mode 100644 index 0000000000..a6e57a88e2 --- /dev/null +++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp @@ -0,0 +1,132 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/LinearFilter.h" // NOLINT(build/include_order) + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +// Filter constants +static constexpr double kFilterStep = 0.005; +static constexpr double kFilterTime = 2.0; +static constexpr double kSinglePoleIIRTimeConstant = 0.015915; +static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003; +static constexpr double kHighPassTimeConstant = 0.006631; +static constexpr double kHighPassExpectedOutput = 10.074717; +static constexpr int32_t kMovAvgTaps = 6; +static constexpr double kMovAvgExpectedOutput = -10.191644; + +enum LinearFilterOutputTestType { + TEST_SINGLE_POLE_IIR, + TEST_HIGH_PASS, + TEST_MOVAVG, + TEST_PULSE +}; + +std::ostream& operator<<(std::ostream& os, + const LinearFilterOutputTestType& type) { + switch (type) { + case TEST_SINGLE_POLE_IIR: + os << "LinearFilter SinglePoleIIR"; + break; + case TEST_HIGH_PASS: + os << "LinearFilter HighPass"; + break; + case TEST_MOVAVG: + os << "LinearFilter MovingAverage"; + break; + case TEST_PULSE: + os << "LinearFilter Pulse"; + break; + } + + return os; +} + +static double GetData(double t) { + constexpr double kPi = 3.14159265358979323846; + return 100.0 * std::sin(2.0 * kPi * t) + 20.0 * std::cos(50.0 * kPi * t); +} + +static double GetPulseData(double t) { + if (std::abs(t - 1.0) < 0.001) { + return 1.0; + } else { + return 0.0; + } +} + +/** + * A fixture that includes a consistent data source wrapped in a filter + */ +class LinearFilterOutputTest + : public testing::TestWithParam { + protected: + std::unique_ptr m_filter; + std::function m_data; + double m_expectedOutput = 0.0; + + void SetUp() override { + switch (GetParam()) { + case TEST_SINGLE_POLE_IIR: { + m_filter = std::make_unique( + frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant, + kFilterStep)); + m_data = GetData; + m_expectedOutput = kSinglePoleIIRExpectedOutput; + break; + } + + case TEST_HIGH_PASS: { + m_filter = std::make_unique( + frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep)); + m_data = GetData; + m_expectedOutput = kHighPassExpectedOutput; + break; + } + + case TEST_MOVAVG: { + m_filter = std::make_unique( + frc::LinearFilter::MovingAverage(kMovAvgTaps)); + m_data = GetData; + m_expectedOutput = kMovAvgExpectedOutput; + break; + } + + case TEST_PULSE: { + m_filter = std::make_unique( + frc::LinearFilter::MovingAverage(kMovAvgTaps)); + m_data = GetPulseData; + m_expectedOutput = 0.0; + break; + } + } + } +}; + +/** + * Test if the linear filters produce consistent output for a given data set. + */ +TEST_P(LinearFilterOutputTest, Output) { + double filterOutput = 0.0; + for (double t = 0.0; t < kFilterTime; t += kFilterStep) { + filterOutput = m_filter->Calculate(m_data(t)); + } + + RecordProperty("LinearFilterOutput", filterOutput); + + EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput) + << "Filter output didn't match expected value"; +} + +INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest, + testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS, + TEST_MOVAVG, TEST_PULSE)); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java new file mode 100644 index 0000000000..c95baf798b --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java @@ -0,0 +1,162 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-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; + +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. + * + *

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]) + * + *

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. + * + *

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. + * + *

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! + * + *

For more on filters, we highly recommend the following articles:
+ * https://en.wikipedia.org/wiki/Linear_filter
+ * https://en.wikipedia.org/wiki/Iir_filter
+ * https://en.wikipedia.org/wiki/Fir_filter
+ * + *

Note 1: calculate() should be called by the user on a known, regular period. You can use a + * Notifier for this or do it "inline" with code in a periodic function. + * + *

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 calculate() gets called at the desired, constant frequency! + */ +public class LinearFilter { + 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 ffGains The "feed forward" or FIR gains. + * @param fbGains The "feed back" or IIR gains. + */ + public LinearFilter(double[] ffGains, double[] fbGains) { + 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); + } + + /** + * 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. + * + *

This filter is stable for time constants greater than zero. + * + * @param timeConstant The discrete-time time constant in seconds. + * @param period The period in seconds between samples taken by the user. + */ + public static LinearFilter singlePoleIIR(double timeConstant, + double period) { + double gain = Math.exp(-period / timeConstant); + double[] ffGains = {1.0 - gain}; + double[] fbGains = {-gain}; + + return new LinearFilter(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. + * + *

This filter is stable for time constants greater than zero. + * + * @param timeConstant The discrete-time time constant in seconds. + * @param period The period in seconds between samples taken by the user. + */ + public static LinearFilter highPass(double timeConstant, + double period) { + double gain = Math.exp(-period / timeConstant); + double[] ffGains = {gain, -gain}; + double[] fbGains = {-gain}; + + return new LinearFilter(ffGains, fbGains); + } + + /** + * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... + + * x[0]). + * + *

This filter is always stable. + * + * @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 LinearFilter movingAverage(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 LinearFilter(ffGains, fbGains); + } + + /** + * Reset the filter state. + */ + public void reset() { + m_inputs.clear(); + m_outputs.clear(); + } + + /** + * Calculates the next value of the filter. + * + * @param input Current input value. + * + * @return The filtered value at this step + */ + public double calculate(double input) { + double retVal = 0.0; + + // Rotate the inputs + m_inputs.addFirst(input); + + // 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; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java index c34e02899f..6842387473 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java @@ -12,7 +12,6 @@ import java.util.concurrent.locks.ReentrantLock; 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.filters.LinearDigitalFilter; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import static java.util.Objects.requireNonNull; @@ -84,8 +83,7 @@ public class PIDBase extends SendableBase implements PIDInterface, PIDOutput { private double m_error; private double m_result; - private PIDSource m_origSource; - private LinearDigitalFilter m_filter; + private LinearFilter m_filter; protected ReentrantLock m_thisMutex = new ReentrantLock(); @@ -168,12 +166,8 @@ public class PIDBase extends SendableBase implements PIDInterface, PIDOutput { m_D = Kd; m_F = Kf; - // 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_pidInput = source; + m_filter = LinearFilter.movingAverage(1); m_pidOutput = output; @@ -203,7 +197,7 @@ public class PIDBase extends SendableBase implements PIDInterface, PIDOutput { */ @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.NPathComplexity"}) protected void calculate() { - if (m_origSource == null || m_pidOutput == null) { + if (m_pidInput == null || m_pidOutput == null) { return; } @@ -235,7 +229,7 @@ public class PIDBase extends SendableBase implements PIDInterface, PIDOutput { m_thisMutex.lock(); try { - input = m_pidInput.pidGet(); + input = m_filter.calculate(m_pidInput.pidGet()); pidSourceType = m_pidInput.getPIDSourceType(); P = m_P; @@ -638,7 +632,7 @@ public class PIDBase extends SendableBase implements PIDInterface, PIDOutput { public double getError() { m_thisMutex.lock(); try { - return getContinuousError(getSetpoint() - m_pidInput.pidGet()); + return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet())); } finally { m_thisMutex.unlock(); } @@ -731,15 +725,14 @@ public class PIDBase extends SendableBase implements PIDInterface, PIDOutput { * 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 LinearDigitalFilter as the input. + * @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 = LinearDigitalFilter.movingAverage(m_origSource, bufLength); - m_pidInput = m_filter; + m_filter = LinearFilter.movingAverage(bufLength); } finally { m_thisMutex.unlock(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java index 65e84b5f78..3cfc2a131b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2015-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. */ @@ -50,7 +50,10 @@ import edu.wpi.first.wpilibj.PIDSource; * 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; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterNoiseTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterNoiseTest.java new file mode 100644 index 0000000000..1bdf6f9367 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterNoiseTest.java @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-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; + +import java.util.Random; + +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.EnumSource; + +import static org.junit.jupiter.api.Assertions.assertTrue; + + +public class LinearFilterNoiseTest { + public enum TestType { + kSinglePoleIIR, kMovAvg + } + + // Filter constants + public static final double kFilterStep = 0.005; + public static final double kFilterTime = 2.0; + public static final double kSinglePoleIIRTimeConstant = 0.015915; + public static final int kMovAvgTaps = 6; + + @SuppressWarnings("ParameterName") + public static double getData(double t) { + return 100.0 * Math.sin(2.0 * Math.PI * t); + } + + /** + * Test if the filter reduces the noise produced by a signal generator. + */ + @ParameterizedTest + @EnumSource(TestType.class) + public void testNoiseReduce(TestType type) { + final LinearFilter filter; + + if (type == TestType.kSinglePoleIIR) { + filter = LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep); + } else { + filter = LinearFilter.movingAverage(kMovAvgTaps); + } + + double noiseGenError = 0.0; + double filterError = 0.0; + + final Random gen = new Random(); + final double kStdDev = 10.0; + + for (double t = 0; t < kFilterTime; t += kFilterStep) { + final double theory = getData(t); + final double noise = gen.nextGaussian() * kStdDev; + filterError += Math.abs(filter.calculate(theory + noise) - theory); + noiseGenError += Math.abs(noise - theory); + } + + assertTrue(noiseGenError > filterError, + "Filter should have reduced noise accumulation from " + noiseGenError + + " but failed. The filter error was " + filterError); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterOutputTest.java new file mode 100644 index 0000000000..0e0d5c1997 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterOutputTest.java @@ -0,0 +1,85 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-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; + +import java.util.function.DoubleFunction; + +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.EnumSource; + +import static org.junit.jupiter.api.Assertions.assertEquals; + + +public class LinearFilterOutputTest { + public enum TestType { + kSinglePoleIIR, + kHighPass, + kMovAvg, + kPulse + } + + // Filter constants + public static final double kFilterStep = 0.005; + public static final double kFilterTime = 2.0; + public static final double kSinglePoleIIRTimeConstant = 0.015915; + public static final double kSinglePoleIIRExpectedOutput = -3.2172003; + public static final double kHighPassTimeConstant = 0.006631; + public static final double kHighPassExpectedOutput = 10.074717; + public static final int kMovAvgTaps = 6; + public static final double kMovAvgExpectedOutput = -10.191644; + + @SuppressWarnings("ParameterName") + public static double getData(double t) { + return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t); + } + + @SuppressWarnings("ParameterName") + public static double getPulseData(double t) { + if (Math.abs(t - 1.0) < 0.001) { + return 1.0; + } else { + return 0.0; + } + } + + /** + * Test if the linear filters produce consistent output for a given data set. + */ + @ParameterizedTest + @EnumSource(TestType.class) + public void testOutput(TestType type) { + final LinearFilter filter; + final DoubleFunction data; + final double expectedOutput; + + if (type == TestType.kSinglePoleIIR) { + filter = LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep); + data = (double t) -> getData(t); + expectedOutput = kSinglePoleIIRExpectedOutput; + } else if (type == TestType.kHighPass) { + filter = LinearFilter.highPass(kHighPassTimeConstant, kFilterStep); + data = (double t) -> getData(t); + expectedOutput = kHighPassExpectedOutput; + } else if (type == TestType.kMovAvg) { + filter = LinearFilter.movingAverage(kMovAvgTaps); + data = (double t) -> getData(t); + expectedOutput = kMovAvgExpectedOutput; + } else { + filter = LinearFilter.movingAverage(kMovAvgTaps); + data = (double t) -> getPulseData(t); + expectedOutput = 0.0; + } + + double filterOutput = 0.0; + for (double t = 0.0; t < kFilterTime; t += kFilterStep) { + filterOutput = filter.calculate(data.apply(t)); + } + + assertEquals(expectedOutput, filterOutput, 0.00005, "Filter output was incorrect."); + } +} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java deleted file mode 100644 index 23490165bd..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java +++ /dev/null @@ -1,97 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-2018 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; - -import java.util.Arrays; -import java.util.Collection; -import java.util.logging.Logger; - -import org.junit.After; -import org.junit.AfterClass; -import org.junit.Before; -import org.junit.Test; -import org.junit.runner.RunWith; -import org.junit.runners.Parameterized; -import org.junit.runners.Parameterized.Parameters; - -import edu.wpi.first.wpilibj.fixtures.FilterNoiseFixture; -import edu.wpi.first.wpilibj.test.AbstractComsSetup; -import edu.wpi.first.wpilibj.test.TestBench; - -import static org.junit.Assert.assertTrue; - - -@RunWith(Parameterized.class) -public class FilterNoiseTest extends AbstractComsSetup { - private static final Logger logger = Logger.getLogger(FilterNoiseTest.class.getName()); - - private static FilterNoiseFixture me = null; - - @Override - protected Logger getClassLogger() { - return logger; - } - - /** - * Constructs the FilterNoiseTest. - * - * @param mef The fixture under test. - */ - public FilterNoiseTest(FilterNoiseFixture mef) { - logger.fine("Constructor with: " + mef.getType()); - if (me != null && !me.equals(mef)) { - me.teardown(); - } - me = mef; - } - - @Parameters(name = "{index}: {0}") - public static Collection[]> generateData() { - return Arrays.asList(new FilterNoiseFixture[][]{ - {TestBench.getInstance().getSinglePoleIIRNoiseFixture()}, - {TestBench.getInstance().getMovAvgNoiseFixture()}}); - } - - @Before - public void setUp() { - me.setup(); - } - - @After - public void tearDown() throws Exception { - me.reset(); - } - - @AfterClass - public static void tearDownAfterClass() { - // Clean up the fixture after the test - me.teardown(); - me = null; - } - - /** - * Test if the filter reduces the noise produced by a signal generator. - */ - @Test - public void testNoiseReduce() { - double noiseGenError = 0.0; - double filterError = 0.0; - - FilterNoiseFixture.NoiseGenerator noise = me.getNoiseGenerator(); - - noise.reset(); - for (double t = 0; t < TestBench.kFilterTime; t += TestBench.kFilterStep) { - final double theoryData = noise.getData(t); - filterError += Math.abs(me.getFilter().pidGet() - theoryData); - noiseGenError += Math.abs(noise.get() - theoryData); - } - - assertTrue(me.getType() + " should have reduced noise accumulation from " + noiseGenError - + " but failed. The filter error was " + filterError, noiseGenError > filterError); - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterOutputTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterOutputTest.java deleted file mode 100644 index a672f1a0e7..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterOutputTest.java +++ /dev/null @@ -1,96 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-2018 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; - -import java.util.Arrays; -import java.util.Collection; -import java.util.logging.Logger; - -import org.junit.After; -import org.junit.AfterClass; -import org.junit.Before; -import org.junit.Test; -import org.junit.runner.RunWith; -import org.junit.runners.Parameterized; -import org.junit.runners.Parameterized.Parameters; - -import edu.wpi.first.wpilibj.fixtures.FilterOutputFixture; -import edu.wpi.first.wpilibj.test.AbstractComsSetup; -import edu.wpi.first.wpilibj.test.TestBench; - -import static org.junit.Assert.assertEquals; - - -@RunWith(Parameterized.class) -public class FilterOutputTest extends AbstractComsSetup { - private static final Logger logger = Logger.getLogger(FilterOutputTest.class.getName()); - - private double m_expectedOutput; - - private static FilterOutputFixture me = null; - - @Override - protected Logger getClassLogger() { - return logger; - } - - /** - * Constructs a filter output test. - * - * @param mef The fixture under test. - */ - public FilterOutputTest(FilterOutputFixture mef) { - logger.fine("Constructor with: " + mef.getType()); - if (me != null && !me.equals(mef)) { - me.teardown(); - } - me = mef; - m_expectedOutput = me.getExpectedOutput(); - } - - @Parameters(name = "{index}: {0}") - public static Collection[]> generateData() { - return Arrays.asList(new FilterOutputFixture[][]{ - {TestBench.getInstance().getSinglePoleIIROutputFixture()}, - {TestBench.getInstance().getHighPassOutputFixture()}, - {TestBench.getInstance().getMovAvgOutputFixture()}, - {TestBench.getInstance().getPulseFixture()}}); - } - - @Before - public void setUp() { - me.setup(); - } - - @After - public void tearDown() throws Exception { - me.reset(); - } - - @AfterClass - public static void tearDownAfterClass() { - // Clean up the fixture after the test - me.teardown(); - me = null; - } - - /** - * Test if the filter produces consistent output for a given data set. - */ - @Test - public void testOutput() { - me.reset(); - - double filterOutput = 0.0; - for (double t = 0.0; t < TestBench.kFilterTime; t += TestBench.kFilterStep) { - filterOutput = me.getFilter().pidGet(); - } - - assertEquals(me.getType() + " output was incorrect.", m_expectedOutput, filterOutput, 0.00005); - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java index cbc48fd296..44f283753e 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java @@ -21,9 +21,8 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; @SuiteClasses({AnalogCrossConnectTest.class, AnalogPotentiometerTest.class, BuiltInAccelerometerTest.class, ConstantsPortsTest.class, CounterTest.class, DigitalGlitchFilterTest.class, DIOCrossConnectTest.class, DriveTest.class, - DriverStationTest.class, EncoderTest.class, FilterNoiseTest.class, FilterOutputTest.class, - GyroTest.class, MotorEncoderTest.class, MotorInvertingTest.class, PCMTest.class, PDPTest.class, - PIDTest.class, PreferencesTest.class, RelayCrossConnectTest.class, SampleTest.class, - TimerTest.class}) + DriverStationTest.class, EncoderTest.class, GyroTest.class, MotorEncoderTest.class, + MotorInvertingTest.class, PCMTest.class, PDPTest.class, PIDTest.class, PreferencesTest.class, + RelayCrossConnectTest.class, SampleTest.class, TimerTest.class}) public class WpiLibJTestSuite extends AbstractTestSuite { } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterNoiseFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterNoiseFixture.java deleted file mode 100644 index ee899b3354..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterNoiseFixture.java +++ /dev/null @@ -1,159 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 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.fixtures; - -import java.lang.reflect.ParameterizedType; -import java.util.Random; -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.test.TestBench; - -/** - * Represents a physically connected Motor and Encoder to allow for unit tests on these different - * pairs
Designed to allow the user to easily setup and tear down the fixture to allow for - * reuse. This class should be explicitly instantiated in the TestBed class to allow any test to - * access this fixture. This allows tests to be mailable so that you can easily reconfigure the - * physical testbed without breaking the tests. - */ -public abstract class FilterNoiseFixture implements ITestFixture { - private static final Logger logger = Logger.getLogger(FilterNoiseFixture.class.getName()); - private boolean m_initialized = false; - private boolean m_tornDown = false; - protected T m_filter; - private NoiseGenerator m_data; - - /** - * Where the implementer of this class should pass the filter constructor. - */ - protected abstract T giveFilter(PIDSource source); - - private void initialize() { - synchronized (this) { - if (!m_initialized) { - m_initialized = true; // This ensures it is only initialized once - - m_data = new NoiseGenerator(TestBench.kStdDev) { - @Override - @SuppressWarnings("ParameterName") - public double getData(double t) { - return 100.0 * Math.sin(2.0 * Math.PI * t); - } - }; - m_filter = giveFilter(m_data); - } - } - } - - @Override - public boolean setup() { - initialize(); - return true; - } - - /** - * Gets the filter for this Object. - * - * @return the filter this object refers too - */ - public T getFilter() { - initialize(); - return m_filter; - } - - /** - * Gets the noise generator for this object. - * - * @return the noise generator that this object refers too - */ - public NoiseGenerator getNoiseGenerator() { - initialize(); - return m_data; - } - - /** - * Retrieves the name of the filter that this object refers to. - * - * @return The simple name of the filter {@link Class#getSimpleName()} - */ - public String getType() { - initialize(); - return m_filter.getClass().getSimpleName(); - } - - // test here? - - @Override - public boolean reset() { - return true; - } - - @Override - public boolean teardown() { - return true; - } - - @Override - public String toString() { - StringBuilder string = new StringBuilder("FilterNoiseFixture<"); - // Get the generic type as a class - @SuppressWarnings("unchecked") - Class class1 = - (Class) ((ParameterizedType) getClass().getGenericSuperclass()) - .getActualTypeArguments()[0]; - string.append(class1.getSimpleName()); - string.append(">"); - return string.toString(); - } - - /** - * Adds Gaussian white noise to a function returning data. The noise will have the standard - * deviation provided in the constructor. - */ - public abstract class NoiseGenerator implements PIDSource { - private double m_noise = 0.0; - - // Make sure first call to pidGet() uses count == 0 - private double m_count = -TestBench.kFilterStep; - - private double m_stdDev; - private Random m_gen = new Random(); - - NoiseGenerator(double stdDev) { - m_stdDev = stdDev; - } - - @SuppressWarnings("ParameterName") - public abstract double getData(double t); - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - } - - @Override - public PIDSourceType getPIDSourceType() { - return PIDSourceType.kDisplacement; - } - - public double get() { - return getData(m_count) + m_noise; - } - - @Override - public double pidGet() { - m_noise = m_gen.nextGaussian() * m_stdDev; - m_count += TestBench.kFilterStep; - return getData(m_count) + m_noise; - } - - public void reset() { - m_count = -TestBench.kFilterStep; - } - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterOutputFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterOutputFixture.java deleted file mode 100644 index 3c31982523..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterOutputFixture.java +++ /dev/null @@ -1,159 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 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.fixtures; - -import java.lang.reflect.ParameterizedType; -import java.util.function.DoubleFunction; -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.test.TestBench; - -/** - * Represents a filter to allow for unit tests on them
Designed to allow the user to easily - * setup and tear down the fixture to allow for reuse. This class should be explicitly instantiated - * in the TestBed class to allow any test to access this fixture. This allows tests to be mailable - * so that you can easily reconfigure the physical testbed without breaking the tests. - */ -public abstract class FilterOutputFixture implements ITestFixture { - private static final Logger logger = Logger.getLogger(FilterOutputFixture.class.getName()); - private boolean m_initialized = false; - private boolean m_tornDown = false; - protected T m_filter; - protected DataWrapper m_data; - private double m_expectedOutput; - - public FilterOutputFixture(double expectedOutput) { - m_expectedOutput = expectedOutput; - } - - /** - * Get expected output of fixture. - */ - public double getExpectedOutput() { - return m_expectedOutput; - } - - public static DoubleFunction getData = new DoubleFunction() { - @Override - @SuppressWarnings("ParameterName") - public Double apply(double t) { - return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t); - } - }; - - public static DoubleFunction getPulseData = new DoubleFunction() { - @Override - @SuppressWarnings("ParameterName") - public Double apply(double t) { - if (Math.abs(t - 1.0) < 0.001) { - return 1.0; - } else { - return 0.0; - } - } - }; - - /** - * Where the implementer of this class should pass the filter constructor. - */ - protected abstract T giveFilter(); - - private void initialize() { - synchronized (this) { - if (!m_initialized) { - m_initialized = true; // This ensures it is only initialized once - - m_filter = giveFilter(); - } - } - } - - @Override - public boolean setup() { - initialize(); - return true; - } - - /** - * Gets the filter for this Object. - * - * @return the filter this object refers too - */ - public T getFilter() { - initialize(); - return m_filter; - } - - /** - * Retrieves the name of the filter that this object refers to. - * - * @return The simple name of the filter {@link Class#getSimpleName()} - */ - public String getType() { - initialize(); - return m_filter.getClass().getSimpleName(); - } - - @Override - public boolean reset() { - m_data.reset(); - return true; - } - - @Override - public boolean teardown() { - return true; - } - - @Override - public String toString() { - StringBuilder string = new StringBuilder("FilterOutputFixture<"); - // Get the generic type as a class - @SuppressWarnings("unchecked") - Class class1 = - (Class) ((ParameterizedType) getClass().getGenericSuperclass()) - .getActualTypeArguments()[0]; - string.append(class1.getSimpleName()); - string.append(">"); - return string.toString(); - } - - public class DataWrapper implements PIDSource { - // Make sure first call to pidGet() uses count == 0 - private double m_count = -TestBench.kFilterStep; - - private DoubleFunction m_func; - - public DataWrapper(DoubleFunction func) { - m_func = func; - } - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - } - - - @Override - public PIDSourceType getPIDSourceType() { - return PIDSourceType.kDisplacement; - } - - - @Override - public double pidGet() { - m_count += TestBench.kFilterStep; - return m_func.apply(m_count); - } - - public void reset() { - m_count = -TestBench.kFilterStep; - } - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java index 39585daf2d..3286db3324 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-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. */ @@ -18,16 +18,12 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.AnalogOutput; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Jaguar; -import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj.Victor; -import edu.wpi.first.wpilibj.filters.LinearDigitalFilter; import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture; import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture; -import edu.wpi.first.wpilibj.fixtures.FilterNoiseFixture; -import edu.wpi.first.wpilibj.fixtures.FilterOutputFixture; import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture; import edu.wpi.first.wpilibj.fixtures.RelayCrossConnectFixture; import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture; @@ -65,17 +61,6 @@ public final class TestBench { public static final int DIOCrossConnectA2 = 7; public static final int DIOCrossConnectA1 = 6; - // Filter constants - public static final double kStdDev = 10.0; - public static final double kFilterStep = 0.005; - public static final double kFilterTime = 2.0; - public static final double kSinglePoleIIRTimeConstant = 0.015915; - public static final double kSinglePoleIIRExpectedOutput = -3.2172003; - public static final double kHighPassTimeConstant = 0.006631; - public static final double kHighPassExpectedOutput = 10.074717; - public static final int kMovAvgTaps = 6; - public static final double kMovAvgExpectedOutput = -10.191644; - /** * The Singleton instance of the Test Bench. */ @@ -343,103 +328,6 @@ public final class TestBench { return encoderPortPairs; } - /** - * Constructs a new set of objects representing a single-pole IIR filter with a noisy data source. - * - * @return a single-pole IIR filter with a noisy data source - */ - public FilterNoiseFixture getSinglePoleIIRNoiseFixture() { - return new FilterNoiseFixture() { - @Override - protected LinearDigitalFilter giveFilter(PIDSource source) { - return LinearDigitalFilter.singlePoleIIR(source, - kSinglePoleIIRTimeConstant, - kFilterStep); - } - }; - } - - /** - * Constructs a new set of objects representing a moving average filter with a noisy data source - * using a linear digital filter. - * - * @return a moving average filter with a noisy data source - */ - public FilterNoiseFixture getMovAvgNoiseFixture() { - return new FilterNoiseFixture() { - @Override - protected LinearDigitalFilter giveFilter(PIDSource source) { - return LinearDigitalFilter.movingAverage(source, kMovAvgTaps); - } - }; - } - - /** - * Constructs a new set of objects representing a single-pole IIR filter with a repeatable data - * source. - * - * @return a single-pole IIR filter with a repeatable data source - */ - public FilterOutputFixture getSinglePoleIIROutputFixture() { - return new FilterOutputFixture(kSinglePoleIIRExpectedOutput) { - @Override - protected LinearDigitalFilter giveFilter() { - m_data = new DataWrapper(getData); - return LinearDigitalFilter.singlePoleIIR(m_data, - kSinglePoleIIRTimeConstant, - kFilterStep); - } - }; - } - - /** - * Constructs a new set of objects representing a high-pass filter with a repeatable data source. - * - * @return a high-pass filter with a repeatable data source - */ - public FilterOutputFixture getHighPassOutputFixture() { - return new FilterOutputFixture(kHighPassExpectedOutput) { - @Override - protected LinearDigitalFilter giveFilter() { - m_data = new DataWrapper(getData); - return LinearDigitalFilter.highPass(m_data, kHighPassTimeConstant, - kFilterStep); - } - }; - } - - /** - * Constructs a new set of objects representing a moving average filter with a repeatable data - * source using a linear digital filter. - * - * @return a moving average filter with a repeatable data source - */ - public FilterOutputFixture getMovAvgOutputFixture() { - return new FilterOutputFixture(kMovAvgExpectedOutput) { - @Override - protected LinearDigitalFilter giveFilter() { - m_data = new DataWrapper(getData); - return LinearDigitalFilter.movingAverage(m_data, kMovAvgTaps); - } - }; - } - - /** - * Constructs a new set of objects representing a moving average filter with a repeatable data - * source using a linear digital filter. - * - * @return a moving average filter with a repeatable data source - */ - public FilterOutputFixture getPulseFixture() { - return new FilterOutputFixture(0.0) { - @Override - protected LinearDigitalFilter giveFilter() { - m_data = new DataWrapper(getPulseData); - return LinearDigitalFilter.movingAverage(m_data, kMovAvgTaps); - } - }; - } - /** * Gets the singleton of the TestBench. If the TestBench is not already allocated in constructs an * new instance of it. Otherwise it returns the existing instance.