WPILibC++  2020.3.2-60-g3011ebe
LinearFilter.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <cassert>
11 #include <cmath>
12 #include <initializer_list>
13 #include <vector>
14 
15 #include <hal/FRCUsageReporting.h>
16 #include <units/units.h>
17 #include <wpi/ArrayRef.h>
18 #include <wpi/circular_buffer.h>
19 
20 namespace frc {
21 
72 template <class T>
73 class LinearFilter {
74  public:
82  : m_inputs(ffGains.size()),
83  m_outputs(fbGains.size()),
84  m_inputGains(ffGains),
85  m_outputGains(fbGains) {
86  static int instances = 0;
87  instances++;
88  HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
89  }
90 
97  LinearFilter(std::initializer_list<double> ffGains,
98  std::initializer_list<double> fbGains)
99  : LinearFilter(wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
100  wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
101 
102  // Static methods to create commonly used filters
114  static LinearFilter<T> SinglePoleIIR(double timeConstant,
115  units::second_t period) {
116  double gain = std::exp(-period.to<double>() / timeConstant);
117  return LinearFilter(1.0 - gain, -gain);
118  }
119 
131  static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
132  double gain = std::exp(-period.to<double>() / timeConstant);
133  return LinearFilter({gain, -gain}, {-gain});
134  }
135 
145  static LinearFilter<T> MovingAverage(int taps) {
146  assert(taps > 0);
147 
148  std::vector<double> gains(taps, 1.0 / taps);
149  return LinearFilter(gains, {});
150  }
151 
155  void Reset() {
156  m_inputs.reset();
157  m_outputs.reset();
158  }
159 
167  T Calculate(T input) {
168  T retVal = T(0.0);
169 
170  // Rotate the inputs
171  m_inputs.push_front(input);
172 
173  // Calculate the new value
174  for (size_t i = 0; i < m_inputGains.size(); i++) {
175  retVal += m_inputs[i] * m_inputGains[i];
176  }
177  for (size_t i = 0; i < m_outputGains.size(); i++) {
178  retVal -= m_outputs[i] * m_outputGains[i];
179  }
180 
181  // Rotate the outputs
182  m_outputs.push_front(retVal);
183 
184  return retVal;
185  }
186 
187  private:
188  wpi::circular_buffer<T> m_inputs;
189  wpi::circular_buffer<T> m_outputs;
190  std::vector<double> m_inputGains;
191  std::vector<double> m_outputGains;
192 };
193 
194 } // namespace frc
frc::LinearFilter::MovingAverage
static LinearFilter< T > MovingAverage(int taps)
Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
Definition: LinearFilter.h:145
wpi::ArrayRef
ArrayRef - Represent a constant reference to an array (0 or more elements consecutively in memory),...
Definition: ArrayRef.h:42
frc::LinearFilter::LinearFilter
LinearFilter(wpi::ArrayRef< double > ffGains, wpi::ArrayRef< double > fbGains)
Create a linear FIR or IIR filter.
Definition: LinearFilter.h:81
wpi
WPILib C++ utilities (wpiutil) namespace.
Definition: Endian.h:31
frc::LinearFilter::Calculate
T Calculate(T input)
Calculates the next value of the filter.
Definition: LinearFilter.h:167
frc::LinearFilter::SinglePoleIIR
static LinearFilter< T > SinglePoleIIR(double timeConstant, units::second_t period)
Creates a one-pole IIR low-pass filter of the form: y[n] = (1 - gain) * x[n] + gain * y[n-1] where ...
Definition: LinearFilter.h:114
wpi::circular_buffer
This is a simple circular buffer so we don't need to "bucket brigade" copy old values.
Definition: circular_buffer.h:20
frc::LinearFilter::HighPass
static LinearFilter< T > HighPass(double timeConstant, units::second_t period)
Creates a first-order high-pass filter of the form: y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y...
Definition: LinearFilter.h:131
frc::LinearFilter::Reset
void Reset()
Reset the filter state.
Definition: LinearFilter.h:155
frc
A class that enforces constraints on the differential drive kinematics.
Definition: PDPSim.h:16
frc::LinearFilter
This class implements a linear, digital filter.
Definition: LinearFilter.h:73
frc::LinearFilter::LinearFilter
LinearFilter(std::initializer_list< double > ffGains, std::initializer_list< double > fbGains)
Create a linear FIR or IIR filter.
Definition: LinearFilter.h:97