Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Clean up LinearDigitalFilter class (#782)

Browse Source 
* Renamed LinearDigitalFilter to LinearFilter
* Filter base class removed since it wasn't useful
* C++: std::shared_ptr<> replaced with double parameter
This commit is contained in:
Tyler Veness
2019-06-28 13:35:57 -07:00
committed by Peter Johnson
parent 311e2de4c1
commit 30e936837c
22 changed files with 771 additions and 960 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
cmake/modules/CompileWarnings.cmake
View File

@@ -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()

63
wpilibc/src/main/native/cpp/LinearFilter.cpp Normal file
View File

@@ -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 <cassert>
#include <cmath>
using namespace frc;
LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
wpi::ArrayRef<double> 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<double> 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;
}

19
wpilibc/src/main/native/cpp/PIDBase.cpp
View File

@@ -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<PIDSource>(&source, NullDeleter<PIDSource>());
// 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<wpi::mutex> 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<wpi::mutex> lock(m_thisMutex);
input = m_pidInput->PIDGet();
input = m_filter.Calculate(m_pidInput->PIDGet());
pidSourceType = m_pidInput->GetPIDSourceType();
P = m_P;

140
wpilibc/src/main/native/include/frc/LinearFilter.h Normal file
View File

@@ -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 <vector>
#include <wpi/ArrayRef.h>
#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:<br>
* 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:<br>
* y[n] is the output at time "n"<br>
* x[n] is the input at time "n"<br>
* y[n-1] is the output from the LAST time step ("n-1")<br>
* x[n-1] is the input from the LAST time step ("n-1")<br>
* b0 … bP are the "feedforward" (FIR) gains<br>
* a0 … aQ are the "feedback" (IIR) gains<br>
* 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:<br>
* https://en.wikipedia.org/wiki/Linear_filter<br>
* https://en.wikipedia.org/wiki/Iir_filter<br>
* https://en.wikipedia.org/wiki/Fir_filter<br>
*
* 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<double> ffGains, wpi::ArrayRef<double> 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:<br>
* y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, 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:<br>
* y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, 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:<br>
* 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<double> m_inputs{0};
circular_buffer<double> m_outputs{0};
std::vector<double> m_inputGains;
std::vector<double> m_outputGains;
};
} // namespace frc

9
wpilibc/src/main/native/include/frc/PIDBase.h
View File

@@ -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 <wpi/mutex.h>
#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<PIDSource> m_origSource;
LinearDigitalFilter m_filter{nullptr, {}, {}};
LinearFilter m_filter{{}, {}};
};
} // namespace frc

3
wpilibc/src/main/native/include/frc/PIDController.h
View File

@@ -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 {

5
wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
View File

@@ -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 <vector>
#include <wpi/ArrayRef.h>
#include <wpi/deprecated.h>
#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<double> ffGains,
wpi::ArrayRef<double> 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<PIDSource> source,
wpi::ArrayRef<double> ffGains,
wpi::ArrayRef<double> fbGains);

137
wpilibc/src/test/native/cpp/FilterNoiseTest.cpp
View File

@@ -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 <cmath>
#include <functional>
#include <memory>
#include <random>
#include <thread>
#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<double(double)> 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<double> m_distr;
};
/**
* A fixture that includes a noise generator wrapped in a filter
*/
class FilterNoiseTest : public testing::TestWithParam<FilterNoiseTestType> {
protected:
std::unique_ptr<PIDSource> m_filter;
std::shared_ptr<NoiseGenerator> m_noise;
static double GetData(double t) { return 100.0 * std::sin(2.0 * kPi * t); }
void SetUp() override {
m_noise = std::make_shared<NoiseGenerator>(GetData, kStdDev);
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::SinglePoleIIR(
m_noise, kSinglePoleIIRTimeConstant, kFilterStep));
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<LinearDigitalFilter>(
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));

157
wpilibc/src/test/native/cpp/FilterOutputTest.cpp
View File

