Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add MedianFilter class for moving-window median (#2136)

Browse Source 
This kind of filter is extremely useful for signals that are susceptible to sudden
outliers - ultrasonics, 1-D LIDAR, and results from vision processing are all
good use-cases.

This also modifies the existing ultrasonic examples accordingly.
This commit is contained in:
Oblarg
2019-11-29 18:13:40 -05:00
committed by Peter Johnson
parent f79b7a058a
commit 5e97c81d80
9 changed files with 335 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
View File

@@ -1,11 +1,12 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-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/AnalogInput.h>
#include <frc/MedianFilter.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/drive/DifferentialDrive.h>
@@ -22,7 +23,10 @@ class Robot : public frc::TimedRobot {
*/
void TeleopPeriodic() override {
// Sensor returns a value from 0-4095 that is scaled to inches
double currentDistance = m_ultrasonic.GetValue() * kValueToInches;
// returned value is filtered with a rolling median filter, since
// ultrasonics tend to be quite noisy and susceptible to sudden outliers
double currentDistance =
m_filter.Calculate(m_ultrasonic.GetVoltage()) * kValueToInches;
// Convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;
// Drive robot
@@ -43,6 +47,9 @@ class Robot : public frc::TimedRobot {
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
// median filter to discard outliers; filters over 10 samples
frc::MedianFilter<double> m_filter{10};
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMVictorSPX m_left{kLeftMotorPort};

7
wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
View File

@@ -6,6 +6,7 @@
/*----------------------------------------------------------------------------*/
#include <frc/AnalogInput.h>
#include <frc/MedianFilter.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
@@ -27,7 +28,8 @@ class Robot : public frc::TimedRobot {
}
void TeleopPeriodic() override {
double output = m_pidController.Calculate(m_ultrasonic.GetAverageVoltage());
double output =
m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue()));
m_robotDrive.ArcadeDrive(output, 0);
}
@@ -51,6 +53,9 @@ class Robot : public frc::TimedRobot {
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
// median filter to discard outliers; filters over 5 samples
frc::MedianFilter<double> m_filter{5};
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMVictorSPX m_left{kLeftMotorPort};