From 72f1b73ffd8939fb2e01ebbd4e0e57dafb534731 Mon Sep 17 00:00:00 2001
From: Vignesh Balasubramaniam <vigneshb.hp@gmail.com>
Date: Tue, 24 Sep 2024 17:15:53 -0500
Subject: [PATCH] Filter NavX2 inertial velocity

---
 build.gradle                                           |  2 +-
 .../frc/robot/subsystems/drive/DriveSubsystem.java     | 10 ++++++++++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/build.gradle b/build.gradle
index 864cbe4..edef8f8 100644
--- a/build.gradle
+++ b/build.gradle
@@ -97,7 +97,7 @@ dependencies {
   nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
   simulationRelease wpi.sim.enableRelease()
 
-  implementation 'com.github.lasarobotics:PurpleLib:87a27eedff'
+  implementation 'com.github.lasarobotics:PurpleLib:172583a8d6'
 
   implementation 'org.apache.commons:commons-math3:3.+'
 
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index 9cdc988..4687b22 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -38,6 +38,7 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -184,6 +185,9 @@ public Hardware(NavX2 navx,
   private Field2d m_field;
   private Alliance m_currentAlliance;
 
+  private LinearFilter m_inertialVelocityXFilter;
+  private LinearFilter m_inertialVelocityYFilter;
+
   private boolean m_isTractionControlEnabled = true;
   private boolean m_autoAimFront = false;
   private boolean m_autoAimBack = false;
@@ -226,6 +230,8 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
     );
     this.m_currentAlliance = Alliance.Blue;
     this.m_allianceCorrection = GlobalConstants.ROTATION_ZERO;
+    this.m_inertialVelocityXFilter = LinearFilter.singlePoleIIR(0.1, GlobalConstants.ROBOT_LOOP_PERIOD);
+    this.m_inertialVelocityXFilter = LinearFilter.singlePoleIIR(0.1, GlobalConstants.ROBOT_LOOP_PERIOD);
 
     // Calibrate and reset navX
     while (m_navx.isCalibrating()) stop();
@@ -798,6 +804,10 @@ public void periodic() {
     // Update current heading
     m_currentHeading = new Rotation2d(getPose().getX() - m_previousPose.getX(), getPose().getY() - m_previousPose.getY());
 
+    // Filter NavX2 velocity
+    m_navx.getInputs().xVelocity = Units.MetersPerSecond.of(m_inertialVelocityXFilter.calculate(m_navx.getInputs().xVelocity.in(Units.MetersPerSecond)));
+    m_navx.getInputs().yVelocity = Units.MetersPerSecond.of(m_inertialVelocityYFilter.calculate(m_navx.getInputs().yVelocity.in(Units.MetersPerSecond)));
+
     if (RobotBase.isSimulation()) return;
     smartDashboard();
     logOutputs();