Fix OTF aiming at field walls with velocity clamping

[phurley67]

Summary

• Adds getWallCorrectedChassisSpeeds() to Swerve that zeros the velocity component pointing off-field when the robot is within ~8 inches of a wall boundary
• getFuturePoseFromTime() and Cannon's hub angular velocity now use corrected speeds
• getCurrentRobotChassisSpeeds() is unchanged — PathPlanner and other consumers are unaffected

Problem

When strafing against a field wall, wheel odometry reports velocity into the wall (wheels spin but robot doesn't move). The OTF solver uses this phantom velocity to predict future pose, causing it to lead shots incorrectly.

Approach

Wall-aware clamping is preferred over pose differentiation because it:

• Adds zero noise (keeps 250Hz wheel odometry for normal driving)
• Has no latency (unlike vision-differentiated velocity)
• Only modifies the specific velocity component that's wrong
• Preserves parallel-to-wall velocity (which is correct and needed for OTF)

Test plan

• Verify static aiming is unaffected (robot not near walls)
• Test OTF while strafing along a wall — shots should no longer over-lead
• Test OTF while strafing into a corner — both X and Y clamping active
• Verify turret feedforward is smooth near walls (no jitter)

Closes #553

🤖 Generated with Claude Code

[Copilot]

Pull request overview

This PR addresses incorrect on-the-fly (OTF) shot leading when the drivetrain is pressed against a field wall by clamping “phantom” off-field velocity components near field boundaries, and then using those corrected speeds in OTF future-pose and hub angular velocity calculations.

Changes:

• Added Swerve.getWallCorrectedChassisSpeeds() to zero field-relative velocity components that point out of bounds when near a wall.
• Updated Swerve.getFuturePoseFromTime() to use wall-corrected chassis speeds for OTF pose prediction.
• Updated Cannon.getHubAngularVelocity(...) to use wall-corrected chassis speeds; added FieldConstants.FIELD_LENGTH/FIELD_WIDTH.

Reviewed changes

Copilot reviewed 3 out of 3 changed files in this pull request and generated 3 comments.

File	Description
src/main/java/frc/robot/subsystems/Swerve.java	Introduces wall-aware chassis-speed clamping and uses it for OTF future pose prediction.
src/main/java/frc/robot/subsystems/Cannon.java	Switches hub angular velocity computation to use wall-corrected drivetrain speeds.
src/main/java/frc/robot/constants/FieldConstants.java	Adds explicit field X/Y extents for wall boundary checks.

[Copilot]

WALL_MARGIN is compared against the robot pose translation, which is typically the robot’s center of rotation. With DriveConstants.RobotWidth = 30 in (half-width ~0.38 m), a margin of 0.20 m likely won’t trigger until the robot center is unrealistically close to/through the wall, so the clamping may never activate when actually pressed against a wall. Consider basing the margin on half the robot’s footprint (e.g., RobotWidth/2 plus tolerance) or explicit bumper-to-center X/Y extents.

[Copilot]

getHubAngularVelocity(Pose2d pose) mixes the passed-in pose (used for getTargetAngle(pose)) with the current distance (getTargetDistance()), which can be inconsistent—especially when this is called with a future-predicted pose during OTF. Use getTargetDistance(pose) so the geometry matches the provided pose. Also, getWallCorrectedChassisSpeeds() is called twice in the same expression; store it in a local variable to avoid potential intra-cycle inconsistencies and extra work.

[Copilot]

FIELD_LENGTH/FIELD_WIDTH introduce a second source of truth for the same dimensions already hard-coded elsewhere in this file (e.g., FIELD). To avoid future drift, consider defining the field dimensions in one place (have FIELD and other rectangles reference these constants, or derive these constants from FIELD).