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] Change kinematics.ToTwist2d(end - start) to kinematics.ToTwist2d(start, end) (#5493)

Browse Source
This commit is contained in:
Joseph Eng
2023-08-01 22:25:26 -07:00
committed by GitHub
parent 815a8403e5
commit 0c93aded8a
23 changed files with 98 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -234,8 +234,16 @@ class SwerveDriveKinematics
wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
Twist2d ToTwist2d(
const SwerveDriveWheelPositions<NumModules>& wheelDeltas) const override {
return ToTwist2d(wheelDeltas.positions);
const SwerveDriveWheelPositions<NumModules>& start,
const SwerveDriveWheelPositions<NumModules>& end) const override {
auto result =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t i = 0; i < NumModules; i++) {
auto startModule = start.positions[i];
auto endModule = end.positions[i];
result[i] = {endModule.distance - startModule.distance, endModule.angle};
}
return ToTwist2d(result);
}
/**