From deca342c74b11ff167197ffce9d88b59e59290c2 Mon Sep 17 00:00:00 2001 From: scootdboot Date: Thu, 17 Apr 2025 08:44:03 -0500 Subject: [PATCH 1/3] pose relative tolerances added --- src/main/java/frc/robot/Constants.java | 6 +++- .../frc/robot/autoalign/AutoAlignUtils.java | 15 +++++++++ .../frc/robot/autoalign/LegacyAutoAlign.java | 19 ++++++++---- .../frc/robot/autoalign/MovingAutoAlign.java | 31 +++++++++++++++++++ .../frc/robot/autons/WaltAutonFactory.java | 5 +-- 5 files changed, 67 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6820f47..aa08ed1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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); diff --git a/src/main/java/frc/robot/autoalign/AutoAlignUtils.java b/src/main/java/frc/robot/autoalign/AutoAlignUtils.java index 35dfc64..a6b4d0a 100644 --- a/src/main/java/frc/robot/autoalign/AutoAlignUtils.java +++ b/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); + } } diff --git a/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java b/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java index dc89f15..c8668ed 100644 --- a/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java +++ b/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java @@ -73,7 +73,8 @@ public static Command moveToPoseUntilInTimeScaledTolerance( Swerve drivetrain, Supplier destinationPose, DoubleSupplier maxToleranceTime, - DoubleSupplier maxLinearTolerance, + DoubleSupplier maxXTolerance, + DoubleSupplier maxYTolerance, DoubleSupplier maxRotationTolerance, Runnable onBeginFunc) { final double[] initialTime = {Double.MAX_VALUE}; @@ -86,18 +87,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); }); } } diff --git a/src/main/java/frc/robot/autoalign/MovingAutoAlign.java b/src/main/java/frc/robot/autoalign/MovingAutoAlign.java index c2617a6..f9523d1 100644 --- a/src/main/java/frc/robot/autoalign/MovingAutoAlign.java +++ b/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; @@ -17,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; 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 +33,19 @@ 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"); + // this would really be called autoAlignWithIntermediateTransformUntilInPoseRelativeTolerances + // but that would be stupid. + // appreciate the concise new name! + public static Command superDuperAutoAlign( + Swerve drivetrain, + Supplier target, + Supplier intermediateTransfrom) { + return moveToPoseUntilInPoseRelativeTolerances(drivetrain, () -> target.get().transformBy(intermediateTransfrom.get()), ChassisSpeeds::new, + () -> MovingAutoAlignK.kXYConstraints) + .andThen(moveToPoseUntilInPoseRelativeTolerances(drivetrain, target, ChassisSpeeds::new, + () -> MovingAutoAlignK.kXYConstraints)); + } + /** *

Returns a Command that automatically aligns with an intermediate pose and target pose that will finish when it is * in tolerances. @@ -84,6 +101,20 @@ public static Command moveToPoseUntilInTolerances( .until(() -> AutoAlignUtils.isInTolerance(swerve.getState().Pose, target.get(), swerve.getState().Speeds)); } + public static Command moveToPoseUntilInPoseRelativeTolerances( + Swerve swerve, + Supplier target, + Supplier speedsModifier, + Supplier 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))); + } + /** *

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. diff --git a/src/main/java/frc/robot/autons/WaltAutonFactory.java b/src/main/java/frc/robot/autons/WaltAutonFactory.java index e81c3c8..115e22e 100644 --- a/src/main/java/frc/robot/autons/WaltAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltAutonFactory.java @@ -262,8 +262,9 @@ private Command autoAlignCommand(Supplier 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 ); From f0b25f1a3b29f7b2e44e60cc6b33dd7979d4d17c Mon Sep 17 00:00:00 2001 From: scootdboot Date: Thu, 17 Apr 2025 08:44:56 -0500 Subject: [PATCH 2/3] implement properly --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 058040d..84f08e4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 From 5105ca43afbf0f796eb280126ebdf1646237d734 Mon Sep 17 00:00:00 2001 From: scootdboot Date: Thu, 17 Apr 2025 10:04:03 -0500 Subject: [PATCH 3/3] target relative error logging --- src/main/java/frc/robot/autoalign/LegacyAutoAlign.java | 10 ++++++++++ src/main/java/frc/robot/autoalign/MovingAutoAlign.java | 9 +++++++++ 2 files changed, 19 insertions(+) diff --git a/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java b/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java index c8668ed..e35a5cd 100644 --- a/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java +++ b/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 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 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())); diff --git a/src/main/java/frc/robot/autoalign/MovingAutoAlign.java b/src/main/java/frc/robot/autoalign/MovingAutoAlign.java index f9523d1..c4d3b0c 100644 --- a/src/main/java/frc/robot/autoalign/MovingAutoAlign.java +++ b/src/main/java/frc/robot/autoalign/MovingAutoAlign.java @@ -19,6 +19,7 @@ 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; @@ -33,6 +34,10 @@ 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! @@ -161,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