diff --git a/wpilibc/Athena/include/WPILib.h b/wpilibc/Athena/include/WPILib.h index 1428d2424f..3c4780f46d 100644 --- a/wpilibc/Athena/include/WPILib.h +++ b/wpilibc/Athena/include/WPILib.h @@ -50,6 +50,7 @@ #include "DriverStation.h" #include "Encoder.h" #include "ErrorBase.h" +#include "Filters/LinearDigitalFilter.h" #include "GearTooth.h" #include "GenericHID.h" #include "interfaces/Accelerometer.h" diff --git a/wpilibc/shared/include/CircularBuffer.h b/wpilibc/shared/include/CircularBuffer.h new file mode 100644 index 0000000000..c9ad0c710d --- /dev/null +++ b/wpilibc/shared/include/CircularBuffer.h @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 + +/** + * This is a simple circular buffer so we don't need to "bucket brigade" copy + * old values. + */ +template +class CircularBuffer { + public: + CircularBuffer(size_t size); + + void PushFront(T value); + void PushBack(T value); + T PopFront(); + T PopBack(); + void Reset(); + + T& operator[](size_t index); + const T& operator[](size_t index) const; + + private: + std::vector m_data; + + // Index of element at front of buffer + size_t m_front = 0; + + // Number of elements used in buffer + size_t m_length = 0; + + size_t ModuloInc(size_t index); + size_t ModuloDec(size_t index); +}; + +#include "CircularBuffer.inc" diff --git a/wpilibc/shared/include/CircularBuffer.inc b/wpilibc/shared/include/CircularBuffer.inc new file mode 100644 index 0000000000..7a27eb8130 --- /dev/null +++ b/wpilibc/shared/include/CircularBuffer.inc @@ -0,0 +1,123 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 + +template +CircularBuffer::CircularBuffer(size_t size) : m_data(size, 0) {} + +/** + * Push new value onto front of the buffer. The value at the back is overwritten + * if the buffer is full. + */ +template +void CircularBuffer::PushFront(T value) { + if (m_data.size() == 0) { + return; + } + + m_front = ModuloDec(m_front); + + m_data[m_front] = value; + + if (m_length < m_data.size()) { + m_length++; + } +} + +/** + * Push new value onto back of the buffer. The value at the front is overwritten + * if the buffer is full. + */ +template +void CircularBuffer::PushBack(T value) { + if (m_data.size() == 0) { + return; + } + + m_data[(m_front + m_length) % m_data.size()] = value; + + if (m_length < m_data.size()) { + m_length++; + } else { + // Increment front if buffer is full to maintain size + m_front = ModuloInc(m_front); + } +} + +/** + * Pop value at front of buffer. + */ +template +T CircularBuffer::PopFront() { + // If there are no elements in the buffer, do nothing + if (m_length == 0) { + return 0; + } + + T& temp = m_data[m_front]; + m_front = ModuloInc(m_front); + m_length--; + return temp; +} + +/** + * Pop value at back of buffer. + */ +template +T CircularBuffer::PopBack() { + // If there are no elements in the buffer, do nothing + if (m_length == 0) { + return 0; + } + + m_length--; + return m_data[(m_front + m_length) % m_data.size()]; +} + +template +void CircularBuffer::Reset() { + std::fill(m_data.begin(), m_data.end(), 0); + m_front = 0; + m_length = 0; +} + +/** + * Returns element at index starting from front of buffer. + */ +template +T& CircularBuffer::operator[](size_t index) { + return m_data[(m_front + index) % m_data.size()]; +} + +/** + * Returns element at index starting from front of buffer. + */ +template +const T& CircularBuffer::operator[](size_t index) const { + return m_data[(m_front + index) % m_data.size()]; +} + +/** + * Increment an index modulo the length of the m_data buffer + */ +template +size_t CircularBuffer::ModuloInc(size_t index) { + return (index + 1) % m_data.size(); +} + +/** + * Decrement an index modulo the length of the m_data buffer + */ +template +size_t CircularBuffer::ModuloDec(size_t index) { + if (index == 0) { + return m_data.size() - 1; + } else { + return index - 1; + } +} diff --git a/wpilibc/shared/include/Filters/Filter.h b/wpilibc/shared/include/Filters/Filter.h new file mode 100644 index 0000000000..40a08bea3a --- /dev/null +++ b/wpilibc/shared/include/Filters/Filter.h @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 "PIDSource.h" + +/** + * Interface for filters + */ +class Filter : public PIDSource { + public: + Filter(std::shared_ptr source); + virtual ~Filter() = default; + + // PIDSource interface + virtual void SetPIDSourceType(PIDSourceType pidSource) override; + PIDSourceType GetPIDSourceType() const; + virtual double PIDGet() override = 0; + + /** + * Returns the current filter estimate without also inserting new data as + * PIDGet() would do. + * + * @return The current filter estimate + */ + virtual double Get() const = 0; + + /** + * Reset the filter state + */ + virtual void Reset() = 0; + + protected: + /** + * Calls PIDGet() of source + * + * @return Current value of source + */ + double PIDGetSource(); + + private: + std::shared_ptr m_source; +}; diff --git a/wpilibc/shared/include/Filters/LinearDigitalFilter.h b/wpilibc/shared/include/Filters/LinearDigitalFilter.h new file mode 100644 index 0000000000..bb48f3763c --- /dev/null +++ b/wpilibc/shared/include/Filters/LinearDigitalFilter.h @@ -0,0 +1,100 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 +#include "Filter.h" +#include "CircularBuffer.h" + +/** + * 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, I highly recommend the following articles: + * http://en.wikipedia.org/wiki/Linear_filter + * http://en.wikipedia.org/wiki/Iir_filter + * http://en.wikipedia.org/wiki/Fir_filter + * + * Note 1: PIDGet() should be called by the user on a known, regular period. + * You can set up a Notifier to do this (look at the WPILib PIDController + * class), or do it "inline" with code in a periodic function. + * + * Note 2: For ALL filters, gains are necessarily a function of frequency. If + * you make a filter that works well for you at, say, 100Hz, you will most + * definitely need to adjust the gains if you then want to run it at 200Hz! + * Combining this with Note 1 - the impetus is on YOU as a developer to make + * sure PIDGet() gets called at the desired, constant frequency! + */ +class LinearDigitalFilter : public Filter { + public: + LinearDigitalFilter(std::shared_ptr source, + std::initializer_list ffGains, + std::initializer_list fbGains); + LinearDigitalFilter(std::shared_ptr source, + std::initializer_list ffGains, + const std::vector& fbGains); + LinearDigitalFilter(std::shared_ptr source, + const std::vector& ffGains, + std::initializer_list fbGains); + LinearDigitalFilter(std::shared_ptr source, + const std::vector& ffGains, + const std::vector& fbGains); + + // Static methods to create commonly used filters + static LinearDigitalFilter SinglePoleIIR(std::shared_ptr source, + double timeConstant, double period); + static LinearDigitalFilter HighPass(std::shared_ptr source, + double timeConstant, double period); + static LinearDigitalFilter MovingAverage(std::shared_ptr source, + unsigned int taps); + + // Filter interface + double Get() const override; + void Reset() override; + + // PIDSource interface + double PIDGet() override; + + private: + CircularBuffer m_inputs; + CircularBuffer m_outputs; + std::vector m_inputGains; + std::vector m_outputGains; +}; diff --git a/wpilibc/shared/src/Filters/Filter.cpp b/wpilibc/shared/src/Filters/Filter.cpp new file mode 100644 index 0000000000..d04981bb56 --- /dev/null +++ b/wpilibc/shared/src/Filters/Filter.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 "Filters/Filter.h" + +Filter::Filter(std::shared_ptr source) { + m_source = source; +} + +void Filter::SetPIDSourceType(PIDSourceType pidSource) { + m_source->SetPIDSourceType(pidSource); +} + +PIDSourceType Filter::GetPIDSourceType() const { + return m_source->GetPIDSourceType(); +} + +double Filter::PIDGetSource() { + return m_source->PIDGet(); +} diff --git a/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp b/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp new file mode 100644 index 0000000000..f37c57c268 --- /dev/null +++ b/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp @@ -0,0 +1,166 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 "Filters/LinearDigitalFilter.h" +#include +#include + +/** + * Create a linear FIR or IIR filter + * + * @param source The PIDSource object that is used to get values + * @param ffGains The "feed forward" or FIR gains + * @param fbGains The "feed back" or IIR gains + */ +LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, + std::initializer_list ffGains, + std::initializer_list fbGains) : + Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), + m_inputGains(ffGains), m_outputGains(fbGains) {} + +/** + * Create a linear FIR or IIR filter + * + * @param source The PIDSource object that is used to get values + * @param ffGains The "feed forward" or FIR gains + * @param fbGains The "feed back" or IIR gains + */ +LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, + std::initializer_list ffGains, + const std::vector& fbGains) : + Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), + m_inputGains(ffGains), m_outputGains(fbGains) {} + +/** + * Create a linear FIR or IIR filter + * + * @param source The PIDSource object that is used to get values + * @param ffGains The "feed forward" or FIR gains + * @param fbGains The "feed back" or IIR gains + */ +LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, + const std::vector& ffGains, + std::initializer_list fbGains) : + Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), + m_inputGains(ffGains), m_outputGains(fbGains) {} + +/** + * Create a linear FIR or IIR filter + * + * @param source The PIDSource object that is used to get values + * @param ffGains The "feed forward" or FIR gains + * @param fbGains The "feed back" or IIR gains + */ +LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, + const std::vector& ffGains, + const std::vector& fbGains) : + Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), + m_inputGains(ffGains), m_outputGains(fbGains) {} + +/** + * 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 source The PIDSource object that is used to get values + * @param timeConstant The discrete-time time constant in seconds + * @param period The period in seconds between samples taken by the user + */ +LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(std::shared_ptr source, + double timeConstant, + double period) { + double gain = std::exp(-period / timeConstant); + return LinearDigitalFilter(std::move(source), {1.0 - gain}, {-gain}); +} + +/** + * 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 source The PIDSource object that is used to get values + * @param timeConstant The discrete-time time constant in seconds + * @param period The period in seconds between samples taken by the user + */ +LinearDigitalFilter LinearDigitalFilter::HighPass(std::shared_ptr source, + double timeConstant, + double period) { + double gain = std::exp(-period / timeConstant); + return LinearDigitalFilter(std::move(source), {gain, -gain}, {-gain}); +} + +/** + * 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 source The PIDSource object that is used to get values + * @param taps The number of samples to average over. Higher = smoother but + * slower + */ +LinearDigitalFilter LinearDigitalFilter::MovingAverage(std::shared_ptr source, + unsigned int taps) { + assert(taps > 0); + + std::vector gains(taps, 1.0 / taps); + return LinearDigitalFilter(std::move(source), gains, {}); +} + +/** + * {@inheritDoc} + */ +double LinearDigitalFilter::Get() const { + double retVal = 0.0; + + // Calculate the new value + for (unsigned int i = 0; i < m_inputGains.size(); i++) { + retVal += m_inputs[i] * m_inputGains[i]; + } + for (unsigned int i = 0; i < m_outputGains.size(); i++) { + retVal -= m_outputs[i] * m_outputGains[i]; + } + + return retVal; +} + +/** + * {@inheritDoc} + */ +void LinearDigitalFilter::Reset() { + m_inputs.Reset(); + m_outputs.Reset(); +} + +/** + * Calculates the next value of the filter + * + * @return The filtered value at this step + */ +double LinearDigitalFilter::PIDGet() { + double retVal = 0.0; + + // Rotate the inputs + m_inputs.PushFront(PIDGetSource()); + + // Calculate the new value + for (unsigned int i = 0; i < m_inputGains.size(); i++) { + retVal += m_inputs[i] * m_inputGains[i]; + } + for (unsigned int i = 0; i < m_outputGains.size(); i++) { + retVal -= m_outputs[i] * m_outputGains[i]; + } + + // Rotate the outputs + m_outputs.PushFront(retVal); + + return retVal; +} diff --git a/wpilibcIntegrationTests/include/TestBench.h b/wpilibcIntegrationTests/include/TestBench.h index bd3d2881ac..380cf6d698 100644 --- a/wpilibcIntegrationTests/include/TestBench.h +++ b/wpilibcIntegrationTests/include/TestBench.h @@ -63,4 +63,14 @@ class TestBench { /* PCM channels */ static const int32_t kSolenoidChannel1 = 0; static const int32_t kSolenoidChannel2 = 1; + + /* 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 int kMovAvgTaps = 6; + static constexpr double kMovAvgExpectedOutput = -10.191644; }; diff --git a/wpilibcIntegrationTests/src/CircularBufferTest.cpp b/wpilibcIntegrationTests/src/CircularBufferTest.cpp new file mode 100644 index 0000000000..f29654764b --- /dev/null +++ b/wpilibcIntegrationTests/src/CircularBufferTest.cpp @@ -0,0 +1,90 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 +#include "gtest/gtest.h" +#include + +static const std::array values = {751.848, 766.366, 342.657, + 234.252, 716.126, 132.344, + 445.697, 22.727, 421.125, + 799.913}; + +static const std::array pushFrontOut = {799.913, 421.125, 22.727, + 445.697, 132.344, 716.126, + 234.252, 342.657}; + +static const std::array pushBackOut = {342.657, 234.252, 716.126, + 132.344, 445.697, 22.727, + 421.125, 799.913}; + +TEST(CircularBufferTest, PushFrontTest) { + CircularBuffer queue(8); + + for (auto& value : values) { + queue.PushFront(value); + } + + for (unsigned int i = 0; i < pushFrontOut.size(); i++) { + EXPECT_EQ(pushFrontOut[i], queue[i]); + } +} + +TEST(CircularBufferTest, PushBackTest) { + CircularBuffer queue(8); + + for (auto& value : values) { + queue.PushBack(value); + } + + for (unsigned int i = 0; i < pushBackOut.size(); i++) { + EXPECT_EQ(pushBackOut[i], queue[i]); + } +} + +TEST(CircularBufferTest, PushPopTest) { + CircularBuffer queue(3); + + // Insert three elements into the buffer + queue.PushBack(1.0); + queue.PushBack(2.0); + queue.PushBack(3.0); + + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + EXPECT_EQ(3.0, queue[2]); + + /* + * The buffer is full now, so pushing subsequent elements will overwrite the + * front-most elements. + */ + + queue.PushBack(4.0); // Overwrite 1 with 4 + + // The buffer now contains 2, 3 and 4 + EXPECT_EQ(2.0, queue[0]); + EXPECT_EQ(3.0, queue[1]); + EXPECT_EQ(4.0, queue[2]); + + queue.PushBack(5.0); // Overwrite 2 with 5 + + // The buffer now contains 3, 4 and 5 + EXPECT_EQ(3.0, queue[0]); + EXPECT_EQ(4.0, queue[1]); + EXPECT_EQ(5.0, queue[2]); + + EXPECT_EQ(5.0, queue.PopBack()); // 5 is removed + + // The buffer now contains 3 and 4 + EXPECT_EQ(3.0, queue[0]); + EXPECT_EQ(4.0, queue[1]); + + EXPECT_EQ(3.0, queue.PopFront()); // 3 is removed + + // Leaving only one element with value == 4 + EXPECT_EQ(4.0, queue[0]); +} diff --git a/wpilibcIntegrationTests/src/FilterNoiseTest.cpp b/wpilibcIntegrationTests/src/FilterNoiseTest.cpp new file mode 100644 index 0000000000..bd52a52cb3 --- /dev/null +++ b/wpilibcIntegrationTests/src/FilterNoiseTest.cpp @@ -0,0 +1,133 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 +#include +#include +#include +#include + +#include + +#include "gtest/gtest.h" +#include "TestBench.h" +#include "Base.h" + +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; +} + +using std::chrono::system_clock; + +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 += TestBench::kFilterStep; + return m_dataFunc(m_count) + m_noise; + } + + void Reset() { + m_count = -TestBench::kFilterStep; + } + + private: + std::function m_dataFunc; + double m_noise = 0.0; + + // Make sure first call to PIDGet() uses m_count == 0 + double m_count = -TestBench::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 * M_PI * 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, + TestBench::kSinglePoleIIRTimeConstant, + TestBench::kFilterStep)); + break; + } + + case TEST_MOVAVG: { + m_filter = std::make_unique(LinearDigitalFilter::MovingAverage(m_noise, + TestBench::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 < TestBench::kFilterTime; t += TestBench::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_CASE_P(Test, FilterNoiseTest, + testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG)); diff --git a/wpilibcIntegrationTests/src/FilterOutputTest.cpp b/wpilibcIntegrationTests/src/FilterOutputTest.cpp new file mode 100644 index 0000000000..08b0918cec --- /dev/null +++ b/wpilibcIntegrationTests/src/FilterOutputTest.cpp @@ -0,0 +1,124 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 +#include +#include +#include +#include + +#include + +#include "gtest/gtest.h" +#include "TestBench.h" +#include "Base.h" + +enum FilterOutputTestType { TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS, TEST_MOVAVG }; + +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; + } + + return os; +} + +class DataWrapper : public PIDSource { + public: + DataWrapper(double (*dataFunc)(double)) { + m_dataFunc = dataFunc; + } + + virtual void SetPIDSourceType(PIDSourceType pidSource) {} + + virtual double PIDGet() { + m_count += TestBench::kFilterStep; + return m_dataFunc(m_count); + } + + void Reset() { + m_count = -TestBench::kFilterStep; + } + + private: + std::function m_dataFunc; + + // Make sure first call to PIDGet() uses m_count == 0 + double m_count = -TestBench::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 * M_PI * t) + 20.0 * std::cos(50.0 * M_PI * t); + } + + void SetUp() override { + m_data = std::make_shared(GetData); + + switch (GetParam()) { + case TEST_SINGLE_POLE_IIR: { + m_filter = std::make_unique(LinearDigitalFilter::SinglePoleIIR(m_data, + TestBench::kSinglePoleIIRTimeConstant, + TestBench::kFilterStep)); + m_expectedOutput = TestBench::kSinglePoleIIRExpectedOutput; + break; + } + + case TEST_HIGH_PASS: { + m_filter = std::make_unique(LinearDigitalFilter::HighPass(m_data, + TestBench::kHighPassTimeConstant, + TestBench::kFilterStep)); + m_expectedOutput = TestBench::kHighPassExpectedOutput; + break; + } + + case TEST_MOVAVG: { + m_filter = std::make_unique(LinearDigitalFilter::MovingAverage(m_data, + TestBench::kMovAvgTaps)); + m_expectedOutput = TestBench::kMovAvgExpectedOutput; + 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 < TestBench::kFilterTime; t += TestBench::kFilterStep) { + filterOutput = m_filter->PIDGet(); + } + + RecordProperty("FilterOutput", filterOutput); + + EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput) + << "Filter output didn't match expected value"; +} + +INSTANTIATE_TEST_CASE_P(Test, FilterOutputTest, + testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS, + TEST_MOVAVG)); diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java new file mode 100644 index 0000000000..b0dd443be3 --- /dev/null +++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java @@ -0,0 +1,125 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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; + +/** + * This is a simple circular buffer so we don't need to "bucket brigade" copy + * old values. + */ +public class CircularBuffer { + private double[] m_data; + private int m_front = 0; + private int m_length = 0; + + public CircularBuffer(int size) { + m_data = new double[size]; + for (double i : m_data) { + i = 0.0; + } + } + + /** + * Push new value onto front of the buffer. The value at the back is + * overwritten if the buffer is full. + */ + public void pushFront(double value) { + if (m_data.length == 0) { + return; + } + + m_front = moduloDec(m_front); + + m_data[m_front] = value; + + if (m_length < m_data.length) { + m_length++; + } + } + + /** + * Push new value onto back of the buffer. The value at the front is + * overwritten if the buffer is full. + */ + public void pushBack(double value) { + if (m_data.length == 0) { + return; + } + + m_data[(m_front + m_length) % m_data.length] = value; + + if (m_length < m_data.length) { + m_length++; + } else { + // Increment front if buffer is full to maintain size + m_front = moduloInc(m_front); + } + } + + /** + * Pop value at front of buffer. + * + * @return value at front of buffer + */ + public double popFront() { + // If there are no elements in the buffer, do nothing + if (m_length == 0) { + return 0.0; + } + + double temp = m_data[m_front]; + m_front = moduloInc(m_front); + m_length--; + return temp; + } + + + /** + * Pop value at back of buffer. + */ + public double popBack() { + // If there are no elements in the buffer, do nothing + if (m_length == 0) { + return 0.0; + } + + m_length--; + return m_data[(m_front + m_length) % m_data.length]; + } + + public void reset() { + for (double i : m_data) { + i = 0.0; + } + m_front = 0; + m_length = 0; + } + + /** + * @return element at index starting from front of buffer. + */ + public double get(int index) { + return m_data[(m_front + index) % m_data.length]; + } + + /** + * Increment an index modulo the length of the m_data buffer + */ + private int moduloInc(int index) { + return (index + 1) % m_data.length; + } + + /** + * Decrement an index modulo the length of the m_data buffer + */ + private int moduloDec(int index) { + if (index == 0) { + return m_data.length - 1; + } else { + return index - 1; + } + } +} diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/Filter.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/Filter.java new file mode 100644 index 0000000000..ad4706473e --- /dev/null +++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/Filter.java @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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.filters; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** + * Superclass for filters + */ +public abstract class Filter implements PIDSource { + private PIDSource m_source; + + public Filter(PIDSource source) { + m_source = source; + } + + /** + * {@inheritDoc} + */ + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_source.setPIDSourceType(pidSource); + } + + /** + * {@inheritDoc} + */ + public PIDSourceType getPIDSourceType() { + return m_source.getPIDSourceType(); + } + + /** + * {@inheritDoc} + */ + @Override + public abstract double pidGet(); + + /** + * Returns the current filter estimate without also inserting new data as + * pidGet() would do. + * + * @return The current filter estimate + */ + public abstract double get(); + + /** + * Reset the filter state + */ + public abstract void reset(); + + /** + * Calls PIDGet() of source + * + * @return Current value of source + */ + protected double pidGetSource() { + return m_source.pidGet(); + } +} diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java new file mode 100644 index 0000000000..b97090b042 --- /dev/null +++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java @@ -0,0 +1,204 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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.filters; + +import edu.wpi.first.wpilibj.filters.Filter; +import edu.wpi.first.wpilibj.CircularBuffer; +import edu.wpi.first.wpilibj.PIDSource; + +/** + * 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, I highly recommend the following articles: + * http://en.wikipedia.org/wiki/Linear_filter + * http://en.wikipedia.org/wiki/Iir_filter + * http://en.wikipedia.org/wiki/Fir_filter + * + * Note 1: PIDGet() should be called by the user on a known, regular period. + * You can set up a Notifier to do this (look at the WPILib PIDController + * class), or do it "inline" with code in a periodic function. + * + * Note 2: For ALL filters, gains are necessarily a function of frequency. If + * you make a filter that works well for you at, say, 100Hz, you will most + * definitely need to adjust the gains if you then want to run it at 200Hz! + * Combining this with Note 1 - the impetus is on YOU as a developer to make + * sure PIDGet() gets called at the desired, constant frequency! + */ +public class LinearDigitalFilter extends Filter { + private CircularBuffer m_inputs; + private CircularBuffer m_outputs; + private double[] m_inputGains; + private double[] m_outputGains; + + /** + * Create a linear FIR or IIR filter + * + * @param source The PIDSource object that is used to get values + * @param ffGains The "feed forward" or FIR gains + * @param fbGains The "feed back" or IIR gains + */ + public LinearDigitalFilter(PIDSource source, double[] ffGains, + double[] fbGains) { + super(source); + m_inputs = new CircularBuffer(ffGains.length); + m_outputs = new CircularBuffer(fbGains.length); + m_inputGains = ffGains; + m_outputGains = fbGains; + } + + /** + * 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 source The PIDSource object that is used to get values + * @param timeConstant The discrete-time time constant in seconds + * @param period The period in seconds between samples taken by the user + */ + public static LinearDigitalFilter singlePoleIIR(PIDSource source, + double timeConstant, + double period) { + double gain = Math.exp(-period / timeConstant); + double[] ffGains = {1.0 - gain}; + double[] fbGains = {-gain}; + + return new LinearDigitalFilter(source, ffGains, fbGains); + } + + /** + * Creates a first-order high-pass filter of the form: + * y[n] = gain*x[n] + (-gain)*x[n-1] + gain*y[n-1] + * where gain = e^(-dt / T), T is the time constant in seconds + * + * This filter is stable for time constants greater than zero + * + * @param source The PIDSource object that is used to get values + * @param timeConstant The discrete-time time constant in seconds + * @param period The period in seconds between samples taken by the user + */ + public static LinearDigitalFilter highPass(PIDSource source, + double timeConstant, + double period) { + double gain = Math.exp(-period / timeConstant); + double[] ffGains = {gain, -gain}; + double[] fbGains = {-gain}; + + return new LinearDigitalFilter(source, ffGains, fbGains); + } + + /** + * Creates a K-tap FIR moving average filter of the form: + * y[n] = 1/k * (x[k] + x[k-1] + ... + x[0]) + * + * This filter is always stable. + * + * @param source The PIDSource object that is used to get values + * @param taps The number of samples to average over. Higher = smoother but + * slower + * @throws IllegalArgumentException if number of taps is less than 1 + */ + public static LinearDigitalFilter movingAverage(PIDSource source, int taps) { + if (taps <= 0) { + throw new IllegalArgumentException("Number of taps was not at least 1"); + } + + double[] ffGains = new double[taps]; + for (int i = 0; i < ffGains.length; i++) { + ffGains[i] = 1.0 / taps; + } + + double[] fbGains = new double[0]; + + return new LinearDigitalFilter(source, ffGains, fbGains); + } + + /** + * {@inheritDoc} + */ + @Override + public double get() { + double retVal = 0.0; + + // Calculate the new value + for (int i = 0; i < m_inputGains.length; i++) { + retVal += m_inputs.get(i) * m_inputGains[i]; + } + for (int i = 0; i < m_outputGains.length; i++) { + retVal -= m_outputs.get(i) * m_outputGains[i]; + } + + return retVal; + } + + /** + * {@inheritDoc} + */ + @Override + public void reset() { + m_inputs.reset(); + m_outputs.reset(); + } + + /** + * Calculates the next value of the filter + * + * @return The filtered value at this step + */ + @Override + public double pidGet() { + double retVal = 0.0; + + // Rotate the inputs + m_inputs.pushFront(pidGetSource()); + + // Calculate the new value + for (int i = 0; i < m_inputGains.length; i++) { + retVal += m_inputs.get(i) * m_inputGains[i]; + } + for (int i = 0; i < m_outputGains.length; i++) { + retVal -= m_outputs.get(i) * m_outputGains[i]; + } + + // Rotate the outputs + m_outputs.pushFront(retVal); + + return retVal; + } +} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CircularBufferTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CircularBufferTest.java new file mode 100644 index 0000000000..56d05c0f29 --- /dev/null +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CircularBufferTest.java @@ -0,0 +1,101 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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 static org.junit.Assert.assertEquals; + +import java.util.logging.Logger; + +import org.junit.Test; + +import edu.wpi.first.wpilibj.CircularBuffer; +import edu.wpi.first.wpilibj.test.AbstractComsSetup; + +public class CircularBufferTest extends AbstractComsSetup { + private static final Logger logger = Logger.getLogger(CircularBufferTest.class.getName()); + private double[] values = {751.848, 766.366, 342.657, 234.252, 716.126, + 132.344, 445.697, 22.727, 421.125, 799.913}; + private double[] pushFrontOut = {799.913, 421.125, 22.727, 445.697, 132.344, + 716.126, 234.252, 342.657}; + private double[] pushBackOut = {342.657, 234.252, 716.126, 132.344, 445.697, + 22.727, 421.125, 799.913}; + + @Test + public void pushFrontTest() { + CircularBuffer queue = new CircularBuffer(8); + + for (double value : values) { + queue.pushFront(value); + } + + for (int i = 0; i < pushFrontOut.length; i++) { + assertEquals(pushFrontOut[i], queue.get(i), 0.00005); + } + } + + @Test + public void pushBackTest() { + CircularBuffer queue = new CircularBuffer(8); + + for (double value : values) { + queue.pushBack(value); + } + + for (int i = 0; i < pushBackOut.length; i++) { + assertEquals(pushBackOut[i], queue.get(i), 0.00005); + } + } + + @Test + public void pushPopTest() { + CircularBuffer queue = new CircularBuffer(3); + + // Insert three elements into the buffer + queue.pushBack(1.0); + queue.pushBack(2.0); + queue.pushBack(3.0); + + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + assertEquals(3.0, queue.get(2), 0.00005); + + /* + * The buffer is full now, so pushing subsequent elements will overwrite the + * front-most elements. + */ + + queue.pushBack(4.0); // Overwrite 1 with 4 + + // The buffer now contains 2, 3, and 4 + assertEquals(2.0, queue.get(0), 0.00005); + assertEquals(3.0, queue.get(1), 0.00005); + assertEquals(4.0, queue.get(2), 0.00005); + + queue.pushBack(5.0); // Overwrite 2 with 5 + + // The buffer now contains 3, 4, and 5 + assertEquals(3.0, queue.get(0), 0.00005); + assertEquals(4.0, queue.get(1), 0.00005); + assertEquals(5.0, queue.get(2), 0.00005); + + assertEquals(5.0, queue.popBack(), 0.00005); // 5 is removed + + // The buffer now contains 3 and 4 + assertEquals(3.0, queue.get(0), 0.00005); + assertEquals(4.0, queue.get(1), 0.00005); + + assertEquals(3.0, queue.popFront(), 0.00005); // 3 is removed + + // Leaving only one element with value == 4 + assertEquals(4.0, queue.get(0), 0.00005); + } + + @Override + protected Logger getClassLogger() { + return logger; + } +} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java new file mode 100644 index 0000000000..c7ddecc441 --- /dev/null +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java @@ -0,0 +1,90 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 static org.junit.Assert.assertTrue; + +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; + + +@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; + } + + 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 theoryData = 0.0; + 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) { + 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 new file mode 100644 index 0000000000..cae46b0acb --- /dev/null +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterOutputTest.java @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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 static org.junit.Assert.assertEquals; + +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; + + +@RunWith(Parameterized.class) +public class FilterOutputTest extends AbstractComsSetup { + private static final Logger logger = Logger.getLogger(FilterOutputTest.class.getName()); + + private double expectedOutput; + + private static FilterOutputFixture me = null; + + @Override + protected Logger getClassLogger() { + return logger; + } + + public FilterOutputTest(FilterOutputFixture mef) { + logger.fine("Constructor with: " + mef.getType()); + if (me != null && !me.equals(mef)) + me.teardown(); + me = mef; + 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()}}); + } + + @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.", 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 8183d2978f..d042d49cf9 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java @@ -19,10 +19,12 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; */ @RunWith(Suite.class) @SuiteClasses({AnalogCrossConnectTest.class, AnalogPotentiometerTest.class, - BuiltInAccelerometerTest.class, CANTalonTest.class, CounterTest.class, - DigitalGlitchFilterTest.class, DIOCrossConnectTest.class, EncoderTest.class, - GyroTest.class, MotorEncoderTest.class, MotorInvertingTest.class, - PCMTest.class, PDPTest.class, PIDTest.class, PreferencesTest.class, - RelayCrossConnectTest.class, SampleTest.class, TimerTest.class}) + BuiltInAccelerometerTest.class, CANTalonTest.class, + CircularBufferTest.class, CounterTest.class, DigitalGlitchFilterTest.class, + DIOCrossConnectTest.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}) 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 new file mode 100644 index 0000000000..e4c4e41427 --- /dev/null +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterNoiseFixture.java @@ -0,0 +1,167 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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.logging.Logger; +import java.util.Random; + +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 initialized = false; + private boolean tornDown = false; + protected T filter; + private NoiseGenerator data; + + /** + * Where the implementer of this class should pass the filter constructor + *$ + * @return + */ + abstract protected T giveFilter(PIDSource source); + + final private void initialize() { + synchronized (this) { + if (!initialized) { + initialized = true; // This ensures it is only initialized once + + data = new NoiseGenerator(TestBench.kStdDev) { + @Override + public double getData(double t) { + return 100.0 * Math.sin(2.0 * Math.PI * t); + } + }; + filter = giveFilter(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 filter; + } + + /** + * Gets the noise generator for this object + *$ + * @return the noise generator that this object refers too + */ + public NoiseGenerator getNoiseGenerator() { + initialize(); + return 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 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 noise = 0.0; + + // Make sure first call to pidGet() uses count == 0 + private double count = -TestBench.kFilterStep; + + private double stdDev; + private Random gen = new Random(); + + NoiseGenerator(double stdDev) { + this.stdDev = stdDev; + } + + abstract public double getData(double t); + + /** + * {@inheritDoc} + */ + @Override + public void setPIDSourceType(PIDSourceType pidSource) {} + + /** + * {@inheritDoc} + */ + @Override + public PIDSourceType getPIDSourceType() { + return PIDSourceType.kDisplacement; + } + + public double get() { + return getData(count) + noise; + } + + /** + * {@inheritDoc} + */ + @Override + public double pidGet() { + noise = gen.nextGaussian() * stdDev; + count += TestBench.kFilterStep; + return getData(count) + noise; + } + + public void reset() { + 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 new file mode 100644 index 0000000000..1b1ee4fc8a --- /dev/null +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterOutputFixture.java @@ -0,0 +1,159 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2014. 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.logging.Logger; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.test.TestBench; + +/** + * Represents a filterphysically 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 FilterOutputFixture implements ITestFixture { + private static final Logger logger = Logger.getLogger(FilterOutputFixture.class.getName()); + private boolean initialized = false; + private boolean tornDown = false; + protected T filter; + private DataWrapper data; + private double expectedOutput; + + public FilterOutputFixture(double expectedOutput) { + this.expectedOutput = expectedOutput; + } + + /** + * Get expected output of fixture + */ + public double getExpectedOutput() { + return expectedOutput; + } + + /** + * Where the implementer of this class should pass the filter constructor + *$ + * @return + */ + abstract protected T giveFilter(PIDSource source); + + final private void initialize() { + synchronized (this) { + if (!initialized) { + initialized = true; // This ensures it is only initialized once + + data = new DataWrapper() { + @Override + public double getData(double t) { + return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t); + } + }; + filter = giveFilter(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 filter; + } + + /** + * Gets the data wrapper for this object + *$ + * @return the data wrapper that this object refers too + */ + public DataWrapper getDataWrapper() { + initialize(); + return 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 filter.getClass().getSimpleName(); + } + + @Override + public boolean reset() { + 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 abstract class DataWrapper implements PIDSource { + // Make sure first call to pidGet() uses count == 0 + private double count = -TestBench.kFilterStep; + + abstract public double getData(double t); + + /** + * {@inheritDoc} + */ + @Override + public void setPIDSourceType(PIDSourceType pidSource) {} + + /** + * {@inheritDoc} + */ + @Override + public PIDSourceType getPIDSourceType() { + return PIDSourceType.kDisplacement; + } + + /** + * {@inheritDoc} + */ + @Override + public double pidGet() { + count += TestBench.kFilterStep; + return getData(count); + } + + public void reset() { + 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 78a058ad07..5dc93ae89d 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 @@ -19,13 +19,17 @@ import edu.wpi.first.wpilibj.CANJaguar; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalOutput; 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.CANMotorEncoderFixture; 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; @@ -79,6 +83,17 @@ 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 */ private static TestBench instance = null; @@ -410,6 +425,86 @@ 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(PIDSource source) { + return LinearDigitalFilter.singlePoleIIR(source, + 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(PIDSource source) { + return LinearDigitalFilter.highPass(source, 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(PIDSource source) { + return LinearDigitalFilter.movingAverage(source, 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