From 7f1c0048302f8d922ccc36905ffc2d9787d607b8 Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Wed, 1 Apr 2026 20:19:00 -0700 Subject: [PATCH] revert drive changes + ensure drive constant dimensions I basically grabbed the working code from vc scrimmage on 3/31 at the very end and slapped it in there. Other drive changes are subtle yet we remeasured today so good to fix --- .../constants/swerve/DriveConstants.java | 11 +- .../subsystems/drivetrain/Drivetrain.java | 111 +++--------------- 2 files changed, 20 insertions(+), 102 deletions(-) diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java index 606cc6a..520df05 100644 --- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -23,9 +23,9 @@ public class DriveConstants { *

* The frame width is 26.5 inches, and each bumper is 3.25 inches. */ - public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.832; + public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.83185; // 32.75 inches in meters - public static double ROBOT_MASS = Units.lbsToKilograms(110); + public static double ROBOT_MASS = Units.lbsToKilograms(110 + 13 + 13.4 + 5.0); /** Radius of the drive wheels [meters]. */ public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95); @@ -34,6 +34,7 @@ public class DriveConstants { /** Distance between the left and right wheels [meters]. */ + // from center of wheels btw public static double TRACK_WIDTH = Units.inchesToMeters(20.75);//22.75 swerve bot, 20.75 comp bot // Mk4i gear ratios @@ -129,8 +130,8 @@ public class DriveConstants { public static final double STEER_PEAK_CURRENT_DURATION = 0.01; public static final boolean STEER_ENABLE_CURRENT_LIMIT = true; - public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 60; - public static final int DRIVE_PEAK_CURRENT_LIMIT = 60; + public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 40; + public static final int DRIVE_PEAK_CURRENT_LIMIT = 40; public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01; public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true; @@ -301,4 +302,4 @@ public class DriveConstants { } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 2b0fae7..55ac197 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.drivetrain; -import java.util.ArrayList; import java.util.Arrays; +import java.util.ArrayList; import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -21,7 +21,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; @@ -120,13 +119,6 @@ public class Drivetrain extends SubsystemBase { // for vision yaw correction private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator(); - - // gyro history for timestamp interpolation - private static final int GYRO_HISTORY_SIZE = 50; - private double[] gyroTimestampsHistory = new double[GYRO_HISTORY_SIZE]; // circular buffer of gyro timestamps for interpolation - private double[] gyroYawsHistory = new double[GYRO_HISTORY_SIZE]; // circular buffer of gyro yaw angles corresponding to timestamps - private int gyroHistoryHead = 0; // index of the most recent entry in the circular buffer - private int gyroHistoryCount = 0; // number of valid entries in the history buffer private final Field2d field = new Field2d(); @@ -315,10 +307,8 @@ public class Drivetrain extends SubsystemBase { } if (VisionConstants.ENABLED) { - recordGyroReading(gyroInputs.yawPosition.getRadians()); - if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) { - ArrayList visionPoses = vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped); + vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped); if (vision.canSeeTag()) { slipped = false; @@ -326,39 +316,33 @@ public class Drivetrain extends SubsystemBase { double currentGyroYaw = gyroInputs.yawPosition.getRadians(); + // to compare bias + ArrayList visionPoses = vision.getEstimatedPoses(getPose()); + for (EstimatedRobotPose visionPose : visionPoses) { if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) { - // get yaw from the vision pose estimate double visionYaw = visionPose.estimatedPose.getRotation().getZ(); - // interpolate gyro yaw to vision timestamp for accurate comparison + // gets at vision timestamp, not current gyro yaw double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds); if (!Double.isNaN(gyroYawAtTimestamp)) { - // wrap gyro yaw - while (gyroYawAtTimestamp > Math.PI) { - gyroYawAtTimestamp -= 2 * Math.PI; - } - while (gyroYawAtTimestamp < -Math.PI) { - gyroYawAtTimestamp += 2 * Math.PI; - } Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp)); Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw)); - // add observation to estimator with default weight + // use weighted observation gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0); } } } - // apply correction when we have enough samples + // check if we have enough samples if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) { double fullBias = gyroBiasEstimator.getAndResetBias(); double bias = gyroBiasEstimator.applyPartialCorrection(fullBias); - Logger.recordOutput("FullBias", fullBias); - Logger.recordOutput("CorrectedBias", bias); + System.out.println("bias: " + bias); + System.out.println("FullBias"+ fullBias); - // only apply if bias exceeds min threshold if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) { gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias)); } @@ -600,21 +584,6 @@ public class Drivetrain extends SubsystemBase { return modules; } - /** - * records a gyro reading with timestamp for later interpolation. - * uses a circular buffer to store recent gyro readings. - * - * @param yaw the gyro yaw in radians - */ - private void recordGyroReading(double yaw) { - gyroTimestampsHistory[gyroHistoryHead] = Timer.getFPGATimestamp(); - gyroYawsHistory[gyroHistoryHead] = yaw; - gyroHistoryHead = (gyroHistoryHead + 1) % GYRO_HISTORY_SIZE; - if (gyroHistoryCount < GYRO_HISTORY_SIZE) { - gyroHistoryCount++; - } - } - /** * gets gyro yaw at a specific timestamp with interpolation * this is used for timestamp-synchronized gyro/vision comparison. @@ -622,60 +591,9 @@ public class Drivetrain extends SubsystemBase { * @param timestampSeconds the timestamp to get the gyro yaw at * @return the gyro yaw in radians, or Double.NaN if no valid data */ - private double getGyroYawAtTimestamp(double timestampSeconds) { - if (gyroHistoryCount < 2) { - return Double.NaN; - } - - // find two closest timestamps and interpolate - int oldestIndex = (gyroHistoryHead - gyroHistoryCount + GYRO_HISTORY_SIZE) % GYRO_HISTORY_SIZE; - int newestIndex = (gyroHistoryHead - 1 + GYRO_HISTORY_SIZE) % GYRO_HISTORY_SIZE; - double oldestTs = gyroTimestampsHistory[oldestIndex]; - double newestTs = gyroTimestampsHistory[newestIndex]; - - // if timestamp is outside our range, use nearest - if (timestampSeconds <= oldestTs) { - return gyroYawsHistory[oldestIndex]; - } - if (timestampSeconds >= newestTs) { - return gyroYawsHistory[newestIndex]; - } - - int prevIndex = -1; - int nextIndex = -1; - int idx = oldestIndex; - for (int i = 0; i < gyroHistoryCount; i++) { - double ts = gyroTimestampsHistory[idx]; - if (ts <= timestampSeconds) { - prevIndex = idx; - } - if (ts >= timestampSeconds) { - nextIndex = idx; - break; - } - idx = (idx + 1) % GYRO_HISTORY_SIZE; - } - - if (prevIndex == -1) { - return gyroYawsHistory[oldestIndex]; - } - if (nextIndex == -1) { - return gyroYawsHistory[newestIndex]; - } - - double t0 = gyroTimestampsHistory[prevIndex]; - double t1 = gyroTimestampsHistory[nextIndex]; - if (prevIndex == nextIndex || t1 == t0) { - return gyroYawsHistory[prevIndex]; - } - - double y0 = gyroYawsHistory[prevIndex]; - double y1 = gyroYawsHistory[nextIndex]; - - double alpha = (timestampSeconds - t0) / (t1 - t0); - - double delta = MathUtil.angleModulus(y1 - y0); - return y0 + alpha * delta; + private double + getGyroYawAtTimestamp(double timestampSeconds) { + return getPose().getRotation().getRadians(); } /** @@ -697,7 +615,6 @@ public class Drivetrain extends SubsystemBase { currentHeading = pose.getRotation().getRadians(); poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose); modulePoses.reset(); - gyroBiasEstimator.reset(); } /** @@ -898,4 +815,4 @@ public class Drivetrain extends SubsystemBase { state, state, state, state }, false); } -} +} \ No newline at end of file -- 2.39.5