Reef relative auto align tolerance

src/main/java/frc/robot/Constants.java

@@ -357,7 +357,7 @@ public class RobotK {
         public static final String kLogTab = "SuperStructure";
         // TODO: get a real distance from the reef for this
         public static final double kRobotCenterDistanceFromReef = Units.inchesToMeters(-17);
-        public static final double kRobotScoringOffset = Units.inchesToMeters(2.9); // positive left robot, measured 4/1/2025
+        public static final double kRobotScoringOffset = Units.inchesToMeters(3.2); // positive left robot, measured 4/1/2025
         public static final Transform2d kTransformReefPoseToRobotPosition = new Transform2d(kRobotCenterDistanceFromReef, kRobotScoringOffset, Rotation2d.fromDegrees(0));
      }
 
@@ -453,6 +453,10 @@ public static class SharedAutoAlignK {
         public static final Distance kFieldTranslationTolerance = Meters.of(0.025); // meters
         public static final Angle kFieldRotationTolerance = Degrees.of(0.5); // degrees
 
+        public static final Distance kSideToSideTolerance = Meters.of(0.01);
+        public static final Distance kReefDistanceTolerance = Meters.of(0.02);
+        public static final Angle kRotationTolerance = Degrees.of(0.5);
+
         public static final double kIntermediatePoseDistance = -Units.inchesToMeters(6); // value in meters
         public static final Transform2d kIntermediatePoseTransform 
             = new Transform2d(kIntermediatePoseDistance, 0, Rotation2d.kZero);

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

@@ -243,7 +243,7 @@ private WaltAutonFactory autonFactoryFactory(
   }
 
   Command autoAlignCmd(boolean rightReef) {
-    return MovingAutoAlign.autoAlignWithIntermediateTransformUntilInTolerances(
+    return MovingAutoAlign.superDuperAutoAlign(
       drivetrain, 
       () -> AutoAlignUtils.getMostLikelyScorePose(drivetrain.getState(), rightReef), 
       () -> SharedAutoAlignK.kIntermediatePoseTransform

src/main/java/frc/robot/autoalign/AutoAlignUtils.java

@@ -256,4 +256,19 @@ public static boolean isInTolerance(Pose2d pose,
     //             velocityTolerance.getAsDouble());
     //     };
     // }
+
+    // rotationTolerance should be in radians for best results
+    public static boolean isInTolerancePoseRelative(Pose2d pose, Pose2d pose2, double xDifferenceTolerance, 
+            double yDifferenceTolerance, double rotationTolerance) {
+        return isInTolerancePoseRelative(pose, pose2, new ChassisSpeeds(), xDifferenceTolerance, yDifferenceTolerance, rotationTolerance);
+    }
+
+    public static boolean isInTolerancePoseRelative(Pose2d pose, Pose2d pose2, ChassisSpeeds speeds, double xDifferenceTolerance, 
+            double yDifferenceTolerance, double rotationTolerance) {
+        final Pose2d relativePose = pose.relativeTo(pose2);
+        return MathUtil.isNear(0.0, relativePose.getX(), xDifferenceTolerance)
+            && MathUtil.isNear(0.0, relativePose.getY(), yDifferenceTolerance)
+            && MathUtil.isNear(0.0, relativePose.getRotation().getRadians(), rotationTolerance)
+            && MathUtil.isNear(0.0, Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond), SharedAutoAlignK.kFinishedVelTolerance);
+    }
 }

src/main/java/frc/robot/autoalign/LegacyAutoAlign.java

@@ -29,6 +29,10 @@ public class LegacyAutoAlign {
     private static final DoubleLogger log_errorX = WaltLogger.logDouble(LegacyAutoAlignK.kLogTab, "x error");
     private static final DoubleLogger log_errorY = WaltLogger.logDouble(LegacyAutoAlignK.kLogTab, "y error");
     private static final DoubleLogger log_errorRot = WaltLogger.logDouble(LegacyAutoAlignK.kLogTab, "rotation error degrees");
+
+    private static final DoubleLogger log_errorXTargetRelative = WaltLogger.logDouble(LegacyAutoAlignK.kLogTab, "x error target relative");
+    private static final DoubleLogger log_errorYTargetRelative = WaltLogger.logDouble(LegacyAutoAlignK.kLogTab, "y error target relative");
+    private static final DoubleLogger log_errorRotTargetRelative = WaltLogger.logDouble(LegacyAutoAlignK.kLogTab, "rot error degrees target relative");
 
 
     private static final SwerveRequest.FieldCentric swreq_driveFieldCentricBlue = new SwerveRequest.FieldCentric()
@@ -47,6 +51,7 @@ public static Command moveToPose(Swerve drivetrain, Supplier<Pose2d> destination
         ).andThen(Commands.run(
             () -> {
                 Pose2d curPose = drivetrain.getState().Pose;
+                Pose2d destRelativePose = curPose.relativeTo(cachedTarget[0]);
 
                 double xSpeed = LegacyAutoAlignK.kAutoAlignXController.calculate(curPose.getX(), cachedTarget[0].getX());
                 log_errorX.accept(cachedTarget[0].getX() - curPose.getX());
@@ -58,6 +63,11 @@ public static Command moveToPose(Swerve drivetrain, Supplier<Pose2d> destination
                     LegacyAutoAlignK.kMaxXYSpeedAutoalign);
                 ySpeed = MathUtil.clamp(ySpeed, -LegacyAutoAlignK.kMaxXYSpeedAutoalign, 
                     LegacyAutoAlignK.kMaxXYSpeedAutoalign);
+
+                log_errorXTargetRelative.accept(destRelativePose.getX());
+                log_errorYTargetRelative.accept(destRelativePose.getY());
+                log_errorRotTargetRelative.accept(destRelativePose.getRotation().getDegrees());
+
                 drivetrain.setControl(swreq_driveFieldCentricBlue.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed));
             }, drivetrain
         )).until(() -> AutoAlignUtils.isInTolerance(drivetrain.getState().Pose, destinationPose.get()));
@@ -73,7 +83,8 @@ public static Command moveToPoseUntilInTimeScaledTolerance(
             Swerve drivetrain, 
             Supplier<Pose2d> destinationPose, 
             DoubleSupplier maxToleranceTime, 
-            DoubleSupplier maxLinearTolerance, 
+            DoubleSupplier maxXTolerance,
+            DoubleSupplier maxYTolerance, 
             DoubleSupplier maxRotationTolerance,
             Runnable onBeginFunc) {
         final double[] initialTime = {Double.MAX_VALUE};
@@ -86,18 +97,24 @@ public static Command moveToPoseUntilInTimeScaledTolerance(
                 double currentTime = Timer.getFPGATimestamp();
                 double deltaTime = currentTime - initialTime[0];
 
-                double deltaLinearTolerance = maxLinearTolerance.getAsDouble() - SharedAutoAlignK.kFieldTranslationTolerance.in(Meters);
+                // double deltaLinearTolerance = maxLinearTolerance.getAsDouble() - SharedAutoAlignK.kFieldTranslationTolerance.in(Meters);
+                double deltaXTolerance = maxXTolerance.getAsDouble() - SharedAutoAlignK.kReefDistanceTolerance.in(Meters);
+                double deltaYTolerance = maxYTolerance.getAsDouble() - SharedAutoAlignK.kSideToSideTolerance.in(Meters);
                 double deltaRotationTolerance = maxRotationTolerance.getAsDouble() - SharedAutoAlignK.kFieldRotationTolerance.in(Radians);
 
                 double percentageTimeComplete = MathUtil.clamp(deltaTime / maxToleranceTime.getAsDouble(), 0, 1);
 
-                double linearTolerance = percentageTimeComplete * deltaLinearTolerance 
-                    + SharedAutoAlignK.kFieldTranslationTolerance.in(Meters);
+                // double linearTolerance = percentageTimeComplete * deltaLinearTolerance 
+                //     + SharedAutoAlignK.kFieldTranslationTolerance.in(Meters);
+                double xTolerance = percentageTimeComplete * deltaXTolerance
+                    + SharedAutoAlignK.kReefDistanceTolerance.in(Meters);
+                double yTolerance = percentageTimeComplete * deltaYTolerance
+                    + SharedAutoAlignK.kSideToSideTolerance.in(Meters);
                 double rotationTolerance = percentageTimeComplete * deltaRotationTolerance
                     + SharedAutoAlignK.kFieldRotationTolerance.in(Radians);
 
-                return AutoAlignUtils.isInTolerance(drivetrain.getState().Pose, destinationPose.get(), 
-                    linearTolerance, rotationTolerance);
+                return AutoAlignUtils.isInTolerancePoseRelative(drivetrain.getState().Pose, destinationPose.get(), 
+                    xTolerance, yTolerance, rotationTolerance);
             });
     }
 }

src/main/java/frc/robot/autoalign/MovingAutoAlign.java

@@ -1,5 +1,8 @@
 package frc.robot.autoalign;
 
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Radians;
+
 import java.util.function.Supplier;
 
 import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
@@ -16,7 +19,9 @@
 import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.LegacyAutoAlignK;
 import frc.robot.Constants.MovingAutoAlignK;
+import frc.robot.Constants.SharedAutoAlignK;
 import frc.robot.subsystems.Swerve;
 import frc.util.WaltLogger;
 import frc.util.WaltLogger.DoubleLogger;
@@ -29,6 +34,23 @@ public class MovingAutoAlign {
     private static final DoubleLogger log_errorY = WaltLogger.logDouble(kTopicPrefix, "y error");
     private static final DoubleLogger log_errorRot = WaltLogger.logDouble(kTopicPrefix, "rotation error degrees");
 
+    private static final DoubleLogger log_errorXTargetRelative = WaltLogger.logDouble(MovingAutoAlignK.kLogTab, "x error target relative");
+    private static final DoubleLogger log_errorYTargetRelative = WaltLogger.logDouble(MovingAutoAlignK.kLogTab, "y error target relative");
+    private static final DoubleLogger log_errorRotTargetRelative = WaltLogger.logDouble(MovingAutoAlignK.kLogTab, "rot error degrees target relative");
+
+    // this would really be called autoAlignWithIntermediateTransformUntilInPoseRelativeTolerances
+    // but that would be stupid.
+    // appreciate the concise new name!
+    public static Command superDuperAutoAlign(
+            Swerve drivetrain,
+            Supplier<Pose2d> target,
+            Supplier<Transform2d> intermediateTransfrom) {
+        return moveToPoseUntilInPoseRelativeTolerances(drivetrain, () -> target.get().transformBy(intermediateTransfrom.get()), ChassisSpeeds::new,
+            () -> MovingAutoAlignK.kXYConstraints)
+            .andThen(moveToPoseUntilInPoseRelativeTolerances(drivetrain, target, ChassisSpeeds::new, 
+                () -> MovingAutoAlignK.kXYConstraints));
+    }
+
     /**
      * <p> Returns a Command that automatically aligns with an intermediate pose and target pose that will finish when it is
      *  in tolerances.
@@ -84,6 +106,20 @@ public static Command moveToPoseUntilInTolerances(
             .until(() -> AutoAlignUtils.isInTolerance(swerve.getState().Pose, target.get(), swerve.getState().Speeds));
     }
 
+    public static Command moveToPoseUntilInPoseRelativeTolerances(
+            Swerve swerve,
+            Supplier<Pose2d> target,
+            Supplier<ChassisSpeeds> speedsModifier,
+            Supplier<TrapezoidProfile.Constraints> xyConstraints) {
+        return moveToPose(swerve, target, speedsModifier, xyConstraints)
+            .until(() -> AutoAlignUtils.isInTolerancePoseRelative(swerve.getState().Pose, 
+                target.get(), 
+                swerve.getState().Speeds,
+                SharedAutoAlignK.kReefDistanceTolerance.in(Meters),
+                SharedAutoAlignK.kSideToSideTolerance.in(Meters), 
+                SharedAutoAlignK.kRotationTolerance.in(Radians)));
+    }
+
     /**
      * <p> Performs moving auto align to attempt to move towards a pose. Does not handle finishing, must be interrupted.
      *  Commenting and formatting might be weird because this was largely taken from 8033's code.
@@ -130,6 +166,10 @@ public static Command moveToPose(
 
                 SwerveDriveState curState = swerve.getState();
                 Pose2d curPose = curState.Pose;
+                Pose2d targetRelativePose2d = curPose.relativeTo(cachedTarget[0]);
+                log_errorXTargetRelative.accept(targetRelativePose2d.getX());
+                log_errorYTargetRelative.accept(targetRelativePose2d.getY());
+                log_errorRotTargetRelative.accept(targetRelativePose2d.getRotation().getDegrees());
                 ChassisSpeeds fieldRelativeChassisSpeeds = Swerve.getFieldRelativeChassisSpeeds(curState);
                 // for some reason only do logging in simulation?
                 // very smart of them to cache whether the robot is in simulation though rather than

src/main/java/frc/robot/autons/WaltAutonFactory.java

@@ -262,8 +262,9 @@ private Command autoAlignCommand(Supplier<ReefLocs> reefLocSup) {
         Command aaCmd = LegacyAutoAlign.moveToPoseUntilInTimeScaledTolerance(
             m_drivetrain,
             () -> destinationPose,
-            () -> 1,
-            () -> 10 * SharedAutoAlignK.kFieldTranslationTolerance.in(Meters),
+            () -> 3,
+            () -> 10 * SharedAutoAlignK.kReefDistanceTolerance.in(Meters),
+            () -> 10 * SharedAutoAlignK.kSideToSideTolerance.in(Meters),
             () -> 10 * SharedAutoAlignK.kFieldRotationTolerance.in(Radians),
             m_onAutoAlignBeginFunc
         );