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