diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 663ff8a51b..6c98337171 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -68,6 +68,27 @@ public class ChassisSpeeds { omegaRadiansPerSecond); } + /** + * Converts a user provided field-relative ChassisSpeeds object into a robot-relative + * ChassisSpeeds object. + * + * @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. + */ + public static ChassisSpeeds fromFieldRelativeSpeeds( + ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) { + return fromFieldRelativeSpeeds( + fieldRelativeSpeeds.vxMetersPerSecond, + fieldRelativeSpeeds.vyMetersPerSecond, + fieldRelativeSpeeds.omegaRadiansPerSecond, + robotAngle); + } + @Override public String toString() { return String.format( diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 7414deca21..37fe768911 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -60,5 +60,26 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { return {vx * robotAngle.Cos() + vy * robotAngle.Sin(), -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega}; } + + /** + * Converts a user provided field-relative ChassisSpeeds object into a + * robot-relative ChassisSpeeds object. + * + * @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. + */ + static ChassisSpeeds FromFieldRelativeSpeeds( + const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) { + return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx, + fieldRelativeSpeeds.vy, + fieldRelativeSpeeds.omega, robotAngle); + } }; } // namespace frc