Skip to content

Fix OTF aiming at field walls with velocity clamping #554

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
phurley67 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix OTF aiming at field walls with velocity clamping #554

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/constants/FieldConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Comment on lines 50 to +53

Copilot AI Apr 7, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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).

Copilot uses AI. Check for mistakes.
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/Cannon.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Comment on lines 414 to 418

Copilot AI Apr 7, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 uses AI. Check for mistakes.
return RadiansPerSecond.of(hubRotation);
Expand Down
33 changes: 32 additions & 1 deletion src/main/java/frc/robot/subsystems/Swerve.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Comment on lines +433 to +437

Copilot AI Apr 7, 2026

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 uses AI. Check for mistakes.
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);
Expand Down
Loading