Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add ChassisSpeeds.fromRobotRelativeSpeeds() (#5744)

Browse Source
This commit is contained in:
Brayden Zee
2023-10-17 13:13:04 -04:00
committed by GitHub
parent f98c943445
commit e814595ea7
4 changed files with 126 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
View File

@@ -108,8 +108,12 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
static ChassisSpeeds FromFieldRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
// CW rotation into chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
return {units::meters_per_second_t{rotated.X().value()},
units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
@@ -133,6 +137,54 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
fieldRelativeSpeeds.omega, robotAngle);
}
/**
* Converts a user provided robot-relative set of speeds into a field-relative
* ChassisSpeeds object.
*
* @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 towards the robot's left.
* @param omega The angular rate of the robot.
* @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.
*/
static ChassisSpeeds FromRobotRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
// CCW rotation out of chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
.RotateBy(robotAngle);
return {units::meters_per_second_t{rotated.X().value()},
units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Converts a user provided robot-relative ChassisSpeeds object into a
* field-relative ChassisSpeeds object.
*
* @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
* in the robot frame of reference. Positive x is the towards 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.
*/
static ChassisSpeeds FromRobotRelativeSpeeds(
const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
robotRelativeSpeeds.vy,
robotRelativeSpeeds.omega, robotAngle);
}
/**
* Adds two ChassisSpeeds and returns the sum.
*