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

[wpimath] Conserve previously calculated swerve module angles when updating states for stationary ChassisSpeeds (#4208)

Browse Source 
* Calculated swerve module states now stored in a member variable
* If ChassisSpeeds(0, 0, 0) is converted to module speeds, the
previously calculated module angle will be conserved, with forward speed
set to 0
* New tests added
This commit is contained in:
Kaitlyn Kenwell
2022-05-06 11:38:20 -04:00
committed by GitHub
parent ef7ed21a9d
commit 708a4bc3bc
5 changed files with 73 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -59,7 +59,7 @@ class SwerveDriveKinematics {
*/
template <typename... Wheels>
explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
: m_modules{wheel, wheels...} {
: m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
static_assert(sizeof...(wheels) >= 1,
"A swerve drive requires at least two modules");
@@ -79,7 +79,7 @@ class SwerveDriveKinematics {
explicit SwerveDriveKinematics(
const wpi::array<Translation2d, NumModules>& wheels)
: m_modules{wheels} {
: m_modules{wheels}, m_moduleStates(wpi::empty_array) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -107,6 +107,9 @@ class SwerveDriveKinematics {
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
*
* In the case that the desired chassis speeds are zero (i.e. the robot will
* be stationary), the previously calculated module angle will be maintained.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis speed
@@ -182,6 +185,7 @@ class SwerveDriveKinematics {
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
mutable wpi::array<SwerveModuleState, NumModules> m_moduleStates;
mutable Translation2d m_previousCoR;
};