@@ -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 <cmath>
#include <functional>
#include <memory>
#include <random>
#include <thread>
#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<double(double)> 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<FilterOutputTestType> {
protected:
std::unique_ptr<PIDSource> m_filter;
std::shared_ptr<DataWrapper> 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<DataWrapper>(GetData);
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::SinglePoleIIR(
m_data, kSinglePoleIIRTimeConstant, kFilterStep));
m_expectedOutput = kSinglePoleIIRExpectedOutput;
break;
}
case TEST_HIGH_PASS: {
m_data = std::make_shared<DataWrapper>(GetData);
m_filter =
std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::HighPass(
m_data, kHighPassTimeConstant, kFilterStep));
m_expectedOutput = kHighPassExpectedOutput;
break;
}
case TEST_MOVAVG: {
m_data = std::make_shared<DataWrapper>(GetData);
m_filter = std::make_unique<LinearDigitalFilter>(
LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps));
m_expectedOutput = kMovAvgExpectedOutput;
break;
}
case TEST_PULSE: {
m_data = std::make_shared<DataWrapper>(GetPulseData);
m_filter = std::make_unique<LinearDigitalFilter>(
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));

92
wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp Normal file
View File

@@ -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 <cmath>
#include <memory>
#include <random>
#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<LinearFilterNoiseTestType> {
protected:
std::unique_ptr<frc::LinearFilter> m_filter;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
kFilterStep));
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<frc::LinearFilter>(
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<double> 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));

132
wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp Normal file
View File

@@ -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 <cmath>
#include <functional>
#include <memory>
#include <random>
#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<LinearFilterOutputTestType> {
protected:
std::unique_ptr<frc::LinearFilter> m_filter;
std::function<double(double)> m_data;
double m_expectedOutput = 0.0;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
kFilterStep));
m_data = GetData;
m_expectedOutput = kSinglePoleIIRExpectedOutput;
break;
}
case TEST_HIGH_PASS: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep));
m_data = GetData;
m_expectedOutput = kHighPassExpectedOutput;
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<frc::LinearFilter>(
frc::LinearFilter::MovingAverage(kMovAvgTaps));
m_data = GetData;
m_expectedOutput = kMovAvgExpectedOutput;
break;
}
case TEST_PULSE: {
m_filter = std::make_unique<frc::LinearFilter>(
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));

162
wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java Normal file
View File

@@ -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.
*
* <p>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])
*
* <p>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.
*
* <p>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.
*
* <p>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!
*
* <p>For more on filters, we highly recommend the following articles:<br>
* https://en.wikipedia.org/wiki/Linear_filter<br>
* https://en.wikipedia.org/wiki/Iir_filter<br>
* https://en.wikipedia.org/wiki/Fir_filter<br>
*
* <p>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.
*
* <p>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.
*
* <p>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.
*
* <p>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]).
*
* <p>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;
}
}

23
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
View File

@@ -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();
}

5
wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
View File

@@ -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;

65
wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterNoiseTest.java Normal file
View File

@@ -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);
}
}

85
wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterOutputTest.java Normal file
View File

@@ -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<Double> 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.");
}
}

97
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterNoiseTest.java
View File

@@ -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<FilterNoiseFixture<?>[]> 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);
}
}

96
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/FilterOutputTest.java
View File

@@ -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<FilterOutputFixture<?>[]> 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);
}
}

7
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java
View File

@@ -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 {
}

159
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterNoiseFixture.java
View File

@@ -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<br> 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<T extends PIDSource> 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<T> class1 =
(Class<T>) ((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;
}
}
}

159
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FilterOutputFixture.java
View File

@@ -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<br> 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<T extends PIDSource> 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<Double> getData = new DoubleFunction<Double>() {
@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<Double> getPulseData = new DoubleFunction<Double>() {
@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<T> class1 =
(Class<T>) ((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<Double> m_func;
public DataWrapper(DoubleFunction<Double> 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;
}
}
}

114
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java
View File

@@ -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<LinearDigitalFilter> getSinglePoleIIRNoiseFixture() {
return new FilterNoiseFixture<LinearDigitalFilter>() {
@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<LinearDigitalFilter> getMovAvgNoiseFixture() {
return new FilterNoiseFixture<LinearDigitalFilter>() {
@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<LinearDigitalFilter> getSinglePoleIIROutputFixture() {
return new FilterOutputFixture<LinearDigitalFilter>(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<LinearDigitalFilter> getHighPassOutputFixture() {
return new FilterOutputFixture<LinearDigitalFilter>(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<LinearDigitalFilter> getMovAvgOutputFixture() {
return new FilterOutputFixture<LinearDigitalFilter>(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<LinearDigitalFilter> getPulseFixture() {
return new FilterOutputFixture<LinearDigitalFilter>(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.