Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +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

9
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
View File

@@ -128,7 +128,6 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
const WheelPositions& wheelPositions) {
m_odometry.Update(gyroAngle, wheelPositions);
// Copy?
WheelPositions internalWheelPositions = wheelPositions;
m_poseBuffer.AddSample(
@@ -151,16 +150,12 @@ PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
WheelPositions wheels_lerp =
this->wheelPositions.Interpolate(endValue.wheelPositions, i);
// Find the distance between this measurement and the
// interpolated measurement.
WheelPositions wheels_delta = wheels_lerp - this->wheelPositions;
// Find the new gyro angle.
auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
// Create a twist to represent this changed based on the interpolated
// Create a twist to represent the change based on the interpolated
// sensor inputs.
auto twist = kinematics.ToTwist2d(wheels_delta);
auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp);
twist.dtheta = (gyro - gyroAngle).Radians();
return {pose.Exp(twist), gyro, wheels_lerp};