From fa84f100a5729d1dd7d7c81cc31bc84a7ee91894 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Fri, 17 Jan 2025 21:39:58 -0600 Subject: [PATCH] Added heading offset. Signed-off-by: thenetworkgrinch --- src/main/java/frc/robot/RobotContainer.java | 53 +++++++------ .../java/swervelib/SwerveInputStream.java | 78 +++++++++++++++++-- 2 files changed, 103 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1cfcc7b5..18e27411 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,25 +56,33 @@ public class RobotContainer * Clone's the angular velocity input stream and converts it to a robotRelative input stream. */ SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) - .allianceRelativeControl(false); + .allianceRelativeControl(false); SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis(2)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX()) + .withControllerRotationAxis(() -> driverXbox.getRawAxis( + 2)) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocitySim.copy() - .withControllerHeadingAxis(() -> Math.sin( - driverXbox.getRawAxis( - 2) * Math.PI) * (Math.PI * 2), - () -> Math.cos( - driverXbox.getRawAxis( - 2) * Math.PI) * - (Math.PI * 2)) - .headingWhile(true); + SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() + .withControllerHeadingAxis(() -> + Math.sin( + driverXbox.getRawAxis( + 2) * + Math.PI) * + (Math.PI * + 2), + () -> + Math.cos( + driverXbox.getRawAxis( + 2) * + Math.PI) * + (Math.PI * + 2)) + .headingWhile(true); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -97,18 +105,19 @@ public RobotContainer() private void configureBindings() { - Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); - Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); - Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngle); + Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); + Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); + Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); + Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( + driveDirectAngle); Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); - Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( + Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( driveDirectAngleKeyboard); if (RobotBase.isSimulation()) { - drivebase.setDefaultCommand(driveFieldOrientedDirectAngleSim); + drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); } else { drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); diff --git a/src/main/java/swervelib/SwerveInputStream.java b/src/main/java/swervelib/SwerveInputStream.java index fe33cbec..ddb4c51b 100644 --- a/src/main/java/swervelib/SwerveInputStream.java +++ b/src/main/java/swervelib/SwerveInputStream.java @@ -113,6 +113,14 @@ public class SwerveInputStream implements Supplier * Field oriented chassis output is relative to your current alliance. */ private Optional allianceRelative = Optional.empty(); + /** + * Heading offset enable state. + */ + private Optional headingOffsetEnabled = Optional.empty(); + /** + * Heading offset to apply during heading based control. + */ + private Optional headingOffset = Optional.empty(); /** * {@link SwerveController} for simple control over heading. */ @@ -206,6 +214,8 @@ public SwerveInputStream copy() newStream.translationCube = translationCube; newStream.robotRelative = robotRelative; newStream.allianceRelative = allianceRelative; + newStream.headingOffsetEnabled = headingOffsetEnabled; + newStream.headingOffset = headingOffset; return newStream; } @@ -233,6 +243,42 @@ public SwerveInputStream robotRelative(boolean enabled) return this; } + /** + * Heading offset enabled boolean supplier. + * + * @param enabled Enable state + * @return self + */ + public SwerveInputStream headingOffset(BooleanSupplier enabled) + { + headingOffsetEnabled = Optional.of(enabled); + return this; + } + + /** + * Heading offset enable + * + * @param enabled Enable state + * @return self + */ + public SwerveInputStream headingOffset(boolean enabled) + { + headingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty(); + return this; + } + + /** + * Set the heading offset angle. + * + * @param angle {@link Rotation2d} offset to apply + * @return self + */ + public SwerveInputStream headingOffset(Rotation2d angle) + { + headingOffset = Optional.of(angle); + return this; + } + /** * Modify the output {@link ChassisSpeeds} so that it is always relative to your alliance. * @@ -709,6 +755,24 @@ private Rotation2d applyAllianceAwareRotation(Rotation2d fieldRelativeRotation) return fieldRelativeRotation; } + /** + * Adds offset to rotation if one is set. + * + * @param fieldRelativeRotation Field-relative {@link Rotation2d} to offset + * @return Offsetted {@link Rotation2d} + */ + private Rotation2d applyHeadingOffset(Rotation2d fieldRelativeRotation) + { + if (headingOffsetEnabled.isPresent() && headingOffsetEnabled.get().getAsBoolean()) + { + if (headingOffset.isPresent()) + { + return fieldRelativeRotation.rotateBy(headingOffset.get()); + } + } + return fieldRelativeRotation; + } + /** * Gets a {@link ChassisSpeeds} * @@ -759,12 +823,14 @@ public ChassisSpeeds get() case HEADING -> { omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(), - applyAllianceAwareRotation(Rotation2d.fromRadians( - swerveController.getJoystickAngle( - controllerHeadingX.get() - .getAsDouble(), - controllerHeadingY.get() - .getAsDouble()))).getRadians()); + applyHeadingOffset( + applyAllianceAwareRotation( + Rotation2d.fromRadians( + swerveController.getJoystickAngle( + controllerHeadingX.get() + .getAsDouble(), + controllerHeadingY.get() + .getAsDouble())))).getRadians()); speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); break; }