From 1613e8deb0ff20fe7116bc1a587ac2a1256a77cb Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Tue, 28 Jan 2025 16:34:17 +0800 Subject: [PATCH] works better Signed-off-by: Jade Turner --- .../frc2025/subsystems/drive/Drive.java | 34 ++++++++++++++++++- .../frc2025/subsystems/drive/GyroIO.java | 13 +++++++ .../subsystems/drive/GyroIOPigeon2.java | 26 +++++++++++++- .../curtinfrc/frc2025/util/BufferUtil.java | 15 ++++++++ 4 files changed, 86 insertions(+), 2 deletions(-) create mode 100644 src/main/java/org/curtinfrc/frc2025/util/BufferUtil.java diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index 68635be4..361817c2 100644 --- 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 xAcceleration = new CircularBuffer(6); + private CircularBuffer yAcceleration = new CircularBuffer(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 index 1cddf9da..e6e9e3b2 100644 --- 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 index b26a2c84..2b4377a3 100644 --- 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 yaw = pigeon.getYaw(); + private final StatusSignal xAcceleration = pigeon.getAccelerationX(); + private final StatusSignal yAcceleration = pigeon.getAccelerationY(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final CircularBuffer xAccelerationBuffer = new CircularBuffer<>(6); + private final CircularBuffer 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 new file mode 100644 index 00000000..b12b0d31 --- /dev/null +++ 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 buffer) { + Double total = 0.0; + for (var i = 0; i < buffer.size(); i++) { + total += buffer.get(i); + } + return total; + } +}