From 9b19f39967e89a3355568d4d3c3a327772aaf234 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 13 Jan 2026 18:19:45 -0500 Subject: [PATCH 1/5] split odometry & vision pose estimates --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/Robot.java | 24 +++++----- src/main/java/frc/robot/auto/AutoMode.java | 4 +- .../frc/robot/commands/DriveCommands.java | 25 ++++++----- .../java/frc/robot/commands/PathCommands.java | 8 ++-- .../frc/robot/subsystems/drive/Drive.java | 44 ++++++++++++------- 6 files changed, 60 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2005300..9550bd0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,10 +65,10 @@ public static final class KrakenX60Constants { } public static final class AutoConstants { - public static final int kAllianceColorSelectorPort = 10; + public static final int kAllianceColorSelectorPort = 0; // max length is 8 - public static final int[] kAutonomousModeSelectorPorts = {11, 12, 13, 18, 19}; + public static final int[] kAutonomousModeSelectorPorts = {1, 2, 3}; } public static final class OIConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1e2831c..81e5ac7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -97,7 +97,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getPose, + drive::getOdometryPose, new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), @@ -119,15 +119,15 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getPose, + drive::getOdometryPose, new VisionIOPhotonVisionSim( - cameraFrontRightName, robotToFrontRightCamera, drive::getPose), + cameraFrontRightName, robotToFrontRightCamera, drive::getOdometryPose), new VisionIOPhotonVisionSim( - cameraFrontLeftName, robotToFrontLeftCamera, drive::getPose), + cameraFrontLeftName, robotToFrontLeftCamera, drive::getOdometryPose), new VisionIOPhotonVisionSim( - cameraBackRightName, robotToBackRightCamera, drive::getPose), + cameraBackRightName, robotToBackRightCamera, drive::getOdometryPose), new VisionIOPhotonVisionSim( - cameraBackLeftName, robotToBackLeftCamera, drive::getPose)); + cameraBackLeftName, robotToBackLeftCamera, drive::getOdometryPose)); break; case REPLAY: // Replaying a log @@ -149,7 +149,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getPose, + drive::getOdometryPose, new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, @@ -284,11 +284,7 @@ public void bindZorroDriver(int port) { zorroDriver .GIn() .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), - drive) + Commands.runOnce(() -> drive.setOdometryRotation(Rotation2d.kZero), drive) .ignoringDisable(true)); // Drive 1m forward while button A is held @@ -318,8 +314,8 @@ public void bindXboxDriver(int port) { .onTrue( Commands.runOnce( () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), + drive.setOdometryPose( + new Pose2d(drive.getOdometryPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/auto/AutoMode.java b/src/main/java/frc/robot/auto/AutoMode.java index 2a6e868..4441334 100644 --- a/src/main/java/frc/robot/auto/AutoMode.java +++ b/src/main/java/frc/robot/auto/AutoMode.java @@ -14,8 +14,8 @@ public abstract class AutoMode { public AutoMode(Drive drivetrain) { autoFactory = new AutoFactory( - drivetrain::getPose, - drivetrain::setPose, + drivetrain::getOdometryPose, + drivetrain::setOdometryPose, drivetrain::followTrajectory, false, drivetrain); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 35f49fe..b059635 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -99,8 +99,8 @@ public static Command joystickDrive( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation()); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation()); } drive.runVelocity(speeds); @@ -140,8 +140,8 @@ public static Command fieldRelativeJoystickDrive( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation())); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation())); }, drive) .withName("Field Relative Joystick Drive"); @@ -209,7 +209,8 @@ public static Command joystickDriveAtFixedOrientation( // Calculate angular speed double omega = angleController.calculate( - drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + drive.getOdometryRotation().getRadians(), + rotationSupplier.get().getRadians()); // Convert to field relative speeds & send command ChassisSpeeds speeds = @@ -221,14 +222,14 @@ public static Command joystickDriveAtFixedOrientation( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation())); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation())); }, drive) .withName("Fixed Orientation Joystick Drive") // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + .beforeStarting(() -> angleController.reset(drive.getOdometryRotation().getRadians())); } /** @@ -261,8 +262,8 @@ public static Command pointAtTarget( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, fieldRotatedSupplier.getAsBoolean() - ? drive.getRotation().plus(Rotation2d.kPi) - : drive.getRotation())); + ? drive.getOdometryRotation().plus(Rotation2d.kPi) + : drive.getOdometryRotation())); }, drive) .withName("Point At Target") @@ -365,14 +366,14 @@ public static Command wheelRadiusCharacterization(Drive drive) { Commands.runOnce( () -> { state.positions = drive.getWheelRadiusCharacterizationPositions(); - state.lastAngle = drive.getRotation(); + state.lastAngle = drive.getOdometryRotation(); state.gyroDelta = 0.0; }), // Update gyro delta Commands.run( () -> { - var rotation = drive.getRotation(); + var rotation = drive.getOdometryRotation(); state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); state.lastAngle = rotation; }) diff --git a/src/main/java/frc/robot/commands/PathCommands.java b/src/main/java/frc/robot/commands/PathCommands.java index 87ff9e0..0665598 100644 --- a/src/main/java/frc/robot/commands/PathCommands.java +++ b/src/main/java/frc/robot/commands/PathCommands.java @@ -38,7 +38,7 @@ public static Command goToTargetPose(Drive drive, Pose2d targetPose) { @Override public Command get() { List waypoints = - PathPlannerPath.waypointsFromPoses(drive.getPose(), targetPose); + PathPlannerPath.waypointsFromPoses(drive.getOdometryPose(), targetPose); return AutoBuilder.followPath(createPath(waypoints, targetPose.getRotation())); } @@ -62,7 +62,7 @@ public static Command dockToTargetPoint( new Supplier() { @Override public Command get() { - var initialPose = drive.getPose(); + var initialPose = drive.getOdometryPose(); var heading = targetPoint.minus(initialPose.getTranslation()).getAngle(); var finalPose = new Pose2d(targetPoint, heading); List waypoints = @@ -95,7 +95,7 @@ public static Command dockToTargetPose(Drive drive, Pose2d targetPose, Distance public Command get() { List waypoints = PathPlannerPath.waypointsFromPoses( - drive.getPose(), + drive.getOdometryPose(), targetPose.plus( new Transform2d(leadDistance.unaryMinus(), Meters.of(0), Rotation2d.kZero)), targetPose); @@ -118,7 +118,7 @@ public static Command advanceForward(Drive drive, Distance leadDistance) { new Supplier() { @Override public Command get() { - var initialPose = drive.getPose(); + var initialPose = drive.getOdometryPose(); var waypoints = PathPlannerPath.waypointsFromPoses( initialPose, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f525347..abb0d00 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -75,7 +75,9 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = + private SwerveDrivePoseEstimator odometryPose = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + private SwerveDrivePoseEstimator visionPose = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); private Boolean firstVisionEstimate = true; @@ -104,8 +106,8 @@ public Drive( // Configure AutoBuilder for PathPlanner AutoBuilder.configure( - this::getPose, - this::setPose, + this::getVisionPose, + this::setOdometryPose, this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( @@ -190,7 +192,8 @@ public void periodic() { } // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + odometryPose.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + visionPose.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } // Update gyro alert @@ -223,7 +226,7 @@ public void runVelocity(ChassisSpeeds speeds) { public void followTrajectory(SwerveSample sample) { // Get the current pose of the robot - Pose2d pose = getPose(); + Pose2d pose = getOdometryPose(); // Generate the next speeds for the robot ChassisSpeeds speeds = @@ -234,7 +237,7 @@ public void followTrajectory(SwerveSample sample) { + headingController.calculate(pose.getRotation().getRadians(), sample.heading)); // Apply the generated speeds - runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getRotation())); + runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getOdometryRotation())); } /** Runs the drive in a straight line with the specified drive output. */ @@ -318,19 +321,30 @@ public double getFFCharacterizationVelocity() { } /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); + @AutoLogOutput(key = "Localizer/Odometry") + public Pose2d getOdometryPose() { + return odometryPose.getEstimatedPosition(); + } + + /** Returns the current vision pose. */ + @AutoLogOutput(key = "Localizer/Vision") + public Pose2d getVisionPose() { + return visionPose.getEstimatedPosition(); } /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); + public Rotation2d getOdometryRotation() { + return getOdometryPose().getRotation(); + } + + /** Sets the current odometry orientation. */ + public void setOdometryRotation(Rotation2d rotation) { + setOdometryPose(new Pose2d(getOdometryPose().getTranslation(), rotation)); } /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + public void setOdometryPose(Pose2d pose) { + odometryPose.resetPosition(rawGyroRotation, getModulePositions(), pose); } /** Adds a new timestamped vision measurement. */ @@ -340,11 +354,11 @@ public void addVisionMeasurement( Matrix visionMeasurementStdDevs) { // Teleport the odometry to the first vision estimate if (firstVisionEstimate && RobotState.isDisabled()) { - setPose(visionRobotPoseMeters); + setOdometryPose(visionRobotPoseMeters); firstVisionEstimate = false; } - poseEstimator.addVisionMeasurement( + visionPose.addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } From 8e10b985fd092de6d8c4a7547768100a0094d44a Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 13 Jan 2026 18:26:09 -0500 Subject: [PATCH 2/5] Changed methods to be either getOdometryPose() or getVisionPose() --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/commands/PathCommands.java | 8 ++++---- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 81e5ac7..b6a05c8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -97,7 +97,7 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getOdometryPose, + drive::getVisionPose, new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), diff --git a/src/main/java/frc/robot/commands/PathCommands.java b/src/main/java/frc/robot/commands/PathCommands.java index 0665598..119bc1d 100644 --- a/src/main/java/frc/robot/commands/PathCommands.java +++ b/src/main/java/frc/robot/commands/PathCommands.java @@ -38,7 +38,7 @@ public static Command goToTargetPose(Drive drive, Pose2d targetPose) { @Override public Command get() { List waypoints = - PathPlannerPath.waypointsFromPoses(drive.getOdometryPose(), targetPose); + PathPlannerPath.waypointsFromPoses(drive.getVisionPose(), targetPose); return AutoBuilder.followPath(createPath(waypoints, targetPose.getRotation())); } @@ -62,7 +62,7 @@ public static Command dockToTargetPoint( new Supplier() { @Override public Command get() { - var initialPose = drive.getOdometryPose(); + var initialPose = drive.getVisionPose(); var heading = targetPoint.minus(initialPose.getTranslation()).getAngle(); var finalPose = new Pose2d(targetPoint, heading); List waypoints = @@ -95,7 +95,7 @@ public static Command dockToTargetPose(Drive drive, Pose2d targetPose, Distance public Command get() { List waypoints = PathPlannerPath.waypointsFromPoses( - drive.getOdometryPose(), + drive.getVisionPose(), targetPose.plus( new Transform2d(leadDistance.unaryMinus(), Meters.of(0), Rotation2d.kZero)), targetPose); @@ -118,7 +118,7 @@ public static Command advanceForward(Drive drive, Distance leadDistance) { new Supplier() { @Override public Command get() { - var initialPose = drive.getOdometryPose(); + var initialPose = drive.getVisionPose(); var waypoints = PathPlannerPath.waypointsFromPoses( initialPose, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index abb0d00..4668fdd 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -226,7 +226,7 @@ public void runVelocity(ChassisSpeeds speeds) { public void followTrajectory(SwerveSample sample) { // Get the current pose of the robot - Pose2d pose = getOdometryPose(); + Pose2d pose = getVisionPose(); // Generate the next speeds for the robot ChassisSpeeds speeds = From 3f0a82973a4169798971430b7b612d9b199150c0 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 13 Jan 2026 18:30:30 -0500 Subject: [PATCH 3/5] changed autonomous selector ports --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9550bd0..c06a1ad 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,10 +65,10 @@ public static final class KrakenX60Constants { } public static final class AutoConstants { - public static final int kAllianceColorSelectorPort = 0; + public static final int kAllianceColorSelectorPort = 3; // max length is 8 - public static final int[] kAutonomousModeSelectorPorts = {1, 2, 3}; + public static final int[] kAutonomousModeSelectorPorts = {0, 1, 2}; } public static final class OIConstants { From 753825734afb39c10298bc02093f09614e7b67e8 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 13 Jan 2026 18:46:26 -0500 Subject: [PATCH 4/5] changed command for resetting robot heading --- src/main/java/frc/robot/Robot.java | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b6a05c8..93aae93 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Meters; import static frc.robot.subsystems.vision.VisionConstants.*; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -312,11 +311,7 @@ public void bindXboxDriver(int port) { xboxDriver .b() .onTrue( - Commands.runOnce( - () -> - drive.setOdometryPose( - new Pose2d(drive.getOdometryPose().getTranslation(), Rotation2d.kZero)), - drive) + Commands.runOnce(() -> drive.setOdometryRotation(Rotation2d.kZero), drive) .ignoringDisable(true)); // Point at target while A button is held From 116b85d2c00b8f77360a2bb0db2cfa58b67bd7d4 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 13 Jan 2026 18:53:55 -0500 Subject: [PATCH 5/5] reset orientation based on alliance --- src/main/java/frc/robot/Robot.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 93aae93..8b596c8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -283,7 +283,13 @@ public void bindZorroDriver(int port) { zorroDriver .GIn() .onTrue( - Commands.runOnce(() -> drive.setOdometryRotation(Rotation2d.kZero), drive) + Commands.runOnce( + () -> + drive.setOdometryRotation( + allianceSelector.fieldRotated() + ? Rotation2d.k180deg + : Rotation2d.kZero), + drive) .ignoringDisable(true)); // Drive 1m forward while button A is held @@ -311,7 +317,13 @@ public void bindXboxDriver(int port) { xboxDriver .b() .onTrue( - Commands.runOnce(() -> drive.setOdometryRotation(Rotation2d.kZero), drive) + Commands.runOnce( + () -> + drive.setOdometryRotation( + allianceSelector.fieldRotated() + ? Rotation2d.k180deg + : Rotation2d.kZero), + drive) .ignoringDisable(true)); // Point at target while A button is held