Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] FromFieldRelativeSpeeds: Add ChassisSpeeds overload (#4494)

Browse Source
This commit is contained in:
shueja-personal
2022-10-21 18:58:23 -07:00
committed by GitHub
parent 9c7e66a27d
commit 3e5bfff1b5
2 changed files with 42 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -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(

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

@@ -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