From d09ad879c1b4f4cdb93a2248c84205d5bd1590bd Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Tue, 7 Apr 2026 19:51:40 -0400 Subject: [PATCH] Add wall-aware velocity clamping to fix OTF aiming at field walls When strafing against a wall, wheel odometry reports velocity into the wall even though the robot isn't moving that direction. This causes the OTF solver to lead shots based on phantom motion. Fix by zeroing the velocity component pointing off-field when the robot pose is within bumper-width of a wall boundary. Closes #553 Co-Authored-By: Claude Opus 4.6 (1M context) --- .../frc/robot/constants/FieldConstants.java | 3 ++ .../java/frc/robot/subsystems/Cannon.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 33 ++++++++++++++++++- 3 files changed, 36 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 510dc0c8..04d4d478 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -48,4 +48,7 @@ public static Pose2d getPose(TargetPose targetPose) { public static final Rectangle2d RED_NO_PASSING_ZONE = new Rectangle2d(new Translation2d(16.541, 4.473), new Translation2d(12.677, 3.486)); // temp, values found with sim public static final double FIELD_MIDDLE_Y = 4.034663; + + public static final double FIELD_LENGTH = 16.540988; // meters (X extent) + public static final double FIELD_WIDTH = 8.069326; // meters (Y extent) } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java index 4b76dbff..65180456 100644 --- a/src/main/java/frc/robot/subsystems/Cannon.java +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -413,7 +413,7 @@ public AngularVelocity getHubAngularVelocity(Pose2d pose) { double robotTargetAngle = getTargetAngle(pose).in(Radians); - Translation2d fieldRelativeVelocity = new Translation2d(drivetrain.getCurrentRobotChassisSpeeds().vxMetersPerSecond, drivetrain.getCurrentRobotChassisSpeeds().vyMetersPerSecond).rotateBy(drivetrain.getPose().getRotation()); + Translation2d fieldRelativeVelocity = new Translation2d(drivetrain.getWallCorrectedChassisSpeeds().vxMetersPerSecond, drivetrain.getWallCorrectedChassisSpeeds().vyMetersPerSecond).rotateBy(drivetrain.getPose().getRotation()); double hubRotation = (-fieldRelativeVelocity.getX() * Math.sin(robotTargetAngle) + fieldRelativeVelocity.getY() * Math.cos(robotTargetAngle)) / getTargetDistance().in(Meters); return RadiansPerSecond.of(hubRotation); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 03d4b3d6..b74a497b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -430,8 +430,39 @@ public ChassisSpeeds getCurrentRobotChassisSpeeds(){ return getState().Speeds; } - public Pose2d getFuturePoseFromTime(Time time) { + // When the robot is pressed against a field wall, wheels spin but the robot + // doesn't actually move into the wall. This zeros the velocity component + // pointing off-field so the OTF solver doesn't lead shots based on phantom motion. + private static final double WALL_MARGIN = 0.20; // meters (~8 in, bumper width + tolerance) + + public ChassisSpeeds getWallCorrectedChassisSpeeds() { ChassisSpeeds speeds = getCurrentRobotChassisSpeeds(); + Pose2d pose = getPose(); + + double sin = pose.getRotation().getSin(); + double cos = pose.getRotation().getCos(); + + // Robot-relative to field-relative + double fieldVx = speeds.vxMetersPerSecond * cos - speeds.vyMetersPerSecond * sin; + double fieldVy = speeds.vxMetersPerSecond * sin + speeds.vyMetersPerSecond * cos; + + Translation2d pos = pose.getTranslation(); + + // Clamp velocity component pointing out of the field near each wall + if (pos.getX() < WALL_MARGIN && fieldVx < 0) fieldVx = 0; + if (pos.getX() > FieldConstants.FIELD_LENGTH - WALL_MARGIN && fieldVx > 0) fieldVx = 0; + if (pos.getY() < WALL_MARGIN && fieldVy < 0) fieldVy = 0; + if (pos.getY() > FieldConstants.FIELD_WIDTH - WALL_MARGIN && fieldVy > 0) fieldVy = 0; + + // Field-relative back to robot-relative + double rrVx = fieldVx * cos + fieldVy * sin; + double rrVy = -fieldVx * sin + fieldVy * cos; + + return new ChassisSpeeds(rrVx, rrVy, speeds.omegaRadiansPerSecond); + } + + public Pose2d getFuturePoseFromTime(Time time) { + ChassisSpeeds speeds = getWallCorrectedChassisSpeeds(); double dt = time.in(Seconds); double driveMultiplier = LightningShuffleboard.getDouble("Cannon", "OTF Multiplier", 1);