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] Add methods to concatenate trajectories (#3139)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Prateek Machiraju
2021-02-16 21:06:36 -05:00
committed by GitHub
parent e42a0b6cf0
commit 9522f2e8c7
5 changed files with 163 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp Normal file
View File

@@ -0,0 +1,34 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
TEST(TrajectoryConcatenate, States) {
auto t1 = frc::TrajectoryGenerator::GenerateTrajectory(
{}, {}, {1_m, 1_m, 0_deg}, {2_mps, 2_mps_sq});
auto t2 = frc::TrajectoryGenerator::GenerateTrajectory(
{1_m, 1_m, 0_deg}, {}, {2_m, 2_m, 45_deg}, {2_mps, 2_mps_sq});
auto t = t1 + t2;
double time = -1.0;
for (size_t i = 0; i < t.States().size(); ++i) {
const auto& state = t.States()[i];
// Make sure that the timestamps are strictly increasing.
EXPECT_GT(state.t.to<double>(), time);
time = state.t.to<double>();
// Ensure that the states in t are the same as those in t1 and t2.
if (i < t1.States().size()) {
EXPECT_EQ(state, t1.States()[i]);
} else {
auto st = t2.States()[i - t1.States().size() + 1];
st.t += t1.TotalTime();
EXPECT_EQ(state, st);
}
}
}