Skip to content

Commit fa84f10

Browse files
thenetworkgrinchthenetworkgrinch
committed
Added heading offset.
Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
1 parent 9d023be commit fa84f10

File tree

2 files changed

+103
-28
lines changed

2 files changed

+103
-28
lines changed

‎src/main/java/frc/robot/RobotContainer.java

+31-22
Original file line numberDiff line numberDiff line change
@@ -56,25 +56,33 @@ public class RobotContainer
5656
* Clone's the angular velocity input stream and converts it to a robotRelative input stream.
5757
*/
5858
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true)
59-
.allianceRelativeControl(false);
59+
.allianceRelativeControl(false);
6060

6161
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
62-
() -> -driverXbox.getLeftY(),
63-
() -> -driverXbox.getLeftX())
64-
.withControllerRotationAxis(() -> driverXbox.getRawAxis(2))
65-
.deadband(OperatorConstants.DEADBAND)
66-
.scaleTranslation(0.8)
67-
.allianceRelativeControl(true);
62+
() -> -driverXbox.getLeftY(),
63+
() -> -driverXbox.getLeftX())
64+
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
65+
2))
66+
.deadband(OperatorConstants.DEADBAND)
67+
.scaleTranslation(0.8)
68+
.allianceRelativeControl(true);
6869
// Derive the heading axis with math!
69-
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocitySim.copy()
70-
.withControllerHeadingAxis(() -> Math.sin(
71-
driverXbox.getRawAxis(
72-
2) * Math.PI) * (Math.PI * 2),
73-
() -> Math.cos(
74-
driverXbox.getRawAxis(
75-
2) * Math.PI) *
76-
(Math.PI * 2))
77-
.headingWhile(true);
70+
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
71+
.withControllerHeadingAxis(() ->
72+
Math.sin(
73+
driverXbox.getRawAxis(
74+
2) *
75+
Math.PI) *
76+
(Math.PI *
77+
2),
78+
() ->
79+
Math.cos(
80+
driverXbox.getRawAxis(
81+
2) *
82+
Math.PI) *
83+
(Math.PI *
84+
2))
85+
.headingWhile(true);
7886

7987
/**
8088
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -97,18 +105,19 @@ public RobotContainer()
97105
private void configureBindings()
98106
{
99107

100-
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
101-
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
102-
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
103-
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngle);
108+
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
109+
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
110+
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
111+
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
112+
driveDirectAngle);
104113
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
105114
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
106-
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
115+
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
107116
driveDirectAngleKeyboard);
108117

109118
if (RobotBase.isSimulation())
110119
{
111-
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleSim);
120+
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
112121
} else
113122
{
114123
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);

‎src/main/java/swervelib/SwerveInputStream.java

+72-6
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,14 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
113113
* Field oriented chassis output is relative to your current alliance.
114114
*/
115115
private Optional<BooleanSupplier> allianceRelative = Optional.empty();
116+
/**
117+
* Heading offset enable state.
118+
*/
119+
private Optional<BooleanSupplier> headingOffsetEnabled = Optional.empty();
120+
/**
121+
* Heading offset to apply during heading based control.
122+
*/
123+
private Optional<Rotation2d> headingOffset = Optional.empty();
116124
/**
117125
* {@link SwerveController} for simple control over heading.
118126
*/
@@ -206,6 +214,8 @@ public SwerveInputStream copy()
206214
newStream.translationCube = translationCube;
207215
newStream.robotRelative = robotRelative;
208216
newStream.allianceRelative = allianceRelative;
217+
newStream.headingOffsetEnabled = headingOffsetEnabled;
218+
newStream.headingOffset = headingOffset;
209219
return newStream;
210220
}
211221

@@ -233,6 +243,42 @@ public SwerveInputStream robotRelative(boolean enabled)
233243
return this;
234244
}
235245

246+
/**
247+
* Heading offset enabled boolean supplier.
248+
*
249+
* @param enabled Enable state
250+
* @return self
251+
*/
252+
public SwerveInputStream headingOffset(BooleanSupplier enabled)
253+
{
254+
headingOffsetEnabled = Optional.of(enabled);
255+
return this;
256+
}
257+
258+
/**
259+
* Heading offset enable
260+
*
261+
* @param enabled Enable state
262+
* @return self
263+
*/
264+
public SwerveInputStream headingOffset(boolean enabled)
265+
{
266+
headingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
267+
return this;
268+
}
269+
270+
/**
271+
* Set the heading offset angle.
272+
*
273+
* @param angle {@link Rotation2d} offset to apply
274+
* @return self
275+
*/
276+
public SwerveInputStream headingOffset(Rotation2d angle)
277+
{
278+
headingOffset = Optional.of(angle);
279+
return this;
280+
}
281+
236282
/**
237283
* Modify the output {@link ChassisSpeeds} so that it is always relative to your alliance.
238284
*
@@ -709,6 +755,24 @@ private Rotation2d applyAllianceAwareRotation(Rotation2d fieldRelativeRotation)
709755
return fieldRelativeRotation;
710756
}
711757

758+
/**
759+
* Adds offset to rotation if one is set.
760+
*
761+
* @param fieldRelativeRotation Field-relative {@link Rotation2d} to offset
762+
* @return Offsetted {@link Rotation2d}
763+
*/
764+
private Rotation2d applyHeadingOffset(Rotation2d fieldRelativeRotation)
765+
{
766+
if (headingOffsetEnabled.isPresent() && headingOffsetEnabled.get().getAsBoolean())
767+
{
768+
if (headingOffset.isPresent())
769+
{
770+
return fieldRelativeRotation.rotateBy(headingOffset.get());
771+
}
772+
}
773+
return fieldRelativeRotation;
774+
}
775+
712776
/**
713777
* Gets a {@link ChassisSpeeds}
714778
*
@@ -759,12 +823,14 @@ public ChassisSpeeds get()
759823
case HEADING ->
760824
{
761825
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
762-
applyAllianceAwareRotation(Rotation2d.fromRadians(
763-
swerveController.getJoystickAngle(
764-
controllerHeadingX.get()
765-
.getAsDouble(),
766-
controllerHeadingY.get()
767-
.getAsDouble()))).getRadians());
826+
applyHeadingOffset(
827+
applyAllianceAwareRotation(
828+
Rotation2d.fromRadians(
829+
swerveController.getJoystickAngle(
830+
controllerHeadingX.get()
831+
.getAsDouble(),
832+
controllerHeadingY.get()
833+
.getAsDouble())))).getRadians());
768834
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
769835
break;
770836
}

0 commit comments

Comments
 (0)