From 3eea62a655cf7405074fe24386652de2c9e3ac0b Mon Sep 17 00:00:00 2001 From: spderman3333 Date: Thu, 21 Aug 2025 19:27:21 -0700 Subject: [PATCH 1/6] Added TODO list for the constants updation (In Drive) --- .../java/com/team2813/subsystems/Drive.java | 42 +++++++++++++++---- 1 file changed, 33 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 4c602e2f..84b50c5e 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -57,9 +57,26 @@ import org.photonvision.targeting.PhotonTrackedTarget; /** This is the Drive. His name is Gary. Please be kind to him and say hi. Have a nice day! */ + +/* +TODO: The compiled list of lines that MIGHT need changing. + 78) Max velocity. + 79) Max rotations per second. + 97) Wheel radius. + 102) Distance from robot center to front (and back). + 103) Distance from robot center to left (and right). + 106) Replace the camera transforms to the cameras on Dr. Womp + 219) Change the camera names to the one on Dr. Womp. + 231) Reset steer offsets. + 225) Change steer PID values. + 242) Change drive PID values. + 255) Change the canID to the canivore on Dr. Womp + 261) Change the Drive and Steer Gear ratio to what is on Dr. Womp (I think we are using MK4i's so it might not need to be changed). + */ + public class Drive extends SubsystemBase implements AutoCloseable { - private static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = 6; - private static final double DEFAULT_MAX_ROTATIONS_PER_SECOND = 1.2; + private static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = 6; // TODO: Get max velocity now since we have the camera cage. + private static final double DEFAULT_MAX_ROTATIONS_PER_SECOND = 1.2; // TODO: Same here. private static final Matrix PHOTON_MULTIPLE_TAG_STD_DEVS = new Matrix<>(Nat.N3(), Nat.N1(), new double[] {0.1, 0.1, 0.1}); private final RobotLocalization localization; @@ -77,14 +94,17 @@ public class Drive extends SubsystemBase implements AutoCloseable { NetworkTableInstance.getDefault().getDoubleTopic("Ambiguity").publish(); /** This measurement is IN INCHES */ - private static final double WHEEL_RADIUS_IN = 1.875; + private static final double WHEEL_RADIUS_IN = 1.875; // TODO: Change this to Dr. Womp's wheel radius. private double multiplier = 1; private double lastVisionEstimateTime = -1; - static final double FRONT_DIST = 0.330200; - static final double LEFT_DIST = 0.330200; + static final double FRONT_DIST = 0.330200; // TODO: Set this for Dr. Womp. + static final double LEFT_DIST = 0.330200; //TODO: Set this for Dr. Womp. + /* + TODO: Remove all of the camera transforms and set them with the new cameras. + */ /** * The transformation for the {@code captain-barnacles} PhotonVision camera. This camera faces the * front @@ -95,7 +115,7 @@ public class Drive extends SubsystemBase implements AutoCloseable { 0.2939800826, 0.1708140348, new Rotation3d(0, -0.1745329252, -0.5235987756)); - + // See above note /** * The transformation for the {@code professor-inking} PhotonVision camera. This camera faces the * back @@ -104,8 +124,6 @@ public class Drive extends SubsystemBase implements AutoCloseable { new Transform3d( 0.0584240386, 0.2979761884, 0.1668812004, new Rotation3d(0, 0, 0.1745329252 + Math.PI)); - // See above comment, do not delete past this line. - /** * Configurable values for the {@code Drive} subsystem * @@ -198,16 +216,19 @@ public Drive( new MultiPhotonPoseEstimator.Builder( networkTableInstance, aprilTagFieldLayout, config.poseStrategy()) // should have named our batteries after Octonauts characters >:( + // TODO: Replace these cameras with the ones on Dr. Womp, same as the transforms. .addCamera("capt-barnacles", captBarnaclesTransform, "Front PhotonVision camera") .addCamera("professor-inkling", professorInklingTransform, "Back PhotonVision camera") .build(); this.config = config; + // TODO: Retune these. double FLSteerOffset = 0.22021484375; double FRSteerOffset = 1.917236; double BLSteerOffset = -0.367919921875; double BRSteerOffset = -0.258544921875; + // TODO: consider retuning PID if we want to. Slot0Configs steerGains = new Slot0Configs() .withKP(50) @@ -218,6 +239,7 @@ public Drive( .withKA(0.084645); // Tune this. // l: 0 h: 10 + // TODO: consider retuning PID if we want to. Slot0Configs driveGains = new Slot0Configs() .withKP(2.5) @@ -230,12 +252,13 @@ public Drive( SwerveDrivetrainConstants drivetrainConstants = new SwerveDrivetrainConstants() .withPigeon2Id(PIGEON_ID) - .withCANBusName("swerve"); // README: tweak to actual pigeon and CanBusName + .withCANBusName("swerve"); // TODO: tweak to actual pigeon and CanBusName SwerveModuleConstantsFactory constantCreator = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + // TODO: Yes, remember this. // WARNING: TUNE ALL OF THESE THINGS!!!!!! .withDriveMotorGearRatio(6.75) .withSteerMotorGearRatio(150.0 / 7) @@ -479,6 +502,7 @@ public ChassisSpeeds getRobotRelativeSpeeds() { private static final Matrix LIMELIGHT_STD_DEVS = new Matrix<>(Nat.N3(), Nat.N1(), new double[] {0.9, 0.9, 0.9}); + // Note: We might want to look in here for Dr. Womp public void addVisionMeasurement(BotPoseEstimate estimate) { double estimateTimestamp = estimate.timestampSeconds(); if (estimateTimestamp > lastVisionEstimateTime) { From d822bbee264559cef25b0267bb6fe4c691abced9 Mon Sep 17 00:00:00 2001 From: spderman3333 Date: Sat, 23 Aug 2025 12:25:27 -0700 Subject: [PATCH 2/6] Reconfigured drive variables, and encoder offsets + motor inversion. --- .idea/vcs.xml | 2 - .../java/com/team2813/subsystems/Drive.java | 63 ++++++++++--------- 2 files changed, 33 insertions(+), 32 deletions(-) diff --git a/.idea/vcs.xml b/.idea/vcs.xml index 553f0ba9..83f3d4c1 100644 --- a/.idea/vcs.xml +++ b/.idea/vcs.xml @@ -2,8 +2,6 @@ - - \ No newline at end of file diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 84b50c5e..0d6a2cd7 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -60,14 +60,14 @@ /* TODO: The compiled list of lines that MIGHT need changing. - 78) Max velocity. - 79) Max rotations per second. - 97) Wheel radius. - 102) Distance from robot center to front (and back). - 103) Distance from robot center to left (and right). + 78) Max velocity. - later + 79) Max rotations per second. -later + 97) Wheel radius. [DONE] + 102) Distance from robot center to front (and back). [DONE] + 103) Distance from robot center to left (and right). [DONE] 106) Replace the camera transforms to the cameras on Dr. Womp 219) Change the camera names to the one on Dr. Womp. - 231) Reset steer offsets. + 231) Reset steer offsets. [DONE] 225) Change steer PID values. 242) Change drive PID values. 255) Change the canID to the canivore on Dr. Womp @@ -75,7 +75,8 @@ */ public class Drive extends SubsystemBase implements AutoCloseable { - private static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = 6; // TODO: Get max velocity now since we have the camera cage. + private static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = + 6; // TODO: Get max velocity now since we have the camera cage. private static final double DEFAULT_MAX_ROTATIONS_PER_SECOND = 1.2; // TODO: Same here. private static final Matrix PHOTON_MULTIPLE_TAG_STD_DEVS = new Matrix<>(Nat.N3(), Nat.N1(), new double[] {0.1, 0.1, 0.1}); @@ -94,17 +95,18 @@ public class Drive extends SubsystemBase implements AutoCloseable { NetworkTableInstance.getDefault().getDoubleTopic("Ambiguity").publish(); /** This measurement is IN INCHES */ - private static final double WHEEL_RADIUS_IN = 1.875; // TODO: Change this to Dr. Womp's wheel radius. + private static final double WHEEL_RADIUS_IN = + 1.537; // TODO: Change this to Dr. Womp's wheel radius. [DONE] private double multiplier = 1; private double lastVisionEstimateTime = -1; - static final double FRONT_DIST = 0.330200; // TODO: Set this for Dr. Womp. - static final double LEFT_DIST = 0.330200; //TODO: Set this for Dr. Womp. + static final double FRONT_DIST = 0.4064; // TODO: Set this for Dr. Womp. [DONE] + static final double LEFT_DIST = 0.3302; // TODO: Set this for Dr. Womp. [DONE] /* - TODO: Remove all of the camera transforms and set them with the new cameras. - */ + TODO: Remove all of the camera transforms and set them with the new cameras. + */ /** * The transformation for the {@code captain-barnacles} PhotonVision camera. This camera faces the * front @@ -115,6 +117,7 @@ public class Drive extends SubsystemBase implements AutoCloseable { 0.2939800826, 0.1708140348, new Rotation3d(0, -0.1745329252, -0.5235987756)); + // See above note /** * The transformation for the {@code professor-inking} PhotonVision camera. This camera faces the @@ -222,11 +225,11 @@ public Drive( .build(); this.config = config; - // TODO: Retune these. - double FLSteerOffset = 0.22021484375; - double FRSteerOffset = 1.917236; - double BLSteerOffset = -0.367919921875; - double BRSteerOffset = -0.258544921875; + // TODO: Retune these. [DONE] + double FLSteerOffset = -0.240234; + double FRSteerOffset = -0.011475; + double BLSteerOffset = -0.108887; + double BRSteerOffset = -0.148438; // TODO: consider retuning PID if we want to. Slot0Configs steerGains = @@ -252,7 +255,7 @@ public Drive( SwerveDrivetrainConstants drivetrainConstants = new SwerveDrivetrainConstants() .withPigeon2Id(PIGEON_ID) - .withCANBusName("swerve"); // TODO: tweak to actual pigeon and CanBusName + .withCANBusName("swerve"); // TODO: tweak to actual pigeon and CanBusName [DONE] SwerveModuleConstantsFactory constantCreator = @@ -283,9 +286,9 @@ public Drive( FLSteerOffset, FRONT_DIST, LEFT_DIST, - true, // May need to change later. - true, // May need to change later. - false); // May need to change later. + false, + true, + false); SwerveModuleConstants frontRight = constantCreator.createModuleConstants( @@ -295,9 +298,9 @@ public Drive( FRSteerOffset, FRONT_DIST, -LEFT_DIST, - true, // May need to change later. - true, // May need to change later. - false); // May need to change later. + false, + true, + false); SwerveModuleConstants backLeft = constantCreator.createModuleConstants( @@ -307,9 +310,9 @@ public Drive( BLSteerOffset, -FRONT_DIST, LEFT_DIST, - true, // May need to change later. - true, // May need to change later. - false); // May need to change later. + false, + true, + false); SwerveModuleConstants backRight = constantCreator.createModuleConstants( @@ -319,9 +322,9 @@ public Drive( BRSteerOffset, -FRONT_DIST, -LEFT_DIST, - true, // May need to change later. - true, // May need to change later. - false); // May need to change later. + false, + true, + false); SwerveModuleConstants[] modules = {frontLeft, frontRight, backLeft, backRight}; drivetrain = new SwerveDrivetrain<>( From 34d3f1b056187457c6c4b95124bdbcfe5a1dcc63 Mon Sep 17 00:00:00 2001 From: spderman3333 Date: Sat, 23 Aug 2025 17:54:49 -0700 Subject: [PATCH 3/6] Ran :spotlessApply --- src/main/java/com/team2813/subsystems/Drive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 0d6a2cd7..35925a5c 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -288,7 +288,7 @@ public Drive( LEFT_DIST, false, true, - false); + false); SwerveModuleConstants frontRight = constantCreator.createModuleConstants( @@ -300,7 +300,7 @@ public Drive( -LEFT_DIST, false, true, - false); + false); SwerveModuleConstants backLeft = constantCreator.createModuleConstants( @@ -312,7 +312,7 @@ public Drive( LEFT_DIST, false, true, - false); + false); SwerveModuleConstants backRight = constantCreator.createModuleConstants( @@ -324,7 +324,7 @@ public Drive( -LEFT_DIST, false, true, - false); + false); SwerveModuleConstants[] modules = {frontLeft, frontRight, backLeft, backRight}; drivetrain = new SwerveDrivetrain<>( From d37caa2208bf0ae85f65a0bac168b2c146d531f6 Mon Sep 17 00:00:00 2001 From: spderman3333 Date: Sat, 23 Aug 2025 18:29:44 -0700 Subject: [PATCH 4/6] Configured offsets for the new cameras, as well as adding them to vision --- .idea/codeStyles/Project.xml | 3 +- .../java/com/team2813/subsystems/Drive.java | 58 +++++++++++-------- 2 files changed, 36 insertions(+), 25 deletions(-) diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml index c7bc7637..340e122a 100644 --- a/.idea/codeStyles/Project.xml +++ b/.idea/codeStyles/Project.xml @@ -7,7 +7,8 @@ - + \ No newline at end of file diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 35925a5c..4537bfa4 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -1,8 +1,22 @@ package com.team2813.subsystems; -import static com.team2813.Constants.*; +import static com.team2813.Constants.BACK_LEFT_DRIVE_ID; +import static com.team2813.Constants.BACK_LEFT_ENCODER_ID; +import static com.team2813.Constants.BACK_LEFT_STEER_ID; +import static com.team2813.Constants.BACK_RIGHT_DRIVE_ID; +import static com.team2813.Constants.BACK_RIGHT_ENCODER_ID; +import static com.team2813.Constants.BACK_RIGHT_STEER_ID; import static com.team2813.Constants.DriverConstants.DRIVER_CONTROLLER; +import static com.team2813.Constants.FRONT_LEFT_DRIVE_ID; +import static com.team2813.Constants.FRONT_LEFT_ENCODER_ID; +import static com.team2813.Constants.FRONT_LEFT_STEER_ID; +import static com.team2813.Constants.FRONT_RIGHT_DRIVE_ID; +import static com.team2813.Constants.FRONT_RIGHT_ENCODER_ID; +import static com.team2813.Constants.FRONT_RIGHT_STEER_ID; +import static com.team2813.Constants.PIGEON_ID; import static com.team2813.lib2813.util.ControlUtils.deadband; +import static edu.wpi.first.units.Units.Centimeters; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.Utils; @@ -33,12 +47,21 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.networktables.*; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; @@ -104,28 +127,15 @@ public class Drive extends SubsystemBase implements AutoCloseable { static final double FRONT_DIST = 0.4064; // TODO: Set this for Dr. Womp. [DONE] static final double LEFT_DIST = 0.3302; // TODO: Set this for Dr. Womp. [DONE] - /* - TODO: Remove all of the camera transforms and set them with the new cameras. - */ - /** - * The transformation for the {@code captain-barnacles} PhotonVision camera. This camera faces the - * front - */ - private static final Transform3d captBarnaclesTransform = + private static final Transform3d leftColorTransform = new Transform3d( - 0.1688157406, - 0.2939800826, - 0.1708140348, - new Rotation3d(0, -0.1745329252, -0.5235987756)); + new Translation3d(Centimeters.of(31), Centimeters.of(21), Centimeters.of(20)), + new Rotation3d(Degrees.of(0), Degrees.of(-30), Degrees.of(-30))); - // See above note - /** - * The transformation for the {@code professor-inking} PhotonVision camera. This camera faces the - * back - */ - private static final Transform3d professorInklingTransform = + private static final Transform3d rightColorTransform = new Transform3d( - 0.0584240386, 0.2979761884, 0.1668812004, new Rotation3d(0, 0, 0.1745329252 + Math.PI)); + new Translation3d(Centimeters.of(29), Centimeters.of(-23), Centimeters.of(13)), + new Rotation3d(Degrees.of(0), Degrees.of(-20), Degrees.of(30))); /** * Configurable values for the {@code Drive} subsystem @@ -220,8 +230,8 @@ public Drive( networkTableInstance, aprilTagFieldLayout, config.poseStrategy()) // should have named our batteries after Octonauts characters >:( // TODO: Replace these cameras with the ones on Dr. Womp, same as the transforms. - .addCamera("capt-barnacles", captBarnaclesTransform, "Front PhotonVision camera") - .addCamera("professor-inkling", professorInklingTransform, "Back PhotonVision camera") + .addCamera("left_color", leftColorTransform, "Left PhotonVision camera") + .addCamera("right_color", rightColorTransform, "Right PhotonVision camera") .build(); this.config = config; From b3388acd44f61ac40369c1861dbbd26a803f5381 Mon Sep 17 00:00:00 2001 From: spderman3333 Date: Fri, 29 Aug 2025 12:13:50 -0700 Subject: [PATCH 5/6] Removed completed todo comments. --- .../java/com/team2813/subsystems/Drive.java | 32 +++++-------------- 1 file changed, 8 insertions(+), 24 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 4537bfa4..60576200 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -83,18 +83,10 @@ /* TODO: The compiled list of lines that MIGHT need changing. - 78) Max velocity. - later - 79) Max rotations per second. -later - 97) Wheel radius. [DONE] - 102) Distance from robot center to front (and back). [DONE] - 103) Distance from robot center to left (and right). [DONE] - 106) Replace the camera transforms to the cameras on Dr. Womp - 219) Change the camera names to the one on Dr. Womp. - 231) Reset steer offsets. [DONE] - 225) Change steer PID values. - 242) Change drive PID values. - 255) Change the canID to the canivore on Dr. Womp - 261) Change the Drive and Steer Gear ratio to what is on Dr. Womp (I think we are using MK4i's so it might not need to be changed). + Max velocity. - later + Max rotations per second. -later + Change steer PID values. + Change drive PID values. */ public class Drive extends SubsystemBase implements AutoCloseable { @@ -118,14 +110,13 @@ public class Drive extends SubsystemBase implements AutoCloseable { NetworkTableInstance.getDefault().getDoubleTopic("Ambiguity").publish(); /** This measurement is IN INCHES */ - private static final double WHEEL_RADIUS_IN = - 1.537; // TODO: Change this to Dr. Womp's wheel radius. [DONE] + private static final double WHEEL_RADIUS_IN = 1.537; private double multiplier = 1; private double lastVisionEstimateTime = -1; - static final double FRONT_DIST = 0.4064; // TODO: Set this for Dr. Womp. [DONE] - static final double LEFT_DIST = 0.3302; // TODO: Set this for Dr. Womp. [DONE] + static final double FRONT_DIST = 0.4064; + static final double LEFT_DIST = 0.3302; private static final Transform3d leftColorTransform = new Transform3d( @@ -228,14 +219,11 @@ public Drive( photonPoseEstimator = new MultiPhotonPoseEstimator.Builder( networkTableInstance, aprilTagFieldLayout, config.poseStrategy()) - // should have named our batteries after Octonauts characters >:( - // TODO: Replace these cameras with the ones on Dr. Womp, same as the transforms. .addCamera("left_color", leftColorTransform, "Left PhotonVision camera") .addCamera("right_color", rightColorTransform, "Right PhotonVision camera") .build(); this.config = config; - // TODO: Retune these. [DONE] double FLSteerOffset = -0.240234; double FRSteerOffset = -0.011475; double BLSteerOffset = -0.108887; @@ -263,16 +251,12 @@ public Drive( .withKA(0); // Tune this. SwerveDrivetrainConstants drivetrainConstants = - new SwerveDrivetrainConstants() - .withPigeon2Id(PIGEON_ID) - .withCANBusName("swerve"); // TODO: tweak to actual pigeon and CanBusName [DONE] + new SwerveDrivetrainConstants().withPigeon2Id(PIGEON_ID).withCANBusName("swerve"); SwerveModuleConstantsFactory constantCreator = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - // TODO: Yes, remember this. - // WARNING: TUNE ALL OF THESE THINGS!!!!!! .withDriveMotorGearRatio(6.75) .withSteerMotorGearRatio(150.0 / 7) .withWheelRadius(Units.Inches.of(WHEEL_RADIUS_IN)) From 5b02cbe8ad8153c52c0897bbec15e868784f9758 Mon Sep 17 00:00:00 2001 From: spderman3333 Date: Fri, 29 Aug 2025 17:51:24 -0700 Subject: [PATCH 6/6] Removed addVisionMeasurement() --- .../java/com/team2813/subsystems/Drive.java | 21 ------------------- 1 file changed, 21 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 60576200..2dba229d 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -39,7 +39,6 @@ import com.google.auto.value.AutoBuilder; import com.team2813.commands.DefaultDriveCommand; import com.team2813.commands.RobotLocalization; -import com.team2813.lib2813.limelight.BotPoseEstimate; import com.team2813.lib2813.preferences.PreferencesInjector; import com.team2813.sysid.SwerveSysidRequest; import com.team2813.vision.MultiPhotonPoseEstimator; @@ -70,7 +69,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.List; -import java.util.Optional; import java.util.function.DoubleSupplier; import java.util.stream.IntStream; import org.photonvision.EstimatedRobotPose; @@ -499,21 +497,6 @@ public ChassisSpeeds getRobotRelativeSpeeds() { private static final Matrix LIMELIGHT_STD_DEVS = new Matrix<>(Nat.N3(), Nat.N1(), new double[] {0.9, 0.9, 0.9}); - // Note: We might want to look in here for Dr. Womp - public void addVisionMeasurement(BotPoseEstimate estimate) { - double estimateTimestamp = estimate.timestampSeconds(); - if (estimateTimestamp > lastVisionEstimateTime) { - // Per the JavaDoc for addVisionMeasurement(), only add vision measurements that are already - // within one meter or so of the current odometry pose estimate. - Pose2d drivePose = getPose(); - var distance = drivePose.getTranslation().getDistance(estimate.pose().getTranslation()); - if (Math.abs(distance) <= config.maxLimelightDifferenceMeters()) { - drivetrain.addVisionMeasurement(estimate.pose(), estimateTimestamp, LIMELIGHT_STD_DEVS); - lastVisionEstimateTime = estimateTimestamp; - } - } - } - private void handlePhotonPose(EstimatedRobotPose estimate) { if (!config.usePhotonVisionLocation) { return; @@ -547,10 +530,6 @@ public void periodic() { // Note: we call limelightLocation() even if config.addLimelightMeasurement is false so // the position is published to network tables, which allows us to view the limelight's // pose estimate in AdvantageScope. - Optional limelightEstimate = localization.limelightLocation(); - if (config.addLimelightMeasurement) { - limelightEstimate.ifPresent(this::addVisionMeasurement); - } // Publish data to NetworkTables expectedStatePublisher.set(drivetrain.getState().ModuleTargets);