works better

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
CurtinFRC · Feb 24, 2025 · 1613e8d · 1613e8d
1 parent 3a4259f
commit 1613e8d
Show file tree

Hide file tree

Showing 4 changed files with 86 additions and 2 deletions.
diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java
@@ -29,6 +29,7 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.CircularBuffer;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -50,6 +51,7 @@
 import org.curtinfrc.frc2025.Constants;
 import org.curtinfrc.frc2025.Constants.Mode;
 import org.curtinfrc.frc2025.generated.TunerConstants;
+import org.curtinfrc.frc2025.util.BufferUtil;
 import org.curtinfrc.frc2025.util.RepulsorFieldPlanner;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -75,6 +77,9 @@ public class Drive extends SubsystemBase {
       };
   private PoseEstimator poseEstimator =
       new PoseEstimator(kinematics, lastModulePositions, rawGyroRotation);
+  private CircularBuffer<Double> xAcceleration = new CircularBuffer<Double>(6);
+  private CircularBuffer<Double> yAcceleration = new CircularBuffer<Double>(6);
+  private ChassisSpeeds lastChassisSpeeds = new ChassisSpeeds();
 
   private double p = 6;
   private double d = 0.2;
@@ -194,12 +199,38 @@ public void periodic() {
       } else {
         // Fallback to calculating the angle delta
         Twist2d twist = kinematics.toTwist2d(moduleDeltas);
+        // Use the angle delta from the kinematics and module deltas
         rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
       }
 
+      var chassisSpeedsDiff = getChassisSpeeds().minus(lastChassisSpeeds);
+      var xAccel = chassisSpeedsDiff.vxMetersPerSecond / 0.02;
+      xAcceleration.addLast(xAccel);
+      var yAccel = chassisSpeedsDiff.vyMetersPerSecond / 0.02;
+      yAcceleration.addLast(yAccel);
+      var avgX = BufferUtil.average(xAcceleration);
+      var avgY = BufferUtil.average(yAcceleration);
+      Logger.recordOutput("FilteredAccelerationX", avgX);
+      Logger.recordOutput("AccelerationX", chassisSpeedsDiff.vxMetersPerSecond / 0.02);
+      Logger.recordOutput("AccelerationY", chassisSpeedsDiff.vyMetersPerSecond / 0.02);
+      Logger.recordOutput(
+          "Difference",
+          Math.abs(chassisSpeedsDiff.vxMetersPerSecond / 0.02 - gyroInputs.xAcceleration));
+      var xDiff = Math.abs(avgX - gyroIO.xAcceleration());
+      Logger.recordOutput("FilteredDifference", xDiff);
+      var yDiff = Math.abs(avgY - gyroIO.yAcceleration());
+      if (xDiff < 1) {
+        xDiff = 1;
+      }
+      if (yDiff < 1) {
+        yDiff = 1;
+      }
       // Apply update
       poseEstimator.updateWithTime(
-          modulePositions, rawGyroRotation, sampleTimestamps[i], VecBuilder.fill(1, 1, 1));
+          modulePositions,
+          rawGyroRotation,
+          sampleTimestamps[i],
+          VecBuilder.fill(1 / xDiff, 1 / yDiff, 1));
     }
 
     Logger.recordOutput("Drive/xPID/setpoint", xController.getSetpoint());
@@ -212,6 +243,7 @@ public void periodic() {
 
     // Update gyro alert
     gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM);
