101 lines
4.5 KiB
C++
101 lines
4.5 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) FIRST 2015-2016. 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 <initializer_list>
|
|
#include <memory>
|
|
#include <vector>
|
|
#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<PIDSource> source,
|
|
std::initializer_list<double> ffGains,
|
|
std::initializer_list<double> fbGains);
|
|
LinearDigitalFilter(std::shared_ptr<PIDSource> source,
|
|
std::initializer_list<double> ffGains,
|
|
const std::vector<double>& fbGains);
|
|
LinearDigitalFilter(std::shared_ptr<PIDSource> source,
|
|
const std::vector<double>& ffGains,
|
|
std::initializer_list<double> fbGains);
|
|
LinearDigitalFilter(std::shared_ptr<PIDSource> source,
|
|
const std::vector<double>& ffGains,
|
|
const std::vector<double>& fbGains);
|
|
|
|
// Static methods to create commonly used filters
|
|
static LinearDigitalFilter SinglePoleIIR(std::shared_ptr<PIDSource> source,
|
|
double timeConstant, double period);
|
|
static LinearDigitalFilter HighPass(std::shared_ptr<PIDSource> source,
|
|
double timeConstant, double period);
|
|
static LinearDigitalFilter MovingAverage(std::shared_ptr<PIDSource> source,
|
|
unsigned int taps);
|
|
|
|
// Filter interface
|
|
double Get() const override;
|
|
void Reset() override;
|
|
|
|
// PIDSource interface
|
|
double PIDGet() override;
|
|
|
|
private:
|
|
CircularBuffer<double> m_inputs;
|
|
CircularBuffer<double> m_outputs;
|
|
std::vector<double> m_inputGains;
|
|
std::vector<double> m_outputGains;
|
|
};
|