Remember verified scoring poses (#148)

* Convert cameras to subsystems, add function to get vision-only pose estimate, use it in pose-seek.

* Added some code to store and load override values for coral pipe poses.

* Finished logic for auto-align target correction.

* Split X/Y gain for DriveToPose

* Fix NaN test

* Use wpilib Preferences not Java's

* Create config class for accessing named preferences

* Add d-padd button to fix auto-align pose

* Revert DriveToPose speeds logic

* Revert PID constants for DriveToPoseCommand

* Switch to field April tags

* Added Aapril tag json, removed reef offset, updated climber servo constants

* Remove reef offset todo

* morning tweaks

* Fix elevator rocket

* Move zorro controller command into teleop init.

* sycronized blue and red auto logic

* removed withtimout on elevator

* added timout to both red and blue L4autos

* Set default stop command for swerve in autonomous

* Prevent elevator rocket in joystick command

* made the elevator and robot move at the same time.

* Tweaked the choro by 3 inches

* Widen tolerance for pose seek display

* Remove init from lifter joystick command

* Tweaked the redCenterToL4G choreo

* commented out the createjoystickcontrolcommand and tweaked choreo.

* Another attempt to prevent skyrocket on teleop init

* Adjusted path for 1 piece auto

* Add a few metrics to smart dashboard

* Slowed down the red Auto in Choero

* Adjust Al;gae L3 height, comment out algae wrist set angle on grab

* Uncomment jostick elevator control

* Added pure odometry tracking for comparison with vision

* Calibrate odometry in robot mode init functions

* Replace Robot.updateOdometry() with DriveTrain.calibrateOdometry()

* Remove unnecessary write of scheduler to NT

* Record outtake pose as potential override

* Add back stem gym tag layout

* Reduce max velocity in DriveToPoseCommand

* A little refactoring

* Publish camera pose estimates to network tables

* spotless

* Fix null pointer in Vision camera polling.  Switch to field layout

* updated climber retract setpoint

* invert logiv for intake supplier

* intake corals in parallel with CG, and stop intaking when hasCoral

* increase voltage of outtake algae to barge

* spotless

* Added Reef.Face.getNeareestPipe()

* Update to wpilib 2025.3.2

* Infer target for validated scoring pose independent of auto-align

* spotless

* Push some outtake and override poses to network tables

* Remove reef pipe dashboard publish

* Don't publish target position array

* spotless

* finish merge and clean

---------

Co-authored-by: nlaverdure <[email protected]>
Co-authored-by: Pez357 <[email protected]>

Author: 3 people

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.3.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
     id 'com.diffplug.spotless' version '6.23.3'
 }
 

src/main/java/frc/game/Reef.java

@@ -9,9 +9,9 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.lib.AutoAlignTarget;
 import frc.lib.Config;
+import frc.lib.PoseLogger;
 import java.util.Optional;
 
 /**
@@ -48,7 +48,7 @@ public static Reef getNearestReef(Pose2d atPose) {
   private Pose2d centerPose;
 
   Reef(Pose2d centerPose) {
-    var adjust = Config.getDefault().loadTransform("Reef." + this.name());
+    var adjust = Config.getDefault().loadTransform("Reef." + name() + ".adjust");
     if (adjust.isPresent()) {
       centerPose = centerPose.transformBy(adjust.get());
     }
@@ -123,7 +123,17 @@ public enum Face {
     private Optional<Pose2d> memoLeftPipePose;
     private Optional<Pose2d> memoRightPipePose;
 
-    class PipeTarget extends AutoAlignTarget {
+    public static PipeTarget getNearestPipe(Pose2d atPose) {
+      var face = getNearestReef(atPose).getNearestFace(atPose);
+      var leftDelta =
+          atPose.getTranslation().minus(face.getLeftPipePose().getTranslation()).getNorm();
+      var rightDelta =
+          atPose.getTranslation().minus(face.getRightPipePose().getTranslation()).getNorm();
+      return leftDelta < rightDelta ? face.getLeftPipe() : face.getRightPipe();
+    }
+
+    public class PipeTarget extends AutoAlignTarget {
+
       private final boolean isLeft;
 
       public PipeTarget(boolean isLeft) {
@@ -166,7 +176,6 @@ public void memoize() {
 
       memoLeftPipePose = Config.getDefault().loadPose2d("Reef.Face." + this.name() + ".leftPipe");
       memoRightPipePose = Config.getDefault().loadPose2d("Reef.Face." + this.name() + ".rightPipe");
-      SmartDashboard.putString("Reef.Face." + this.name(), centerPose.toString());
     }
 
     /**
@@ -192,7 +201,7 @@ public Pose2d getCenterPose() {
      *
      * @return left pipe target
      */
