Rename our allwpilib (which is now 2020) to not have 2019 in the name
Change-Id: I3c07f85ed32ab8b97db765a9b43f2a6ce7da964a
diff --git a/wpilibc/src/test/native/c/test.c b/wpilibc/src/test/native/c/test.c
new file mode 100644
index 0000000..105e289
--- /dev/null
+++ b/wpilibc/src/test/native/c/test.c
@@ -0,0 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cscore.h>
+#include <hal/HAL.h>
+#include <ntcore.h>
diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
new file mode 100644
index 0000000..0bb1002
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <memory>
+#include <random>
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+// Filter constants
+static constexpr units::second_t kFilterStep = 0.005_s;
+static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr int32_t kMovAvgTaps = 6;
+
+enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
+
+std::ostream& operator<<(std::ostream& os,
+ const LinearFilterNoiseTestType& type) {
+ switch (type) {
+ case TEST_SINGLE_POLE_IIR:
+ os << "LinearFilter SinglePoleIIR";
+ break;
+ case TEST_MOVAVG:
+ os << "LinearFilter MovingAverage";
+ break;
+ }
+
+ return os;
+}
+
+static double GetData(double t) {
+ return 100.0 * std::sin(2.0 * wpi::math::pi * t);
+}
+
+class LinearFilterNoiseTest
+ : public testing::TestWithParam<LinearFilterNoiseTestType> {
+ protected:
+ std::unique_ptr<frc::LinearFilter<double>> m_filter;
+
+ void SetUp() override {
+ switch (GetParam()) {
+ case TEST_SINGLE_POLE_IIR: {
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+ kFilterStep));
+ break;
+ }
+
+ case TEST_MOVAVG: {
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+ break;
+ }
+ }
+ }
+};
+
+/**
+ * Test if the filter reduces the noise produced by a signal generator
+ */
+TEST_P(LinearFilterNoiseTest, NoiseReduce) {
+ double noiseGenError = 0.0;
+ double filterError = 0.0;
+
+ std::random_device rd;
+ std::mt19937 gen{rd()};
+ std::normal_distribution<double> distr{0.0, 10.0};
+
+ for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+ double theory = GetData(t.to<double>());
+ double noise = distr(gen);
+ filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
+ noiseGenError += std::abs(noise - theory);
+ }
+
+ RecordProperty("FilterError", filterError);
+
+ // The filter should have produced values closer to the theory
+ EXPECT_GT(noiseGenError, filterError)
+ << "Filter should have reduced noise accumulation but failed";
+}
+
+INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
+ testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));
diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
new file mode 100644
index 0000000..8db91b3
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+// Filter constants
+static constexpr units::second_t kFilterStep = 0.005_s;
+static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
+static constexpr double kHighPassTimeConstant = 0.006631;
+static constexpr double kHighPassExpectedOutput = 10.074717;
+static constexpr int32_t kMovAvgTaps = 6;
+static constexpr double kMovAvgExpectedOutput = -10.191644;
+
+enum LinearFilterOutputTestType {
+ TEST_SINGLE_POLE_IIR,
+ TEST_HIGH_PASS,
+ TEST_MOVAVG,
+ TEST_PULSE
+};
+
+std::ostream& operator<<(std::ostream& os,
+ const LinearFilterOutputTestType& type) {
+ switch (type) {
+ case TEST_SINGLE_POLE_IIR:
+ os << "LinearFilter SinglePoleIIR";
+ break;
+ case TEST_HIGH_PASS:
+ os << "LinearFilter HighPass";
+ break;
+ case TEST_MOVAVG:
+ os << "LinearFilter MovingAverage";
+ break;
+ case TEST_PULSE:
+ os << "LinearFilter Pulse";
+ break;
+ }
+
+ return os;
+}
+
+static double GetData(double t) {
+ return 100.0 * std::sin(2.0 * wpi::math::pi * t) +
+ 20.0 * std::cos(50.0 * wpi::math::pi * t);
+}
+
+static double GetPulseData(double t) {
+ if (std::abs(t - 1.0) < 0.001) {
+ return 1.0;
+ } else {
+ return 0.0;
+ }
+}
+
+/**
+ * A fixture that includes a consistent data source wrapped in a filter
+ */
+class LinearFilterOutputTest
+ : public testing::TestWithParam<LinearFilterOutputTestType> {
+ protected:
+ std::unique_ptr<frc::LinearFilter<double>> m_filter;
+ std::function<double(double)> m_data;
+ double m_expectedOutput = 0.0;
+
+ void SetUp() override {
+ switch (GetParam()) {
+ case TEST_SINGLE_POLE_IIR: {
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+ kFilterStep));
+ m_data = GetData;
+ m_expectedOutput = kSinglePoleIIRExpectedOutput;
+ break;
+ }
+
+ case TEST_HIGH_PASS: {
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+ kFilterStep));
+ m_data = GetData;
+ m_expectedOutput = kHighPassExpectedOutput;
+ break;
+ }
+
+ case TEST_MOVAVG: {
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+ m_data = GetData;
+ m_expectedOutput = kMovAvgExpectedOutput;
+ break;
+ }
+
+ case TEST_PULSE: {
+ m_filter = std::make_unique<frc::LinearFilter<double>>(
+ frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+ m_data = GetPulseData;
+ m_expectedOutput = 0.0;
+ break;
+ }
+ }
+ }
+};
+
+/**
+ * Test if the linear filters produce consistent output for a given data set.
+ */
+TEST_P(LinearFilterOutputTest, Output) {
+ double filterOutput = 0.0;
+ for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+ filterOutput = m_filter->Calculate(m_data(t.to<double>()));
+ }
+
+ RecordProperty("LinearFilterOutput", filterOutput);
+
+ EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+ << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
+ testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
+ TEST_MOVAVG, TEST_PULSE));
diff --git a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
new file mode 100644
index 0000000..7fe7d2e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/MedianFilter.h"
+#include "gtest/gtest.h"
+
+TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
+ frc::MedianFilter<double> filter{10};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(4);
+
+ EXPECT_EQ(filter.Calculate(1000), 3.5);
+}
+
+TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
+ frc::MedianFilter<double> filter{10};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(4);
+ filter.Calculate(7);
+
+ EXPECT_EQ(filter.Calculate(1000), 4);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestEven) {
+ frc::MedianFilter<double> filter{6};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(0);
+ filter.Calculate(5);
+ filter.Calculate(4);
+ filter.Calculate(1000);
+
+ EXPECT_EQ(filter.Calculate(99), 4.5);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestOdd) {
+ frc::MedianFilter<double> filter{5};
+
+ filter.Calculate(3);
+ filter.Calculate(0);
+ filter.Calculate(5);
+ filter.Calculate(4);
+ filter.Calculate(1000);
+
+ EXPECT_EQ(filter.Calculate(99), 5);
+}
diff --git a/wpilibc/src/test/native/cpp/MockSpeedController.cpp b/wpilibc/src/test/native/cpp/MockSpeedController.cpp
new file mode 100644
index 0000000..875fec1
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/MockSpeedController.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "MockSpeedController.h"
+
+using namespace frc;
+
+void MockSpeedController::Set(double speed) {
+ m_speed = m_isInverted ? -speed : speed;
+}
+
+double MockSpeedController::Get() const { return m_speed; }
+
+void MockSpeedController::SetInverted(bool isInverted) {
+ m_isInverted = isInverted;
+}
+
+bool MockSpeedController::GetInverted() const { return m_isInverted; }
+
+void MockSpeedController::Disable() { m_speed = 0; }
+
+void MockSpeedController::StopMotor() { Disable(); }
+
+void MockSpeedController::PIDWrite(double output) { Set(output); }
diff --git a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
new file mode 100644
index 0000000..ae253a7
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <thread>
+
+#include "frc/SlewRateLimiter.h"
+#include "gtest/gtest.h"
+
+TEST(SlewRateLimiterTest, SlewRateLimitTest) {
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
+}
+
+TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
+}
diff --git a/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp b/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
new file mode 100644
index 0000000..cb81fc9
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SpeedControllerGroup.h" // NOLINT(build/include_order)
+
+#include <memory>
+#include <vector>
+
+#include "MockSpeedController.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+enum SpeedControllerGroupTestType { TEST_ONE, TEST_TWO, TEST_THREE };
+
+std::ostream& operator<<(std::ostream& os,
+ const SpeedControllerGroupTestType& type) {
+ switch (type) {
+ case TEST_ONE:
+ os << "SpeedControllerGroup with one speed controller";
+ break;
+ case TEST_TWO:
+ os << "SpeedControllerGroup with two speed controllers";
+ break;
+ case TEST_THREE:
+ os << "SpeedControllerGroup with three speed controllers";
+ break;
+ }
+
+ return os;
+}
+
+/**
+ * A fixture used for SpeedControllerGroup testing.
+ */
+class SpeedControllerGroupTest
+ : public testing::TestWithParam<SpeedControllerGroupTestType> {
+ protected:
+ std::vector<MockSpeedController> m_speedControllers;
+ std::unique_ptr<SpeedControllerGroup> m_group;
+
+ void SetUp() override {
+ switch (GetParam()) {
+ case TEST_ONE: {
+ m_speedControllers.emplace_back();
+ m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0]);
+ break;
+ }
+
+ case TEST_TWO: {
+ m_speedControllers.emplace_back();
+ m_speedControllers.emplace_back();
+ m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+ m_speedControllers[1]);
+ break;
+ }
+
+ case TEST_THREE: {
+ m_speedControllers.emplace_back();
+ m_speedControllers.emplace_back();
+ m_speedControllers.emplace_back();
+ m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+ m_speedControllers[1],
+ m_speedControllers[2]);
+ break;
+ }
+ }
+ }
+};
+
+TEST_P(SpeedControllerGroupTest, Set) {
+ m_group->Set(1.0);
+
+ for (auto& speedController : m_speedControllers) {
+ EXPECT_FLOAT_EQ(speedController.Get(), 1.0);
+ }
+}
+
+TEST_P(SpeedControllerGroupTest, GetInverted) {
+ m_group->SetInverted(true);
+
+ EXPECT_TRUE(m_group->GetInverted());
+}
+
+TEST_P(SpeedControllerGroupTest, SetInvertedDoesNotModifySpeedControllers) {
+ for (auto& speedController : m_speedControllers) {
+ speedController.SetInverted(false);
+ }
+ m_group->SetInverted(true);
+
+ for (auto& speedController : m_speedControllers) {
+ EXPECT_EQ(speedController.GetInverted(), false);
+ }
+}
+
+TEST_P(SpeedControllerGroupTest, SetInvertedDoesInvert) {
+ m_group->SetInverted(true);
+ m_group->Set(1.0);
+
+ for (auto& speedController : m_speedControllers) {
+ EXPECT_FLOAT_EQ(speedController.Get(), -1.0);
+ }
+}
+
+TEST_P(SpeedControllerGroupTest, Disable) {
+ m_group->Set(1.0);
+ m_group->Disable();
+
+ for (auto& speedController : m_speedControllers) {
+ EXPECT_FLOAT_EQ(speedController.Get(), 0.0);
+ }
+}
+
+TEST_P(SpeedControllerGroupTest, StopMotor) {
+ m_group->Set(1.0);
+ m_group->StopMotor();
+
+ for (auto& speedController : m_speedControllers) {
+ EXPECT_FLOAT_EQ(speedController.Get(), 0.0);
+ }
+}
+
+TEST_P(SpeedControllerGroupTest, PIDWrite) {
+ m_group->PIDWrite(1.0);
+
+ for (auto& speedController : m_speedControllers) {
+ EXPECT_FLOAT_EQ(speedController.Get(), 1.0);
+ }
+}
+
+INSTANTIATE_TEST_SUITE_P(Test, SpeedControllerGroupTest,
+ testing::Values(TEST_ONE, TEST_TWO, TEST_THREE));
diff --git a/wpilibc/src/test/native/cpp/WatchdogTest.cpp b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
new file mode 100644
index 0000000..10ff996
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
@@ -0,0 +1,173 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Watchdog.h" // NOLINT(build/include_order)
+
+#include <stdint.h>
+
+#include <thread>
+
+#include <wpi/raw_ostream.h>
+
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_EnableDisable) {
+#else
+TEST(WatchdogTest, EnableDisable) {
+#endif
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
+
+ wpi::outs() << "Run 1\n";
+ watchdog.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ watchdog.Disable();
+
+ EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
+
+ wpi::outs() << "Run 2\n";
+ watchdogCounter = 0;
+ watchdog.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(600));
+ watchdog.Disable();
+
+ EXPECT_EQ(1u, watchdogCounter)
+ << "Watchdog either didn't trigger or triggered more than once";
+
+ wpi::outs() << "Run 3\n";
+ watchdogCounter = 0;
+ watchdog.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+ watchdog.Disable();
+
+ EXPECT_EQ(1u, watchdogCounter)
+ << "Watchdog either didn't trigger or triggered more than once";
+}
+
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_Reset) {
+#else
+TEST(WatchdogTest, Reset) {
+#endif
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
+
+ watchdog.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ watchdog.Reset();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ watchdog.Disable();
+
+ EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
+}
+
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_SetTimeout) {
+#else
+TEST(WatchdogTest, SetTimeout) {
+#endif
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(1.0_s, [&] { watchdogCounter++; });
+
+ watchdog.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ watchdog.SetTimeout(0.2_s);
+
+ EXPECT_EQ(0.2, watchdog.GetTimeout());
+ EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+ watchdog.Disable();
+
+ EXPECT_EQ(1u, watchdogCounter)
+ << "Watchdog either didn't trigger or triggered more than once";
+}
+
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_IsExpired) {
+#else
+TEST(WatchdogTest, IsExpired) {
+#endif
+ Watchdog watchdog(0.2_s, [] {});
+ EXPECT_FALSE(watchdog.IsExpired());
+ watchdog.Enable();
+
+ EXPECT_FALSE(watchdog.IsExpired());
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+ EXPECT_TRUE(watchdog.IsExpired());
+
+ watchdog.Disable();
+ EXPECT_TRUE(watchdog.IsExpired());
+
+ watchdog.Reset();
+ EXPECT_FALSE(watchdog.IsExpired());
+}
+
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_Epochs) {
+#else
+TEST(WatchdogTest, Epochs) {
+#endif
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
+
+ wpi::outs() << "Run 1\n";
+ watchdog.Enable();
+ watchdog.AddEpoch("Epoch 1");
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ watchdog.AddEpoch("Epoch 2");
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ watchdog.AddEpoch("Epoch 3");
+ watchdog.Disable();
+
+ EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
+
+ wpi::outs() << "Run 2\n";
+ watchdog.Enable();
+ watchdog.AddEpoch("Epoch 1");
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ watchdog.Reset();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ watchdog.AddEpoch("Epoch 2");
+ watchdog.Disable();
+
+ EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
+}
+
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_MultiWatchdog) {
+#else
+TEST(WatchdogTest, MultiWatchdog) {
+#endif
+ uint32_t watchdogCounter1 = 0;
+ uint32_t watchdogCounter2 = 0;
+
+ Watchdog watchdog1(0.2_s, [&] { watchdogCounter1++; });
+ Watchdog watchdog2(0.6_s, [&] { watchdogCounter2++; });
+
+ watchdog2.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ EXPECT_EQ(0u, watchdogCounter1) << "Watchdog triggered early";
+ EXPECT_EQ(0u, watchdogCounter2) << "Watchdog triggered early";
+
+ // Sleep enough such that only the watchdog enabled later times out first
+ watchdog1.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+ watchdog1.Disable();
+ watchdog2.Disable();
+
+ EXPECT_EQ(1u, watchdogCounter1)
+ << "Watchdog either didn't trigger or triggered more than once";
+ EXPECT_EQ(0u, watchdogCounter2) << "Watchdog triggered early";
+}
diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
new file mode 100644
index 0000000..c68cb37
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+class PIDInputOutputTest : public testing::Test {
+ protected:
+ frc2::PIDController* controller;
+
+ void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+
+ void TearDown() override { delete controller; }
+};
+
+TEST_F(PIDInputOutputTest, ContinuousInputTest) {
+ controller->SetP(1);
+ controller->EnableContinuousInput(-180, 180);
+
+ EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
+}
+
+TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
+ controller->SetP(4);
+
+ EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025, 0));
+}
+
+TEST_F(PIDInputOutputTest, IntegralGainOutputTest) {
+ controller->SetI(4);
+
+ double out = 0;
+
+ for (int i = 0; i < 5; i++) {
+ out = controller->Calculate(0.025, 0);
+ }
+
+ EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
+ controller->SetD(4);
+
+ controller->Calculate(0, 0);
+
+ EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+ controller->Calculate(0.0025, 0));
+}
diff --git a/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
new file mode 100644
index 0000000..3251098
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+static constexpr double kSetpoint = 50.0;
+static constexpr double kRange = 200;
+static constexpr double kTolerance = 10.0;
+
+TEST(PIDToleranceTest, InitialTolerance) {
+ frc2::PIDController controller{0.5, 0.0, 0.0};
+ controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+ EXPECT_TRUE(controller.AtSetpoint());
+}
+
+TEST(PIDToleranceTest, AbsoluteTolerance) {
+ frc2::PIDController controller{0.5, 0.0, 0.0};
+ controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+ controller.SetTolerance(kTolerance);
+ controller.SetSetpoint(kSetpoint);
+
+ controller.Calculate(0.0);
+
+ EXPECT_FALSE(controller.AtSetpoint())
+ << "Error was in tolerance when it should not have been. Error was "
+ << controller.GetPositionError();
+
+ controller.Calculate(kSetpoint + kTolerance / 2);
+
+ EXPECT_TRUE(controller.AtSetpoint())
+ << "Error was not in tolerance when it should have been. Error was "
+ << controller.GetPositionError();
+
+ controller.Calculate(kSetpoint + 10 * kTolerance);
+
+ EXPECT_FALSE(controller.AtSetpoint())
+ << "Error was in tolerance when it should not have been. Error was "
+ << controller.GetPositionError();
+}
diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
new file mode 100644
index 0000000..a600054
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/controller/RamseteController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
+
+units::radian_t boundRadians(units::radian_t value) {
+ while (value > units::radian_t{wpi::math::pi}) {
+ value -= units::radian_t{wpi::math::pi * 2};
+ }
+ while (value <= units::radian_t{-wpi::math::pi}) {
+ value += units::radian_t{wpi::math::pi * 2};
+ }
+ return value;
+}
+
+TEST(RamseteControllerTest, ReachesReference) {
+ frc::RamseteController controller{2.0, 0.7};
+ frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+ auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+ frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ constexpr auto kDt = 0.02_s;
+ auto totalTime = trajectory.TotalTime();
+ for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
+ auto state = trajectory.Sample(kDt * i);
+ auto [vx, vy, omega] = controller.Calculate(robotPose, state);
+ static_cast<void>(vy);
+
+ robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
+ }
+
+ auto& endPose = trajectory.States().back().pose;
+ EXPECT_NEAR_UNITS(endPose.Translation().X(), robotPose.Translation().X(),
+ kTolerance);
+ EXPECT_NEAR_UNITS(endPose.Translation().Y(), robotPose.Translation().Y(),
+ kTolerance);
+ EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() -
+ robotPose.Rotation().Radians()),
+ 0_rad, kAngularTolerance);
+}
diff --git a/wpilibc/src/test/native/cpp/drive/DriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DriveTest.cpp
new file mode 100644
index 0000000..1ebb6b3
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/drive/DriveTest.cpp
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "MockSpeedController.h"
+#include "frc/RobotDrive.h"
+#include "frc/drive/DifferentialDrive.h"
+#include "frc/drive/MecanumDrive.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class DriveTest : public testing::Test {
+ protected:
+ MockSpeedController m_rdFrontLeft;
+ MockSpeedController m_rdRearLeft;
+ MockSpeedController m_rdFrontRight;
+ MockSpeedController m_rdRearRight;
+ MockSpeedController m_frontLeft;
+ MockSpeedController m_rearLeft;
+ MockSpeedController m_frontRight;
+ MockSpeedController m_rearRight;
+ frc::RobotDrive m_robotDrive{m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight,
+ m_rdRearRight};
+ frc::DifferentialDrive m_differentialDrive{m_frontLeft, m_frontRight};
+ frc::MecanumDrive m_mecanumDrive{m_frontLeft, m_rearLeft, m_frontRight,
+ m_rearRight};
+
+ double m_testJoystickValues[9] = {-1.0, -0.9, -0.5, -0.01, 0.0,
+ 0.01, 0.5, 0.9, 1.0};
+ double m_testGyroValues[19] = {0, 45, 90, 135, 180, 225, 270,
+ 305, 360, 540, -45, -90, -135, -180,
+ -225, -270, -305, -360, -540};
+};
+
+TEST_F(DriveTest, TankDrive) {
+ int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+ double leftJoystick, rightJoystick;
+ m_differentialDrive.SetDeadband(0.0);
+ m_differentialDrive.SetSafetyEnabled(false);
+ m_mecanumDrive.SetSafetyEnabled(false);
+ m_robotDrive.SetSafetyEnabled(false);
+ for (int i = 0; i < joystickSize; i++) {
+ for (int j = 0; j < joystickSize; j++) {
+ leftJoystick = m_testJoystickValues[i];
+ rightJoystick = m_testJoystickValues[j];
+ m_robotDrive.TankDrive(leftJoystick, rightJoystick, false);
+ m_differentialDrive.TankDrive(leftJoystick, rightJoystick, false);
+ ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+ ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+ }
+ }
+}
+
+TEST_F(DriveTest, TankDriveSquared) {
+ int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+ double leftJoystick, rightJoystick;
+ m_differentialDrive.SetDeadband(0.0);
+ m_differentialDrive.SetSafetyEnabled(false);
+ m_mecanumDrive.SetSafetyEnabled(false);
+ m_robotDrive.SetSafetyEnabled(false);
+ for (int i = 0; i < joystickSize; i++) {
+ for (int j = 0; j < joystickSize; j++) {
+ leftJoystick = m_testJoystickValues[i];
+ rightJoystick = m_testJoystickValues[j];
+ m_robotDrive.TankDrive(leftJoystick, rightJoystick, true);
+ m_differentialDrive.TankDrive(leftJoystick, rightJoystick, true);
+ ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+ ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+ }
+ }
+}
+
+TEST_F(DriveTest, ArcadeDriveSquared) {
+ int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+ double moveJoystick, rotateJoystick;
+ m_differentialDrive.SetDeadband(0.0);
+ m_differentialDrive.SetSafetyEnabled(false);
+ m_mecanumDrive.SetSafetyEnabled(false);
+ m_robotDrive.SetSafetyEnabled(false);
+ for (int i = 0; i < joystickSize; i++) {
+ for (int j = 0; j < joystickSize; j++) {
+ moveJoystick = m_testJoystickValues[i];
+ rotateJoystick = m_testJoystickValues[j];
+ m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, true);
+ m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, true);
+ ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+ ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+ }
+ }
+}
+
+TEST_F(DriveTest, ArcadeDrive) {
+ int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+ double moveJoystick, rotateJoystick;
+ m_differentialDrive.SetDeadband(0.0);
+ m_differentialDrive.SetSafetyEnabled(false);
+ m_mecanumDrive.SetSafetyEnabled(false);
+ m_robotDrive.SetSafetyEnabled(false);
+ for (int i = 0; i < joystickSize; i++) {
+ for (int j = 0; j < joystickSize; j++) {
+ moveJoystick = m_testJoystickValues[i];
+ rotateJoystick = m_testJoystickValues[j];
+ m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, false);
+ m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, false);
+ ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+ ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+ }
+ }
+}
+
+TEST_F(DriveTest, MecanumCartesian) {
+ int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+ int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
+ double xJoystick, yJoystick, rotateJoystick, gyroValue;
+ m_mecanumDrive.SetDeadband(0.0);
+ m_mecanumDrive.SetSafetyEnabled(false);
+ m_differentialDrive.SetSafetyEnabled(false);
+ m_robotDrive.SetSafetyEnabled(false);
+ for (int i = 0; i < joystickSize; i++) {
+ for (int j = 0; j < joystickSize; j++) {
+ for (int k = 0; k < joystickSize; k++) {
+ for (int l = 0; l < gyroSize; l++) {
+ xJoystick = m_testJoystickValues[i];
+ yJoystick = m_testJoystickValues[j];
+ rotateJoystick = m_testJoystickValues[k];
+ gyroValue = m_testGyroValues[l];
+ m_robotDrive.MecanumDrive_Cartesian(xJoystick, yJoystick,
+ rotateJoystick, gyroValue);
+ m_mecanumDrive.DriveCartesian(xJoystick, -yJoystick, rotateJoystick,
+ -gyroValue);
+ ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
+ << "X: " << xJoystick << " Y: " << yJoystick
+ << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+ ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
+ << "X: " << xJoystick << " Y: " << yJoystick
+ << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+ ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
+ << "X: " << xJoystick << " Y: " << yJoystick
+ << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+ ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
+ << "X: " << xJoystick << " Y: " << yJoystick
+ << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+ }
+ }
+ }
+ }
+}
+
+TEST_F(DriveTest, MecanumPolar) {
+ int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+ int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
+ double magnitudeJoystick, directionJoystick, rotateJoystick;
+ m_mecanumDrive.SetDeadband(0.0);
+ m_mecanumDrive.SetSafetyEnabled(false);
+ m_differentialDrive.SetSafetyEnabled(false);
+ m_robotDrive.SetSafetyEnabled(false);
+ for (int i = 0; i < joystickSize; i++) {
+ for (int j = 0; j < gyroSize; j++) {
+ for (int k = 0; k < joystickSize; k++) {
+ magnitudeJoystick = m_testJoystickValues[i];
+ directionJoystick = m_testGyroValues[j];
+ rotateJoystick = m_testJoystickValues[k];
+ m_robotDrive.MecanumDrive_Polar(magnitudeJoystick, directionJoystick,
+ rotateJoystick);
+ m_mecanumDrive.DrivePolar(magnitudeJoystick, directionJoystick,
+ rotateJoystick);
+ ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
+ << "Magnitude: " << magnitudeJoystick
+ << " Direction: " << directionJoystick
+ << " Rotate: " << rotateJoystick;
+ ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
+ << "Magnitude: " << magnitudeJoystick
+ << " Direction: " << directionJoystick
+ << " Rotate: " << rotateJoystick;
+ ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
+ << "Magnitude: " << magnitudeJoystick
+ << " Direction: " << directionJoystick
+ << " Rotate: " << rotateJoystick;
+ ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
+ << "Magnitude: " << magnitudeJoystick
+ << " Direction: " << directionJoystick
+ << " Rotate: " << rotateJoystick;
+ }
+ }
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
new file mode 100644
index 0000000..8b5f674
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Pose2dTest, TransformBy) {
+ const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
+ const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+
+ const auto transformed = initial + transform;
+
+ EXPECT_NEAR(transformed.Translation().X().to<double>(),
+ 1 + 5 / std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(transformed.Translation().Y().to<double>(),
+ 2 + 5 / std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
+}
+
+TEST(Pose2dTest, RelativeTo) {
+ const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+ const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+ const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+ EXPECT_NEAR(finalRelativeToInitial.Translation().X().to<double>(),
+ 5.0 * std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(finalRelativeToInitial.Translation().Y().to<double>(), 0.0,
+ kEpsilon);
+ EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
+ kEpsilon);
+}
+
+TEST(Pose2dTest, Equality) {
+ const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+ const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
+ EXPECT_TRUE(a == b);
+}
+
+TEST(Pose2dTest, Inequality) {
+ const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+ const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
+ EXPECT_TRUE(a != b);
+}
+
+TEST(Pose2dTest, Minus) {
+ const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+ const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+ const auto transform = final - initial;
+
+ EXPECT_NEAR(transform.Translation().X().to<double>(), 5.0 * std::sqrt(2.0),
+ kEpsilon);
+ EXPECT_NEAR(transform.Translation().Y().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
new file mode 100644
index 0000000..ba80787
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Rotation2dTest, RadiansToDegrees) {
+ const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
+ const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
+
+ EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
+ EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, DegreesToRadians) {
+ const auto one = Rotation2d(45.0_deg);
+ const auto two = Rotation2d(30.0_deg);
+
+ EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
+ EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByFromZero) {
+ const Rotation2d zero;
+ auto sum = zero + Rotation2d(90.0_deg);
+
+ EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
+ EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByNonZero) {
+ auto rot = Rotation2d(90.0_deg);
+ rot += Rotation2d(30.0_deg);
+
+ EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Minus) {
+ const auto one = Rotation2d(70.0_deg);
+ const auto two = Rotation2d(30.0_deg);
+
+ EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Equality) {
+ const auto one = Rotation2d(43_deg);
+ const auto two = Rotation2d(43_deg);
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Rotation2dTest, Inequality) {
+ const auto one = Rotation2d(43_deg);
+ const auto two = Rotation2d(43.5_deg);
+ EXPECT_TRUE(one != two);
+}
diff --git a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
new file mode 100644
index 0000000..99fced7
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Translation2dTest, Sum) {
+ const Translation2d one{1.0_m, 3.0_m};
+ const Translation2d two{2.0_m, 5.0_m};
+
+ const auto sum = one + two;
+
+ EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
+ EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Difference) {
+ const Translation2d one{1.0_m, 3.0_m};
+ const Translation2d two{2.0_m, 5.0_m};
+
+ const auto difference = one - two;
+
+ EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
+ EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
+}
+
+TEST(Translation2dTest, RotateBy) {
+ const Translation2d another{3.0_m, 0.0_m};
+ const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
+
+ EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Multiplication) {
+ const Translation2d original{3.0_m, 5.0_m};
+ const auto mult = original * 3;
+
+ EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
+ EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
+}
+
+TEST(Translation2d, Division) {
+ const Translation2d original{3.0_m, 5.0_m};
+ const auto div = original / 2;
+
+ EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
+ EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
+}
+
+TEST(Translation2dTest, Norm) {
+ const Translation2d one{3.0_m, 5.0_m};
+ EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
+}
+
+TEST(Translation2dTest, Distance) {
+ const Translation2d one{1_m, 1_m};
+ const Translation2d two{6_m, 6_m};
+ EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
+}
+
+TEST(Translation2dTest, UnaryMinus) {
+ const Translation2d original{-4.5_m, 7_m};
+ const auto inverted = -original;
+
+ EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
+ EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
+}
+
+TEST(Translation2dTest, Equality) {
+ const Translation2d one{9_m, 5.5_m};
+ const Translation2d two{9_m, 5.5_m};
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Translation2dTest, Inequality) {
+ const Translation2d one{9_m, 5.5_m};
+ const Translation2d two{9_m, 5.7_m};
+ EXPECT_TRUE(one != two);
+}
diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
new file mode 100644
index 0000000..faf36d9
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Twist2dTest, Straight) {
+ const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
+ const auto straightPose = Pose2d().Exp(straight);
+
+ EXPECT_NEAR(straightPose.Translation().X().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(straightPose.Translation().Y().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, QuarterCircle) {
+ const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
+ units::radian_t(wpi::math::pi / 2.0)};
+ const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+
+ EXPECT_NEAR(quarterCirclePose.Translation().X().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(quarterCirclePose.Translation().Y().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
+ kEpsilon);
+}
+
+TEST(Twist2dTest, DiagonalNoDtheta) {
+ const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
+ const auto diagonalPose = Pose2d().Exp(diagonal);
+
+ EXPECT_NEAR(diagonalPose.Translation().X().to<double>(), 2.0, kEpsilon);
+ EXPECT_NEAR(diagonalPose.Translation().Y().to<double>(), 2.0, kEpsilon);
+ EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, Equality) {
+ const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+ const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Twist2dTest, Inequality) {
+ const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+ const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
+ EXPECT_TRUE(one != two);
+}
+
+TEST(Twist2dTest, Pose2dLog) {
+ const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
+ const Pose2d start{};
+
+ const auto twist = start.Log(end);
+
+ EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
new file mode 100644
index 0000000..3fb63fb
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(ChassisSpeeds, FieldRelativeConstruction) {
+ const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+ 1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
+
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
+ EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
+ EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
new file mode 100644
index 0000000..ad0d3c7
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const ChassisSpeeds chassisSpeeds;
+ const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const DifferentialDriveWheelSpeeds wheelSpeeds;
+ const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
+ const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
+ const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
+ units::radians_per_second_t{wpi::math::pi}};
+ const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const DifferentialDriveWheelSpeeds wheelSpeeds{
+ units::meters_per_second_t(+0.381 * wpi::math::pi),
+ units::meters_per_second_t(-0.381 * wpi::math::pi)};
+ const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
new file mode 100644
index 0000000..8acaf92
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+using namespace frc;
+
+TEST(DifferentialDriveOdometry, EncoderDistances) {
+ DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
+
+ const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
+ units::meter_t(5 * wpi::math::pi));
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
new file mode 100644
index 0000000..75f395d
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -0,0 +1,230 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class MecanumDriveKinematicsTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
+};
+
+TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
+ ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
+ */
+
+ EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
+ 3.536_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
+ [[5][0][0]]
+ */
+
+ EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
+ */
+
+ EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
+ -2.828427_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
+ [[5][0][0]]
+ */
+
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps,
+ units::radians_per_second_t(2 * wpi::math::pi)};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
+ */
+
+ EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
+ -106.62919_mps, 106.62919_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
+ be [[0][0][2pi]]
+ */
+
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
+ ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
+ */
+
+ EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
+ -13.435_mps, 16.26_mps};
+
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
+ [[2][3][1]]
+ */
+
+ EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
+ */
+
+ EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
+ 33.941_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from the
+ wheel velocities should be [[12][-12][1]]
+ */
+
+ EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
+ OffCenterTranslationRotationInverseKinematics) {
+ ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
+ */
+ EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
+ OffCenterTranslationRotationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
+ 36.06_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from the
+ wheel velocities should be [[17][-10][1]]
+ */
+
+ EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
+ MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
+ wheelSpeeds.Normalize(5.5_mps);
+
+ double kFactor = 5.5 / 7.0;
+
+ EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
new file mode 100644
index 0000000..5d990bf
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class MecanumDriveOdometryTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
+ MecanumDriveOdometry odometry{kinematics, 0_rad};
+};
+
+TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
+ odometry.ResetPosition(Pose2d(), 0_rad);
+ MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
+ 3.536_mps};
+
+ odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
+ auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
+
+ EXPECT_NEAR(secondPose.Translation().X().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(secondPose.Translation().Y().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, TwoIterations) {
+ odometry.ResetPosition(Pose2d(), 0_rad);
+ MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+ odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
+ auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
+ odometry.ResetPosition(Pose2d(), 0_rad);
+ MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
+ 39.986_mps};
+ odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
+ auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 12, 0.01);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
+ EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
+ odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+ MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+ odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
+ auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
new file mode 100644
index 0000000..8fd67ea
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 0.1;
+
+class SwerveDriveKinematicsTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
+};
+
+TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
+ ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
+
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
+ SwerveModuleState state{5.0_mps, Rotation2d()};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
+ SwerveModuleState state{5_mps, Rotation2d(90_deg)};
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps,
+ units::radians_per_second_t(2 * wpi::math::pi)};
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
+ SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
+ SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
+ SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
+ SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps,
+ units::radians_per_second_t(2 * wpi::math::pi)};
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
+ SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
+ SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
+ SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
+ SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+ OffCenterCORRotationAndTranslationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
+ auto [fl, fr, bl, br] =
+ m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
+
+ EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+ OffCenterCORRotationAndTranslationForwardKinematics) {
+ SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
+ SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
+ SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
+ SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
+ SwerveModuleState state1{5.0_mps, Rotation2d()};
+ SwerveModuleState state2{6.0_mps, Rotation2d()};
+ SwerveModuleState state3{4.0_mps, Rotation2d()};
+ SwerveModuleState state4{7.0_mps, Rotation2d()};
+
+ std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+ SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
+
+ double kFactor = 5.5 / 7.0;
+
+ EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
new file mode 100644
index 0000000..d0399dc
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 0.01;
+
+class SwerveDriveOdometryTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
+ SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
+};
+
+TEST_F(SwerveDriveOdometryTest, TwoIterations) {
+ SwerveModuleState state{5_mps, Rotation2d()};
+
+ m_odometry.ResetPosition(Pose2d(), 0_rad);
+ m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
+ SwerveModuleState(), SwerveModuleState(),
+ SwerveModuleState());
+ auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
+ state, state);
+
+ EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
+
+TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
+ SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
+ SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
+ SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
+ SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
+
+ SwerveModuleState zero{0_mps, Rotation2d()};
+
+ m_odometry.ResetPosition(Pose2d(), 0_rad);
+ m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
+ auto pose =
+ m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
+
+ EXPECT_NEAR(12.0, pose.Translation().X().to<double>(), kEpsilon);
+ EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
+ EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
+
+TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
+ m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+ SwerveModuleState state{5_mps, Rotation2d()};
+
+ m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
+ SwerveModuleState(), SwerveModuleState(),
+ SwerveModuleState());
+ auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
+ state, state);
+
+ EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/main.cpp b/wpilibc/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..c6b6c58
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/main.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+ HAL_Initialize(500, 0);
+ ::testing::InitGoogleTest(&argc, argv);
+ int ret = RUN_ALL_TESTS();
+ return ret;
+}
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp b/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
new file mode 100644
index 0000000..172d7c6
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "shuffleboard/MockActuatorSendable.h"
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+MockActuatorSendable::MockActuatorSendable(wpi::StringRef name) {
+ SendableRegistry::GetInstance().Add(this, name);
+}
+
+void MockActuatorSendable::InitSendable(SendableBuilder& builder) {
+ builder.SetActuator(true);
+}
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp
new file mode 100644
index 0000000..ae21526
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardInstance.h" // NOLINT(build/include_order)
+
+#include <memory>
+#include <string>
+
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "frc/shuffleboard/ShuffleboardInstance.h"
+#include "gtest/gtest.h"
+#include "shuffleboard/MockActuatorSendable.h"
+
+using namespace frc;
+
+class ShuffleboardInstanceTest : public testing::Test {
+ void SetUp() override {
+ m_ntInstance = nt::NetworkTableInstance::Create();
+ m_shuffleboardInstance =
+ std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
+ }
+
+ protected:
+ nt::NetworkTableInstance m_ntInstance;
+ std::unique_ptr<detail::ShuffleboardInstance> m_shuffleboardInstance;
+};
+
+TEST_F(ShuffleboardInstanceTest, PathFluent) {
+ auto entry = m_shuffleboardInstance->GetTab("Tab Title")
+ .GetLayout("List Layout", "List")
+ .Add("Data", "string")
+ .WithWidget("Text View")
+ .GetEntry();
+
+ EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
+ EXPECT_EQ("/Shuffleboard/Tab Title/List Layout/Data", entry.GetName())
+ << "Entry path generated incorrectly";
+}
+
+TEST_F(ShuffleboardInstanceTest, NestedLayoutsFluent) {
+ auto entry = m_shuffleboardInstance->GetTab("Tab")
+ .GetLayout("First", "List")
+ .GetLayout("Second", "List")
+ .GetLayout("Third", "List")
+ .GetLayout("Fourth", "List")
+ .Add("Value", "string")
+ .GetEntry();
+
+ EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
+ EXPECT_EQ("/Shuffleboard/Tab/First/Second/Third/Fourth/Value",
+ entry.GetName())
+ << "Entry path generated incorrectly";
+}
+
+TEST_F(ShuffleboardInstanceTest, NestedLayoutsOop) {
+ ShuffleboardTab& tab = m_shuffleboardInstance->GetTab("Tab");
+ ShuffleboardLayout& first = tab.GetLayout("First", "List");
+ ShuffleboardLayout& second = first.GetLayout("Second", "List");
+ ShuffleboardLayout& third = second.GetLayout("Third", "List");
+ ShuffleboardLayout& fourth = third.GetLayout("Fourth", "List");
+ SimpleWidget& widget = fourth.Add("Value", "string");
+ auto entry = widget.GetEntry();
+
+ EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
+ EXPECT_EQ("/Shuffleboard/Tab/First/Second/Third/Fourth/Value",
+ entry.GetName())
+ << "Entry path generated incorrectly";
+}
+
+TEST_F(ShuffleboardInstanceTest, LayoutTypeIsSet) {
+ std::string layoutType = "Type";
+ m_shuffleboardInstance->GetTab("Tab").GetLayout("Title", layoutType);
+ m_shuffleboardInstance->Update();
+ nt::NetworkTableEntry entry = m_ntInstance.GetEntry(
+ "/Shuffleboard/.metadata/Tab/Title/PreferredComponent");
+ EXPECT_EQ(layoutType, entry.GetString("Not Set")) << "Layout type not set";
+}
+
+TEST_F(ShuffleboardInstanceTest, NestedActuatorWidgetsAreDisabled) {
+ MockActuatorSendable sendable("Actuator");
+ m_shuffleboardInstance->GetTab("Tab")
+ .GetLayout("Title", "Type")
+ .Add(sendable);
+ auto controllableEntry =
+ m_ntInstance.GetEntry("/Shuffleboard/Tab/Title/Actuator/.controllable");
+ m_shuffleboardInstance->Update();
+
+ // Note: we use the unsafe `GetBoolean()` method because if the value is NOT
+ // a boolean, or if it is not present, then something has clearly gone very,
+ // very wrong
+ bool controllable = controllableEntry.GetValue()->GetBoolean();
+ // Sanity check
+ EXPECT_TRUE(controllable)
+ << "The nested actuator widget should be enabled by default";
+ m_shuffleboardInstance->DisableActuatorWidgets();
+ controllable = controllableEntry.GetValue()->GetBoolean();
+ EXPECT_FALSE(controllable)
+ << "The nested actuator widget should have been disabled";
+}
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTest.cpp
new file mode 100644
index 0000000..d39d59d
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTest.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/Shuffleboard.h"
+#include "frc/shuffleboard/ShuffleboardTab.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class ShuffleboardTest : public testing::Test {};
+
+TEST_F(ShuffleboardTest, TabObjectsCached) {
+ ShuffleboardTab& tab1 = Shuffleboard::GetTab("testTabObjectsCached");
+ ShuffleboardTab& tab2 = Shuffleboard::GetTab("testTabObjectsCached");
+ EXPECT_EQ(&tab1, &tab2) << "Tab objects were not cached";
+}
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp
new file mode 100644
index 0000000..8e39915
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "frc/shuffleboard/ShuffleboardInstance.h"
+#include "frc/shuffleboard/ShuffleboardTab.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class SuppliedValueWidgetTest : public testing::Test {
+ void SetUp() override {
+ m_ntInstance = nt::NetworkTableInstance::Create();
+ m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
+ m_tab = &(m_instance->GetTab("Tab"));
+ }
+
+ protected:
+ nt::NetworkTableInstance m_ntInstance;
+ ShuffleboardTab* m_tab;
+ std::unique_ptr<detail::ShuffleboardInstance> m_instance;
+};
+
+TEST_F(SuppliedValueWidgetTest, AddString) {
+ std::string str = "foo";
+ m_tab->AddString("String", [&str]() { return str; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/String");
+
+ m_instance->Update();
+ EXPECT_EQ("foo", entry.GetValue()->GetString());
+}
+
+TEST_F(SuppliedValueWidgetTest, AddNumber) {
+ int num = 0;
+ m_tab->AddNumber("Num", [&num]() { return ++num; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Num");
+
+ m_instance->Update();
+ EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
+}
+
+TEST_F(SuppliedValueWidgetTest, AddBoolean) {
+ bool value = true;
+ m_tab->AddBoolean("Bool", [&value]() { return value; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Bool");
+
+ m_instance->Update();
+ EXPECT_EQ(true, entry.GetValue()->GetBoolean());
+}
+
+TEST_F(SuppliedValueWidgetTest, AddStringArray) {
+ std::vector<std::string> strings = {"foo", "bar"};
+ m_tab->AddStringArray("Strings", [&strings]() { return strings; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Strings");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetStringArray();
+
+ EXPECT_EQ(strings.size(), actual.size());
+ for (size_t i = 0; i < strings.size(); i++) {
+ EXPECT_EQ(strings[i], actual[i]);
+ }
+}
+
+TEST_F(SuppliedValueWidgetTest, AddNumberArray) {
+ std::vector<double> nums = {0, 1, 2, 3};
+ m_tab->AddNumberArray("Numbers", [&nums]() { return nums; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Numbers");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetDoubleArray();
+
+ EXPECT_EQ(nums.size(), actual.size());
+ for (size_t i = 0; i < nums.size(); i++) {
+ EXPECT_FLOAT_EQ(nums[i], actual[i]);
+ }
+}
+
+TEST_F(SuppliedValueWidgetTest, AddBooleanArray) {
+ std::vector<int> bools = {true, false};
+ m_tab->AddBooleanArray("Booleans", [&bools]() { return bools; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Booleans");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetBooleanArray();
+
+ EXPECT_EQ(bools.size(), actual.size());
+ for (size_t i = 0; i < bools.size(); i++) {
+ EXPECT_FLOAT_EQ(bools[i], actual[i]);
+ }
+}
+
+TEST_F(SuppliedValueWidgetTest, AddRaw) {
+ wpi::StringRef bytes = "\1\2\3";
+ m_tab->AddRaw("Raw", [&bytes]() { return bytes; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Raw");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetRaw();
+ EXPECT_EQ(bytes, actual);
+}
diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
new file mode 100644
index 0000000..f2f0a38
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace frc {
+class CubicHermiteSplineTest : public ::testing::Test {
+ protected:
+ static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
+ const Pose2d& b) {
+ // Start the timer.
+ const auto start = std::chrono::high_resolution_clock::now();
+
+ // Generate and parameterize the spline.
+
+ const auto [startCV, endCV] =
+ SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b);
+
+ const auto splines =
+ SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
+ std::vector<Spline<3>::PoseWithCurvature> poses;
+
+ poses.push_back(splines[0].GetPoint(0.0));
+
+ for (auto&& spline : splines) {
+ auto x = SplineParameterizer::Parameterize(spline);
+ poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
+ }
+
+ // End timer.
+ const auto finish = std::chrono::high_resolution_clock::now();
+
+ // Calculate the duration (used when benchmarking)
+ const auto duration =
+ std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+ for (unsigned int i = 0; i < poses.size() - 1; i++) {
+ auto& p0 = poses[i];
+ auto& p1 = poses[i + 1];
+
+ // Make sure the twist is under the tolerance defined by the Spline class.
+ auto twist = p0.first.Log(p1.first);
+ EXPECT_LT(std::abs(twist.dx.to<double>()),
+ SplineParameterizer::kMaxDx.to<double>());
+ EXPECT_LT(std::abs(twist.dy.to<double>()),
+ SplineParameterizer::kMaxDy.to<double>());
+ EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+ SplineParameterizer::kMaxDtheta.to<double>());
+ }
+
+ // Check first point.
+ EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
+ a.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
+ a.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+ a.Rotation().Radians().to<double>(), 1E-9);
+
+ // Check interior waypoints
+ bool interiorsGood = true;
+ for (auto& waypoint : waypoints) {
+ bool found = false;
+ for (auto& state : poses) {
+ if (std::abs(
+ waypoint.Distance(state.first.Translation()).to<double>()) <
+ 1E-9) {
+ found = true;
+ }
+ }
+ interiorsGood &= found;
+ }
+
+ EXPECT_TRUE(interiorsGood);
+
+ // Check last point.
+ EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
+ b.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
+ b.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+ b.Rotation().Radians().to<double>(), 1E-9);
+
+ static_cast<void>(duration);
+ }
+};
+} // namespace frc
+
+TEST_F(CubicHermiteSplineTest, StraightLine) {
+ Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(CubicHermiteSplineTest, SCurve) {
+ Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
+ std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
+ Translation2d(2_m, -1_m)};
+ Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
+ Run(start, waypoints, end);
+}
+
+TEST_F(CubicHermiteSplineTest, OneInterior) {
+ Pose2d start{0_m, 0_m, 0_rad};
+ std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
+ Pose2d end{4_m, 0_m, 0_rad};
+ Run(start, waypoints, end);
+}
+
+TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
+ EXPECT_THROW(
+ Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
+ Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
+ SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(
+ Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
+ Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
+ SplineParameterizer::MalformedSplineException);
+}
diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
new file mode 100644
index 0000000..8a87053
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace frc {
+class QuinticHermiteSplineTest : public ::testing::Test {
+ protected:
+ static void Run(const Pose2d& a, const Pose2d& b) {
+ // Start the timer.
+ const auto start = std::chrono::high_resolution_clock::now();
+
+ // Generate and parameterize the spline.
+ const auto spline = SplineHelper::QuinticSplinesFromControlVectors(
+ SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0];
+ const auto poses = SplineParameterizer::Parameterize(spline);
+
+ // End timer.
+ const auto finish = std::chrono::high_resolution_clock::now();
+
+ // Calculate the duration (used when benchmarking)
+ const auto duration =
+ std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+ for (unsigned int i = 0; i < poses.size() - 1; i++) {
+ auto& p0 = poses[i];
+ auto& p1 = poses[i + 1];
+
+ // Make sure the twist is under the tolerance defined by the Spline class.
+ auto twist = p0.first.Log(p1.first);
+ EXPECT_LT(std::abs(twist.dx.to<double>()),
+ SplineParameterizer::kMaxDx.to<double>());
+ EXPECT_LT(std::abs(twist.dy.to<double>()),
+ SplineParameterizer::kMaxDy.to<double>());
+ EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+ SplineParameterizer::kMaxDtheta.to<double>());
+ }
+
+ // Check first point.
+ EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
+ a.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
+ a.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+ a.Rotation().Radians().to<double>(), 1E-9);
+
+ // Check last point.
+ EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
+ b.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
+ b.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+ b.Rotation().Radians().to<double>(), 1E-9);
+
+ static_cast<void>(duration);
+ }
+};
+} // namespace frc
+
+TEST_F(QuinticHermiteSplineTest, StraightLine) {
+ Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
+ Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
+ Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
+ Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
+}
+
+TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
+ EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+ Pose2d(1_m, 0_m, Rotation2d(180_deg))),
+ SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
+ Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
+ SplineParameterizer::MalformedSplineException);
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
new file mode 100644
index 0000000..37f471a
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(CentripetalAccelerationConstraintTest, Constraint) {
+ const auto maxCentripetalAcceleration = 7_fps_sq;
+
+ auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+ config.AddConstraint(
+ CentripetalAccelerationConstraint(maxCentripetalAcceleration));
+
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ auto centripetalAcceleration =
+ units::math::pow<2>(point.velocity) * point.curvature / 1_rad;
+
+ EXPECT_TRUE(centripetalAcceleration <
+ maxCentripetalAcceleration + 0.05_mps_sq);
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
new file mode 100644
index 0000000..df5ddbd
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
+ const auto maxVelocity = 12_fps;
+ const DifferentialDriveKinematics kinematics{27_in};
+
+ auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+ config.AddConstraint(
+ DifferentialDriveKinematicsConstraint(kinematics, maxVelocity));
+
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+ point.velocity * point.curvature};
+
+ auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_TRUE(left < maxVelocity + 0.05_mps);
+ EXPECT_TRUE(right < maxVelocity + 0.05_mps);
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
new file mode 100644
index 0000000..eb230da
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
+ // Pick an unreasonably large kA to ensure the constraint has to do some work
+ SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
+ 3_V / 1_mps_sq};
+ const DifferentialDriveKinematics kinematics{0.5_m};
+ const auto maxVoltage = 10_V;
+
+ auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+ config.AddConstraint(
+ DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
+
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+ point.velocity * point.curvature};
+
+ auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ // Not really a strictly-correct test as we're using the chassis accel
+ // instead of the wheel accel, but much easier than doing it "properly" and
+ // a reasonable check anyway
+ EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
+ maxVoltage + 0.05_V);
+ EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
+ -maxVoltage - 0.05_V);
+ EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
+ maxVoltage + 0.05_V);
+ EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
+ -maxVoltage - 0.05_V);
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
new file mode 100644
index 0000000..90046c5
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(TrajectoryGenerationTest, ObeysConstraints) {
+ TrajectoryConfig config{12_fps, 12_fps_sq};
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps);
+ EXPECT_TRUE(units::math::abs(point.acceleration) <=
+ 12_fps_sq + 0.01_fps_sq);
+ }
+}
+
+TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
+ const auto t = TrajectoryGenerator::GenerateTrajectory(
+ std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+ Pose2d(1_m, 0_m, Rotation2d(180_deg))},
+ TrajectoryConfig(12_fps, 12_fps_sq));
+
+ ASSERT_EQ(t.States().size(), 1u);
+ ASSERT_EQ(t.TotalTime(), 0_s);
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
new file mode 100644
index 0000000..999d2bb
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(TrajectoryJsonTest, DeserializeMatches) {
+ TrajectoryConfig config{12_fps, 12_fps_sq};
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ Trajectory deserialized;
+ EXPECT_NO_THROW(deserialized = TrajectoryUtil::DeserializeTrajectory(
+ TrajectoryUtil::SerializeTrajectory(trajectory)));
+ EXPECT_EQ(trajectory.States(), deserialized.States());
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
new file mode 100644
index 0000000..af69b58
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
+ std::vector<frc::Trajectory::State> statesB) {
+ for (unsigned int i = 0; i < statesA.size() - 1; i++) {
+ auto a1 = statesA[i].pose;
+ auto a2 = statesA[i + 1].pose;
+
+ auto b1 = statesB[i].pose;
+ auto b2 = statesB[i + 1].pose;
+
+ auto a = a2.RelativeTo(a1);
+ auto b = b2.RelativeTo(b1);
+
+ EXPECT_NEAR(a.Translation().X().to<double>(),
+ b.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(a.Translation().Y().to<double>(),
+ b.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(a.Rotation().Radians().to<double>(),
+ b.Rotation().Radians().to<double>(), 1E-9);
+ }
+}
+
+TEST(TrajectoryTransforms, TransformBy) {
+ frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
+ config);
+
+ auto transformedTrajectory =
+ trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
+
+ auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+ EXPECT_NEAR(firstPose.Translation().X().to<double>(), 1.0, 1E-9);
+ EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 2.0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
+
+ TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}
+
+TEST(TrajectoryTransforms, RelativeTo) {
+ frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
+ frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
+
+ auto transformedTrajectory =
+ trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
+
+ auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+ EXPECT_NEAR(firstPose.Translation().X().to<double>(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
+
+ TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
new file mode 100644
index 0000000..bd9ffef
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -0,0 +1,233 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrapezoidProfile.h" // NOLINT(build/include_order)
+
+#include <chrono>
+#include <cmath>
+
+#include "gtest/gtest.h"
+
+static constexpr auto kDt = 10_ms;
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
+ if (val1 <= val2) { \
+ EXPECT_LE(val1, val2); \
+ } else { \
+ EXPECT_NEAR_UNITS(val1, val2, eps); \
+ }
+
+TEST(TrapezoidProfileTest, ReachesGoal) {
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
+
+ for (int i = 0; i < 450; ++i) {
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
+
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto lastPos = state.position;
+ for (int i = 0; i < 1600; ++i) {
+ if (i == 400) {
+ constraints.maxVelocity = 0.75_mps;
+ }
+
+ profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ auto estimatedVel = (state.position - lastPos) / kDt;
+
+ if (i >= 400) {
+ // Since estimatedVel can have floating point rounding errors, we check
+ // whether value is less than or within an error delta of the new
+ // constraint.
+ EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
+
+ EXPECT_LE(state.velocity, constraints.maxVelocity);
+ }
+
+ lastPos = state.position;
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// There is some somewhat tricky code for dealing with going backwards
+TEST(TrapezoidProfileTest, Backwards) {
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
+
+ for (int i = 0; i < 400; ++i) {
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
+
+ for (int i = 0; i < 200; ++i) {
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_NE(state, goal);
+
+ goal = {0.0_m, 0.0_mps};
+ for (int i = 0; i < 550; ++i) {
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed
+TEST(TrapezoidProfileTest, TopSpeed) {
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
+
+ for (int i = 0; i < 200; ++i) {
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
+
+ for (int i = 0; i < 2000; ++i) {
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, TimingToCurrent) {
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+ frc::TrapezoidProfile<units::meter>::State state;
+
+ for (int i = 0; i < 400; i++) {
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingToGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal && state == goal) {
+ // Expected value using for loop index is just an approximation since the
+ // time left in the profile doesn't increase linearly at the endpoints
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal &&
+ (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+ reachedGoal = true;
+ }
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal && state == goal) {
+ // Expected value using for loop index is just an approximation since the
+ // time left in the profile doesn't increase linearly at the endpoints
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+ 0.75_mps_sq};
+ frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+
+ frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal &&
+ (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+ reachedGoal = true;
+ }
+ }
+}
diff --git a/wpilibc/src/test/native/include/MockSpeedController.h b/wpilibc/src/test/native/include/MockSpeedController.h
new file mode 100644
index 0000000..e0c788f
--- /dev/null
+++ b/wpilibc/src/test/native/include/MockSpeedController.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/SpeedController.h"
+
+namespace frc {
+
+class MockSpeedController : public SpeedController {
+ public:
+ void Set(double speed) override;
+ double Get() const override;
+ void SetInverted(bool isInverted) override;
+ bool GetInverted() const override;
+ void Disable() override;
+ void StopMotor() override;
+
+ void PIDWrite(double output) override;
+
+ private:
+ double m_speed = 0.0;
+ bool m_isInverted = false;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h b/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
new file mode 100644
index 0000000..1cba8e3
--- /dev/null
+++ b/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/StringRef.h>
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+/**
+ * A mock sendable that marks itself as an actuator.
+ */
+class MockActuatorSendable : public Sendable,
+ public SendableHelper<MockActuatorSendable> {
+ public:
+ explicit MockActuatorSendable(wpi::StringRef name);
+
+ void InitSendable(SendableBuilder& builder) override;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
new file mode 100644
index 0000000..4a496d7
--- /dev/null
+++ b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+class TestTrajectory {
+ public:
+ static Trajectory GetTrajectory(TrajectoryConfig& config) {
+ // 2018 cross scale auto waypoints
+ const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
+ const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
+
+ config.SetReversed(true);
+
+ auto vector = std::vector<Translation2d>{
+ (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
+ .Translation(),
+ (sideStart +
+ Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
+ .Translation()};
+
+ return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,
+ crossScale, config);
+ }
+};
+
+} // namespace frc