Skip to content

Commit

Permalink
Return self reference from ChassisSpeeds modifiers
Browse files Browse the repository at this point in the history
  • Loading branch information
mjansen4857 committed Nov 25, 2024
1 parent 1c220eb commit 3b546c4
Showing 1 changed file with 12 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -189,8 +189,9 @@ public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dt
* swerve drivetrain.
*
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Reference to itself
*/
public void discretize(double dtSeconds) {
public ChassisSpeeds discretize(double dtSeconds) {
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
Expand All @@ -205,6 +206,8 @@ public void discretize(double dtSeconds) {
vxMetersPerSecond = twist.dx / dtSeconds;
vyMetersPerSecond = twist.dy / dtSeconds;
omegaRadiansPerSecond = twist.dtheta / dtSeconds;

return this;
}

/**
Expand Down Expand Up @@ -285,13 +288,16 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return Reference to itself
*/
public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
public ChassisSpeeds toRobotRelativeSpeeds(Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated =
new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
vxMetersPerSecond = rotated.getX();
vyMetersPerSecond = rotated.getY();

return this;
}

/**
Expand Down Expand Up @@ -371,12 +377,15 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return Reference to itself
*/
public void toFieldRelativeSpeeds(Rotation2d robotAngle) {
public ChassisSpeeds toFieldRelativeSpeeds(Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
vxMetersPerSecond = rotated.getX();
vyMetersPerSecond = rotated.getY();

return this;
}

/**
Expand Down

0 comments on commit 3b546c4

Please sign in to comment.