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

[wpimath] Position Delta Odometry for Mecanum (#4514)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:59 -04:00
committed by GitHub
parent 4170ec6107
commit 901fc555f4
28 changed files with 1222 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -49,6 +49,18 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& wheelDeltas) const {
Vectord<4> wheelDeltasVector{
wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
return {units::meter_t{twistVector(0)}, // NOLINT
units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,