Skip to content

Commit

Permalink
ChassisSpeeds factories use instance methods & un-deprecate
Browse files Browse the repository at this point in the history
  • Loading branch information
@mjansen4857
mjansen4857 committed Nov 29, 2024
1 parent 7753b30 commit d0b3f9a
Showing 1 changed file with 36 additions and 39 deletions.
75 changes: 36 additions & 39 deletions wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,33 +98,23 @@ public Twist2d toTwist2d(double dtSeconds) {
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param omegaRadiansPerSecond Angular velocity.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
double dtSeconds) {
// Construct the desired pose after a timestep, relative to the current pose. The desired pose
// has decoupled translation and rotation.
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
new Rotation2d(omegaRadiansPerSecond * dtSeconds));

// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = Pose2d.kZero.log(desiredDeltaPose);

// Turn the chassis translation/rotation deltas into average velocities
return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
var speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
speeds.discretize(dtSeconds);
return speeds;
}

/**
Expand All @@ -138,14 +128,15 @@ public static ChassisSpeeds discretize(
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
* @param omega Angular velocity.
* @param dt The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) {
return discretize(
Expand All @@ -163,12 +154,13 @@ public static ChassisSpeeds discretize(
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param continuousSpeeds The continuous speeds.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
return discretize(
continuousSpeeds.vxMetersPerSecond,
Expand Down Expand Up @@ -211,6 +203,9 @@ public void discretize(double dtSeconds) {
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vxMetersPerSecond The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vyMetersPerSecond The component of speed in the y direction relative to the field.
Expand All @@ -220,24 +215,24 @@ public void discretize(double dtSeconds) {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated =
new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
var speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
speeds.toRobotRelativeSpeeds(robotAngle);
return speeds;
}

/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vx The component of speed in the x direction relative to the field. Positive x is away
* from your alliance wall.
* @param vy The component of speed in the y direction relative to the field. Positive y is to
Expand All @@ -247,9 +242,7 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
Expand All @@ -260,16 +253,17 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
* Converts a user provided field-relative ChassisSpeeds object into a robot-relative
* ChassisSpeeds object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds in the field frame
* of reference. Positive x is away from your alliance wall. Positive y is to your left when
* standing behind the alliance wall.
* @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 ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
Expand Down Expand Up @@ -298,6 +292,9 @@ public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vxMetersPerSecond The component of speed in the x direction relative to the robot.
* Positive x is towards the robot's front.
* @param vyMetersPerSecond The component of speed in the y direction relative to the robot.
Expand All @@ -307,23 +304,24 @@ public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
var speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
speeds.toFieldRelativeSpeeds(robotAngle);
return speeds;
}

/**
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vx The component of speed in the x direction relative to the robot. Positive x is
* towards the robot's front.
* @param vy The component of speed in the y direction relative to the robot. Positive y is
Expand All @@ -333,9 +331,7 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
Expand All @@ -346,16 +342,17 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
* Converts a user provided robot-relative ChassisSpeeds object into a field-relative
* ChassisSpeeds object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame
* of reference. Positive x is towards the robot's front. Positive y is towards the robot's
* left.
* @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 ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
Expand Down

0 comments on commit d0b3f9a

Please sign in to comment.