-    public AutoAlignTarget getLeftPipe() {
+    public PipeTarget getLeftPipe() {
       return new PipeTarget(true);
     }
 
@@ -201,7 +210,8 @@ public AutoAlignTarget getLeftPipe() {
      *
      * @return right pipe target
      */
-    public AutoAlignTarget getRightPipe() {
+    public PipeTarget getRightPipe() {
+
       return new PipeTarget(false);
     }
 
@@ -241,7 +251,10 @@ public Pose2d getRightPipePose() {
      */
     public void memoizeLeftPipePose(Pose2d pose) {
       memoLeftPipePose = Optional.of(pose);
-      Config.getDefault().storePose2d("Reef.Face." + this.toString() + ".leftPipe", pose);
+      var tag = "Reef.Face." + name() + ".leftPipe";
+      Config.getDefault().storePose2d(tag, pose);
+      PoseLogger.getDefault().publish(tag + ".old", leftPipePose);
+      PoseLogger.getDefault().publish(tag + ".new", pose);
     }
 
     /**
@@ -251,7 +264,10 @@ public void memoizeLeftPipePose(Pose2d pose) {
      */
     public void memoizeRightPipePose(Pose2d pose) {
       memoRightPipePose = Optional.of(pose);
-      Config.getDefault().storePose2d("Reef.Face." + this.toString() + ".rightPipe", pose);
+      var tag = "Reef.Face." + name() + ".rightPipe";
+      Config.getDefault().storePose2d(tag, pose);
+      PoseLogger.getDefault().publish(tag + ".old", leftPipePose);
+      PoseLogger.getDefault().publish(tag + ".new", pose);
     }
   }
 }

src/main/java/frc/robot/Robot.java

@@ -1,11 +1,10 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Seconds;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StructArrayPublisher;
-import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -55,8 +54,6 @@
 import frc.robot.vision.Camera;
 import frc.robot.vision.Vision;
 import java.util.Arrays;
-import java.util.HashMap;
-import java.util.Map;
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
@@ -88,15 +85,10 @@ public class Robot extends TimedRobot {
   private BooleanSupplier algaeModeSupplier;
   private Supplier<Gamepiece> gamepieceSupplier;
   private int usbCheckDelay = OIConstants.kUSBCheckNumLoops;
-  private Map<String, StructPublisher<Pose2d>> posePublishers = new HashMap<>();
   private Optional<AutoAlignTarget> currentAutoAlignTarget = Optional.empty();
-  private Pose2d nearestLeftPipe;
-  private Pose2d nearestRightPipe;
-
-  private StructArrayPublisher<Pose2d> reefTargetPositionsPublisher =
-      NetworkTableInstance.getDefault()
-          .getStructArrayTopic("Reef Target Positions", Pose2d.struct)
-          .publish();
+  private Optional<Pose2d> recentOuttakePose = Optional.empty();
+  private Optional<Pose2d> nearestLeftPipe = Optional.empty();
+  private Optional<Pose2d> nearestRightPipe = Optional.empty();
 
   public Robot() {
     gamepieceSupplier =
@@ -139,12 +131,12 @@ public Gamepiece get() {
     logger.monitor(
         "nearestLeftPipe",
         () -> {
-          return Optional.ofNullable(nearestLeftPipe);
+          return nearestLeftPipe;
         });
     logger.monitor(
         "nearestRightPipe",
         () -> {
-          return Optional.ofNullable(nearestRightPipe);
+          return nearestRightPipe;
         });
     Arrays.stream(Camera.values())
         .forEach(
@@ -162,8 +154,6 @@ public void robotInit() {
     // https://docs.wpilib.org/en/stable/docs/software/telemetry/datalog.html#logging-joystick-data
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
-
-    reefTargetPositionsPublisher.set(DriveConstants.kReefTargetPoses);
     swerve.calibrateOdometry();
   }
 
@@ -184,8 +174,8 @@ public void robotPeriodic() {
 
     var nearestReef = Reef.getNearestReef(swerve.getPose());
     var nearestReefFace = nearestReef.getNearestFace(swerve.getPose());
-    nearestLeftPipe = nearestReefFace.getLeftPipePose();
-    nearestRightPipe = nearestReefFace.getRightPipePose();
+    nearestLeftPipe = Optional.of(nearestReefFace.getLeftPipePose());
+    nearestRightPipe = Optional.of(nearestReefFace.getRightPipePose());
   }
 
   @Override
@@ -292,23 +282,58 @@ public void configureButtonBindings() {
     configureOperatorButtonBindings();
   }
 
-  /** Store the supplied auto-align target for possible fixing. */
-  protected Supplier<Pose2d> startAutoAlign(AutoAlignTarget target) {
+  /**
+   * Store the supplied auto-align target for possible fixing.
+   *
+   * @return a supplier that will return the target pose
+   */
+  protected Supplier<Pose2d> startAutoAlign(boolean isLeftPipe) {
+    var pose = swerve.getPose();
+    var face = Reef.getNearestReef(pose).getNearestFace(pose);
+    var target = isLeftPipe ? face.getLeftPipe() : face.getRightPipe();
     currentAutoAlignTarget = Optional.of(target);
+    PoseLogger.getDefault().publish("autoAlignTarget", target.getPose());
     return () -> target.getPose();
   }
 
-  protected void rememberOutputPose(Pose2d pose) {
-    currentAutoAlignTarget.ifPresent(target -> target.setPose(pose));
+  /**
+   * Remember the last outtake pose for potential recording.
+   *
+   * @param pose
+   */
+  protected void rememberOutputPose() {
+    recentOuttakePose = Optional.of(swerve.getPose());
+    PoseLogger.getDefault().publish("recentOuttakePose", recentOuttakePose.get());
   }
 
   /**
-   * Fix the most recent auto-align target by ammending its position with the one suppliked.
+   * Note that the last outtake was done at a good position.
+   *
+   * <p>If the outtake command occurred within five inches of a coral pipe, or if the outtake
+   * followed an auto-align command, then we will remember the outtake location for the
+   * corresponding target.
    *
    * @param newPose new pose to use for this target
    */
-  protected void fixAutoAlign(Pose2d newPose) {
-    currentAutoAlignTarget.ifPresent(target -> target.memoize());
+  protected void lastOuttakeWasAtGoodLocation() {
+    recentOuttakePose.ifPresent(
+        pose -> {
+          var target = Reef.Face.getNearestPipe(pose);
+          var deltaInches =
+              Meters.of(pose.getTranslation().minus(target.getPose().getTranslation()).getNorm())
+                  .in(Inches);
+          if (deltaInches < 6) {
+            target.setPose(pose);
+            target.memoize();
+          } else {
+            currentAutoAlignTarget.ifPresent(
+                autoTarget -> {
+                  autoTarget.setPose(pose);
+                  autoTarget.memoize();
+                });
+          }
+        });
+    recentOuttakePose = Optional.empty();
     currentAutoAlignTarget = Optional.empty();
   }
 
@@ -322,31 +347,17 @@ private void configureDriverButtonBindings() {
           // swerve.initializeRelativeTurningEncoder();
         }).ignoringDisable(true));
 
-    // Drive to nearest pose
-    // driver.AIn()
-    //     .whileTrue(new DriveToPoseCommand(swerve, () -> swerve.getNearestPose()));
-
-    //TODO: add a button binding to call fixAutoAlign(swerve.getPose())
-    driver.AIn().whileTrue(new DriveToPoseCommand(swerve, 
-      () -> startAutoAlign(Reef.getNearestReef(swerve.getPose()).getNearestFace(swerve.getPose()).getLeftPipe()).get()));
-    driver.DIn().whileTrue(new DriveToPoseCommand(swerve, 
-      () -> startAutoAlign(Reef.getNearestReef(swerve.getPose()).getNearestFace(swerve.getPose()).getRightPipe()).get()));
+    driver.AIn().whileTrue(new DriveToPoseCommand(swerve, () -> startAutoAlign(true).get()));
+    driver.DIn().whileTrue(new DriveToPoseCommand(swerve, () -> startAutoAlign(false).get()));
 
     // Outtake grippers
     var outtaking = driver.HIn();
-    outtaking.onTrue(new InstantCommand(() -> rememberOutputPose(swerve.getPose())));
     lifter.atProcessor.and(outtaking)
         .whileTrue(algaeRoller.outtakeToProcessor());
-    lifter.atAlgaeElse.and(outtaking)
+    lifter.atProcessor.negate().and(outtaking)
         .whileTrue(algaeRoller.outtakeToBarge());
-    lifter.atCoralL1.and(outtaking)
-        .whileTrue(coralRoller.outtakeToL1());
-    lifter.atCoralL2.and(outtaking)
-        .whileTrue(coralRoller.outtakeToL2());
-    lifter.atCoralL3.and(outtaking)
-        .whileTrue(coralRoller.outtakeToL3());
-    lifter.atCoralElse.and(outtaking)
-        .whileTrue(coralRoller.outtakeToL4());
+    outtaking
+        .onTrue(new InstantCommand(() -> rememberOutputPose()));
   }
 
   private void configureOperatorButtonBindings() {
@@ -426,8 +437,8 @@ public boolean getAsBoolean() {
      * Left and right D-pad buttons will cause the robot to go to the left/right
      * pipe on the nearest reef face.
      */
-    operator.povLeft().onTrue(new InstantCommand(() -> fixAutoAlign(swerve.getPose())));
-    operator.povRight().onTrue(new InstantCommand(() -> fixAutoAlign(swerve.getPose())));
+    operator.povLeft().onTrue(new InstantCommand(() -> lastOuttakeWasAtGoodLocation()));
+    operator.povRight().onTrue(new InstantCommand(() -> lastOuttakeWasAtGoodLocation()));
 
 
 
@@ -510,7 +521,7 @@ protected Command createRollerAnimationCommand() {
      */
     BooleanSupplier intakeSupplier =
         () -> {
-          return coralRoller.getRollerVelocity() > 1 || algaeRoller.getRollerVelocity() > 1;
+          return coralRoller.getRollerVelocity() < 1 || algaeRoller.getRollerVelocity() < 1;
         };
 
     /*