works better

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
CurtinFRC · Jan 28, 2025 · 7d84222 · 7d84222
1 parent af3b0c2
commit 7d84222
Show file tree

Hide file tree

Showing 6 changed files with 93 additions and 11 deletions.
diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java
@@ -30,7 +30,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-  public static final RobotType robotType = RobotType.SIMBOT;
+  public static final RobotType robotType = RobotType.DEVBOT;
   public static final double ROBOT_X = 550; // mm
   public static final double ROBOT_Y = 570;
 

diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java
@@ -30,7 +30,6 @@
 import org.curtinfrc.frc2025.subsystems.ejector.EjectorIOSim;
 import org.curtinfrc.frc2025.subsystems.elevator.Elevator;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIO;
-import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIONeoMaxMotionLaserCAN;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIOSim;
 import org.curtinfrc.frc2025.subsystems.intake.Intake;
 import org.curtinfrc.frc2025.subsystems.intake.IntakeIO;
@@ -158,12 +157,12 @@ public Robot() {
           vision =
               new Vision(
                   drive::addVisionMeasurement,
-                  new VisionIOLimelightGamepiece(camera0Name),
-                  new VisionIOLimelight(camera1Name, drive::getRotation),
-                  new VisionIOQuestNav());
-          elevator = new Elevator(new ElevatorIONeoMaxMotionLaserCAN());
-          intake = new Intake(new IntakeIONEO());
-          ejector = new Ejector(new EjectorIONEO());
+                  new VisionIO() {},
+                  new VisionIO() {},
+                  new VisionIO() {});
+          elevator = new Elevator(new ElevatorIO() {});
+          intake = new Intake(new IntakeIO() {});
+          ejector = new Ejector(new EjectorIO() {});
         }
 
         case SIMBOT -> {

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;
@@ -49,6 +50,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;
@@ -74,6 +76,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 final PIDController xController = new PIDController(10.0, 0.0, 0.0);
   private final PIDController yController = new PIDController(10.0, 0.0, 0.0);
@@ -170,18 +175,44 @@ public void periodic() {
         // Use the real gyro angle
         rawGyroRotation = gyroInputs.odometryYawPositions[i];
       } else {
-        // Use the angle delta from the kinematics and module deltas
         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));
     }
 
     // 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;
+  }
+}