Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Return self reference from ChassisSpeeds modifiers #7433

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+12 −3
Open

Return self reference from ChassisSpeeds modifiers #7433

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 12 additions & 3 deletions wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View file
Open in desktop
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
Loading