Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* 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. */
@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.examples.ultrasonic;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.MedianFilter;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -31,6 +32,9 @@ public class Robot extends TimedRobot {
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
// median filter to discard outliers; filters over 10 samples
private final MedianFilter m_filter = new MedianFilter(10);
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive
= new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -43,7 +47,9 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// 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.getValue()) * kValueToInches;
// convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
View File

@@ -8,6 +8,7 @@
package edu.wpi.first.wpilibj.examples.ultrasonicpid;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.MedianFilter;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.PIDController;
@@ -37,6 +38,9 @@ public class Robot extends TimedRobot {
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
// median filter to discard outliers; filters over 5 samples
private final MedianFilter m_filter = new MedianFilter(5);
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive
= new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
@@ -51,8 +55,10 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// returned value is filtered with a rolling median filter, since ultrasonics
// tend to be quite noisy and susceptible to sudden outliers
double pidOutput
= m_pidController.calculate(m_ultrasonic.getAverageVoltage());
= m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
m_robotDrive.arcadeDrive(pidOutput, 0);
}