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

[wpimath] Add TimeInterpolatableBuffer (#2695)

Browse Source 
These classes are useful for storing previous robot positions to use in conjunction with the upcoming pose estimators.

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: cttew <cttewari@gmail.com>
This commit is contained in:
Matt
2021-12-30 20:08:05 -07:00
committed by GitHub
parent b8d019cdb4
commit 315be873c4
14 changed files with 485 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -6,6 +6,7 @@
#include <algorithm>
#include <wpi/MathExtras.h>
#include <wpi/json.h>
#include "units/math.h"
@@ -25,7 +26,7 @@ bool Trajectory::State::operator!=(const Trajectory::State& other) const {
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
const auto newT = Lerp(t, endValue.t, i);
const auto newT = wpi::Lerp(t, endValue.t, i);
// Find the delta time between the current state and the interpolated state.
const auto deltaT = newT - t;
@@ -58,8 +59,8 @@ Trajectory::State Trajectory::State::Interpolate(State endValue,
newS / endValue.pose.Translation().Distance(pose.Translation());
return {newT, newV, acceleration,
Lerp(pose, endValue.pose, interpolationFrac),
Lerp(curvature, endValue.curvature, interpolationFrac)};
wpi::Lerp(pose, endValue.pose, interpolationFrac),
wpi::Lerp(curvature, endValue.curvature, interpolationFrac)};
}
Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {