some auto position reset stuff

src/main/java/frc/robot/auto/Autos.java

@@ -20,6 +20,7 @@
 import frc.robot.subsystems.superstructure.ShotParameters;
 import frc.robot.subsystems.superstructure.Superstructure;
 import frc.robot.subsystems.vision.PhotonVision;
+import org.littletonrobotics.junction.Logger;
 
 import java.util.List;
 import java.util.Set;
@@ -146,7 +147,7 @@ private Command followPath(final ChoreoTrajectory choreoTrajectory, final Timer
     }
 
     private Command resetPose(final ChoreoTrajectory trajectory) {
-        return Commands.defer(() -> swerve.resetPoseCommand(
+        return Commands.defer(() -> photonVision.resetPoseCommand(
                         Robot.IsRedAlliance.getAsBoolean()
                                 ? trajectory.getFlippedInitialPose()
                                 : trajectory.getInitialPose()
@@ -1255,6 +1256,9 @@ public EventLoop driveAndNoteDetect() {
 
         autoTriggers.autoEnabled().whileTrue(preloadSubwooferAndFollow0(autoTriggers.trajectories, timer));
 
+        Logger.recordOutput("OInitial pose", autoTriggers.trajectories.get(0).getInitialPose());
+        Logger.recordOutput("Initial pose", autoTriggers.trajectories.get(0).getFlippedInitialPose());
+
         autoTriggers.atTime(2.23).onTrue(
                 Commands.parallel(
                         intake.intakeCommand().asProxy(),

src/main/java/frc/robot/subsystems/drive/Swerve.java

@@ -404,14 +404,10 @@ public Command zeroRotationCommand() {
         return runOnce(this::zeroRotation);
     }
 
-    public void resetPose(final Pose2d robotPose) {
+    private void resetPose(final Pose2d robotPose) {
         poseEstimator.resetPosition(gyro.getYawRotation2d(), getModulePositions(), robotPose);
     }
 
-    public Command resetPoseCommand(final Pose2d robotPose) {
-        return runOnce(() -> resetPose(robotPose));
-    }
-
     public ChassisSpeeds getRobotRelativeSpeeds() {
         return kinematics.toChassisSpeeds(
                 frontLeft.getState(),

src/main/java/frc/robot/subsystems/drive/estimator/PoseEstimator.java

@@ -67,7 +67,8 @@ public PoseEstimator(
             Kinematics<?, T> kinematics,
             Odometry<T> odometry,
             Matrix<N3, N1> stateStdDevs,
-            Matrix<N3, N1> visionMeasurementStdDevs) {
+            Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
         m_kinematics = kinematics;
         m_odometry = odometry;
 
@@ -175,7 +176,11 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS
 
         // Step 4: Convert back to Twist2d.
         var scaledTwist =
-                new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+                new Twist2d(
+                        k_times_twist.get(0, 0),
+                        k_times_twist.get(1, 0),
+                        k_times_twist.get(2, 0)
+                );
 
         // Step 5: Reset Odometry to state at sample with vision adjustment.
         m_odometry.resetPosition(
@@ -226,7 +231,8 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS
     public void addVisionMeasurement(
             Pose2d visionRobotPoseMeters,
             double timestampSeconds,
-            Matrix<N3, N1> visionMeasurementStdDevs) {
+            Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
         setVisionMeasurementStdDevs(visionMeasurementStdDevs);
         addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
     }
@@ -256,7 +262,8 @@ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T
         m_odometry.update(gyroAngle, wheelPositions);
         m_poseBuffer.addSample(
                 currentTimeSeconds,
-                new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()));
+                new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy())
+        );
 
         return getEstimatedPosition();
     }

src/main/java/frc/robot/subsystems/drive/estimator/SwerveDrivePoseEstimator.java

@@ -17,124 +17,129 @@
 /**
  * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
  * vision measurements with swerve drive encoder distance measurements. It is intended to be a
- * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
+ * drop-in replacement for {@link SwerveDriveOdometry}.
  *
- * <p>{@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator#update} should be called every robot loop.
+ * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
  *
- * <p>{@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
+ * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
  * want; if you never call it, then this class will behave as regular encoder odometry.
  */
 public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveDriveWheelPositions> {
-  private final int m_numModules;
+    private final int m_numModules;
 
-  /**
-   * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
-   * measurements.
-   *
-   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
-   * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9
-   * meters for x, 0.9 meters for y, and 0.9 radians for heading.
-   *
-   * @param kinematics A correctly-configured kinematics object for your drivetrain.
-   * @param gyroAngle The current gyro angle.
-   * @param modulePositions The current distance measurements and rotations of the swerve modules.
-   * @param initialPoseMeters The starting pose estimate.
-   */
-  public SwerveDrivePoseEstimator(
-      SwerveDriveKinematics kinematics,
-      Rotation2d gyroAngle,
-      SwerveModulePosition[] modulePositions,
-      Pose2d initialPoseMeters) {
-    this(
-        kinematics,
-        gyroAngle,
-        modulePositions,
-        initialPoseMeters,
-        VecBuilder.fill(0.1, 0.1, 0.1),
-        VecBuilder.fill(0.9, 0.9, 0.9));
-  }
-
-  /**
-   * Constructs a SwerveDrivePoseEstimator.
-   *
-   * @param kinematics A correctly-configured kinematics object for your drivetrain.
-   * @param gyroAngle The current gyro angle.
-   * @param modulePositions The current distance and rotation measurements of the swerve modules.
-   * @param initialPoseMeters The starting pose estimate.
-   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
-   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
-   *     less.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
-   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
-   *     the vision pose measurement less.
-   */
-  public SwerveDrivePoseEstimator(
-      SwerveDriveKinematics kinematics,
-      Rotation2d gyroAngle,
-      SwerveModulePosition[] modulePositions,
-      Pose2d initialPoseMeters,
-      Matrix<N3, N1> stateStdDevs,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
-    super(
-        kinematics,
-        new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
-        stateStdDevs,
-        visionMeasurementStdDevs);
+    /**
+     * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
+     * measurements.
+     *
+     * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+     * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9
+     * meters for x, 0.9 meters for y, and 0.9 radians for heading.
+     *
+     * @param kinematics A correctly-configured kinematics object for your drivetrain.
+     * @param gyroAngle The current gyro angle.
+     * @param modulePositions The current distance measurements and rotations of the swerve modules.
+     * @param initialPoseMeters The starting pose estimate.
+     */
+    public SwerveDrivePoseEstimator(
+            SwerveDriveKinematics kinematics,
+            Rotation2d gyroAngle,
+            SwerveModulePosition[] modulePositions,
+            Pose2d initialPoseMeters
+    ) {
+        this(
+                kinematics,
+                gyroAngle,
+                modulePositions,
+                initialPoseMeters,
+                VecBuilder.fill(0.1, 0.1, 0.1),
+                VecBuilder.fill(0.9, 0.9, 0.9));
+    }
 
-    m_numModules = modulePositions.length;
-  }
+    /**
+     * Constructs a SwerveDrivePoseEstimator.
+     *
+     * @param kinematics A correctly-configured kinematics object for your drivetrain.
+     * @param gyroAngle The current gyro angle.
+     * @param modulePositions The current distance and rotation measurements of the swerve modules.
+     * @param initialPoseMeters The starting pose estimate.
+     * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+     *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+     *     less.
+     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+     *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+     *     the vision pose measurement less.
+     */
+    public SwerveDrivePoseEstimator(
+            SwerveDriveKinematics kinematics,
+            Rotation2d gyroAngle,
+            SwerveModulePosition[] modulePositions,
+            Pose2d initialPoseMeters,
+            Matrix<N3, N1> stateStdDevs,
+            Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
+        super(
+                kinematics,
+                new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
+                stateStdDevs,
+                visionMeasurementStdDevs);
 
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>The gyroscope angle does not need to be reset in the user's robot code. The library
-   * automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param modulePositions The current distance measurements and rotations of the swerve modules.
-   * @param poseMeters The position on the field that your robot is at.
-   */
-  public void resetPosition(
-      Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
-    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters);
-  }
+        m_numModules = modulePositions.length;
+    }
 
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param gyroAngle The current gyro angle.
-   * @param modulePositions The current distance measurements and rotations of the swerve modules.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
-  }
+    /**
+     * Resets the robot's position on the field.
+     *
+     * <p>The gyroscope angle does not need to be reset in the user's robot code. The library
+     * automatically takes care of offsetting the gyro angle.
+     *
+     * @param gyroAngle The angle reported by the gyroscope.
+     * @param modulePositions The current distance measurements and rotations of the swerve modules.
+     * @param poseMeters The position on the field that your robot is at.
+     */
+    public void resetPosition(
+            Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters
+    ) {
+        resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters);
+    }
 
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param currentTimeSeconds Time at which this method was called, in seconds.
-   * @param gyroAngle The current gyroscope angle.
-   * @param modulePositions The current distance measurements and rotations of the swerve modules.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return updateWithTime(
-        currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
-  }
+    /**
+     * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+     * loop.
+     *
+     * @param gyroAngle The current gyro angle.
+     * @param modulePositions The current distance measurements and rotations of the swerve modules.
+     * @return The estimated pose of the robot in meters.
+     */
+    public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+        return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+    }
 
-  @Override
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) {
-    if (wheelPositions.positions.length != m_numModules) {
-      throw new IllegalArgumentException(
-          "Number of modules is not consistent with number of wheel locations provided in "
-              + "constructor");
+    /**
+     * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+     * loop.
+     *
+     * @param currentTimeSeconds Time at which this method was called, in seconds.
+     * @param gyroAngle The current gyroscope angle.
+     * @param modulePositions The current distance measurements and rotations of the swerve modules.
+     * @return The estimated pose of the robot in meters.
+     */
+    public Pose2d updateWithTime(
+            double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions
+    ) {
+        return updateWithTime(
+                currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
     }
 
-    return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
-  }
+    @Override
+    public Pose2d updateWithTime(
+            double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions
+    ) {
+        if (wheelPositions.positions.length != m_numModules) {
+            throw new IllegalArgumentException(
+                    "Number of modules is not consistent with number of wheel locations provided in "
+                            + "constructor");
+        }
+
+        return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
+    }
 }

src/main/java/frc/robot/subsystems/vision/PhotonVision.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.drive.Swerve;
@@ -28,6 +29,8 @@
 import java.util.function.Supplier;
 import java.util.stream.Collectors;
 
+import static edu.wpi.first.wpilibj2.command.Commands.runOnce;
+
 public class PhotonVision extends VirtualSubsystem {
     public static final String PhotonLogKey = "Vision";
 
@@ -254,6 +257,11 @@ private void update() {
             final VisionIO.VisionIOInputs inputs = visionIOInputsEntry.getValue();
             final String logKey = PhotonLogKey + "/" + inputs.name;
 
+            Logger.recordOutput(
+                    logKey + "/CameraPose",
+                    new Pose3d(swerve.getPose()).transformBy(Constants.Vision.ROBOT_TO_REAR_NOTE)
+            );
+
             final EstimatedRobotPose estimatedRobotPose = runner.getEstimatedRobotPose(visionIO);
             if (estimatedRobotPose != null) {
                 final EstimatedRobotPose lastEstimatedPose = lastEstimatedRobotPose.get(visionIO);
@@ -285,6 +293,11 @@ private void update() {
             final VisionIO.VisionIOInputs inputs = visionIOInputsEntry.getValue();
             final String logKey = PhotonLogKey + "/" + inputs.name;
 
+            Logger.recordOutput(
+                    logKey + "/CameraPose",
+                    new Pose3d(swerve.getPose()).transformBy(Constants.Vision.ROBOT_TO_REAR_NOTE)
+            );
+
             final NoteTrackingResult noteTrackingResult = runner.getNoteTrackingResult(visionIO);
             if (noteTrackingResult != null) {
                 Logger.recordOutput(logKey + "/HasTargets", noteTrackingResult.hasTargets);
@@ -296,10 +309,6 @@ private void update() {
                         logKey + "/BestNotePose",
                         new Pose3d(optionalBestNotePose.orElseGet(Pose2d::new))
                 );
-                Logger.recordOutput(
-                        "NoteCameraPose",
-                        new Pose3d(swerve.getPose()).transformBy(Constants.Vision.ROBOT_TO_REAR_NOTE)
-                );
 
                 final Pose2d[] notePose2ds = noteTrackingResult
                         .getNotePoses(swerve::getPose);
@@ -380,6 +389,10 @@ public void resetPosition(final Pose2d robotPose) {
         resetPosition(robotPose, swerve.getYaw());
     }
 
+    public Command resetPoseCommand(final Pose2d robotPose) {
+        return runOnce(() -> resetPosition(robotPose));
+    }
+
     public List<Pose2d> getNotePoses() {
         final List<Pose2d> notePoses = new ArrayList<>();
         for (final VisionIO visionIO : noteTrackingVisionIOInputsMap.keySet()) {