Skip to content

Commit

Permalink
Added heading offset.
Browse files Browse the repository at this point in the history 
Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
  • Loading branch information
@thenetworkgrinch
thenetworkgrinch committed Jan 18, 2025
1 parent 9d023be commit fa84f10
Show file tree
Hide file tree
Showing 2 changed files with 103 additions and 28 deletions.
53 changes: 31 additions & 22 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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);
Expand Down
78 changes: 72 additions & 6 deletions src/main/java/swervelib/SwerveInputStream.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,14 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* Field oriented chassis output is relative to your current alliance.
*/
private Optional<BooleanSupplier> allianceRelative = Optional.empty();
/**
* Heading offset enable state.
*/
private Optional<BooleanSupplier> headingOffsetEnabled = Optional.empty();
/**
* Heading offset to apply during heading based control.
*/
private Optional<Rotation2d> headingOffset = Optional.empty();
/**
* {@link SwerveController} for simple control over heading.
*/
Expand Down Expand Up @@ -206,6 +214,8 @@ public SwerveInputStream copy()
newStream.translationCube = translationCube;
newStream.robotRelative = robotRelative;
newStream.allianceRelative = allianceRelative;
newStream.headingOffsetEnabled = headingOffsetEnabled;
newStream.headingOffset = headingOffset;
return newStream;
}

Expand Down Expand Up @@ -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.
*
Expand Down Expand Up @@ -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}
*
Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit fa84f10

Please sign in to comment.