+    lastChassisSpeeds = getChassisSpeeds();
   }
 
   /**

diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIO.java
@@ -15,6 +15,7 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.AutoLogOutput;
 
 public interface GyroIO {
   @AutoLog
@@ -24,7 +25,19 @@ public static class GyroIOInputs {
     public double yawVelocityRadPerSec = 0.0;
     public double[] odometryYawTimestamps = new double[] {};
     public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+    public double xAcceleration = 0;
+    public double yAcceleration = 0;
   }
 
   public default void updateInputs(GyroIOInputs inputs) {}
+
+  @AutoLogOutput(key = "Gyro/FilteredXAcceleration")
+  public default double xAcceleration() {
+    return 0;
+  }
+
+  @AutoLogOutput(key = "Gyro/FilteredYAcceleration")
+  public default double yAcceleration() {
+    return 0;
+  }
 }
diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIOPigeon2.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIOPigeon2.java
@@ -13,6 +13,7 @@
 
 package org.curtinfrc.frc2025.subsystems.drive;
 
+import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
 import static org.curtinfrc.frc2025.subsystems.drive.DriveConstants.*;
 
 import com.ctre.phoenix6.BaseStatusSignal;
@@ -24,8 +25,11 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearAcceleration;
+import edu.wpi.first.util.CircularBuffer;
 import java.util.Queue;
 import org.curtinfrc.frc2025.generated.TunerConstants;
+import org.curtinfrc.frc2025.util.BufferUtil;
 
 /** IO implementation for Pigeon 2. */
 public class GyroIOPigeon2 implements GyroIO {
@@ -34,9 +38,13 @@ public class GyroIOPigeon2 implements GyroIO {
           TunerConstants.DrivetrainConstants.Pigeon2Id,
           TunerConstants.DrivetrainConstants.CANBusName);
   private final StatusSignal<Angle> yaw = pigeon.getYaw();
+  private final StatusSignal<LinearAcceleration> xAcceleration = pigeon.getAccelerationX();
+  private final StatusSignal<LinearAcceleration> yAcceleration = pigeon.getAccelerationY();
   private final Queue<Double> yawPositionQueue;
   private final Queue<Double> yawTimestampQueue;
   private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
+  private final CircularBuffer<Double> xAccelerationBuffer = new CircularBuffer<>(6);
+  private final CircularBuffer<Double> yAccelerationBuffer = new CircularBuffer<>(6);
 
   public GyroIOPigeon2() {
     pigeon.getConfigurator().apply(new Pigeon2Configuration());
@@ -50,7 +58,9 @@ public GyroIOPigeon2() {
 
   @Override
   public void updateInputs(GyroIOInputs inputs) {
-    inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+    inputs.connected =
+        BaseStatusSignal.refreshAll(yaw, yawVelocity, xAcceleration, yAcceleration)
+            .equals(StatusCode.OK);
     inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
     inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
 
@@ -60,7 +70,21 @@ public void updateInputs(GyroIOInputs inputs) {
         yawPositionQueue.stream()
             .map((Double value) -> Rotation2d.fromDegrees(value))
             .toArray(Rotation2d[]::new);
+    inputs.xAcceleration = xAcceleration.getValue().in(MetersPerSecondPerSecond);
+    xAccelerationBuffer.addLast(inputs.xAcceleration);
+    inputs.yAcceleration = yAcceleration.getValue().in(MetersPerSecondPerSecond);
+    yAccelerationBuffer.addLast(inputs.yAcceleration);
     yawTimestampQueue.clear();
     yawPositionQueue.clear();
   }
+
+  @Override
+  public double xAcceleration() {
+    return BufferUtil.average(xAccelerationBuffer);
+  }
+
+  @Override
+  public double yAcceleration() {
+    return BufferUtil.average(yAccelerationBuffer);
+  }
 }
diff --git a/src/main/java/org/curtinfrc/frc2025/util/BufferUtil.java b/src/main/java/org/curtinfrc/frc2025/util/BufferUtil.java
@@ -0,0 +1,15 @@
+package org.curtinfrc.frc2025.util;
+
+import edu.wpi.first.util.CircularBuffer;
+
+public class BufferUtil {
+  private BufferUtil() {}
+
+  public static Double average(CircularBuffer<Double> buffer) {
+    Double total = 0.0;
+    for (var i = 0; i < buffer.size(); i++) {
+      total += buffer.get(i);
+    }
+    return total;
+  }
+}