Squashed 'third_party/allwpilib_2019/' content from commit bd05dfa1c
Change-Id: I2b1c2250cdb9b055133780c33593292098c375b7
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: bd05dfa1c7cca74c4fac451e7b9d6a37e7b53447
diff --git a/wpilibc/src/test/native/cpp/CircularBufferTest.cpp b/wpilibc/src/test/native/cpp/CircularBufferTest.cpp
new file mode 100644
index 0000000..ada4f9a
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/CircularBufferTest.cpp
@@ -0,0 +1,211 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-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/circular_buffer.h" // NOLINT(build/include_order)
+
+#include <array>
+
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const std::array<double, 10> values = {
+ {751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727,
+ 421.125, 799.913}};
+
+static const std::array<double, 8> pushFrontOut = {
+ {799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657}};
+
+static const std::array<double, 8> pushBackOut = {
+ {342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}};
+
+TEST(CircularBufferTest, PushFrontTest) {
+ circular_buffer<double> queue(8);
+
+ for (auto& value : values) {
+ queue.push_front(value);
+ }
+
+ for (size_t i = 0; i < pushFrontOut.size(); i++) {
+ EXPECT_EQ(pushFrontOut[i], queue[i]);
+ }
+}
+
+TEST(CircularBufferTest, PushBackTest) {
+ circular_buffer<double> queue(8);
+
+ for (auto& value : values) {
+ queue.push_back(value);
+ }
+
+ for (size_t i = 0; i < pushBackOut.size(); i++) {
+ EXPECT_EQ(pushBackOut[i], queue[i]);
+ }
+}
+
+TEST(CircularBufferTest, PushPopTest) {
+ circular_buffer<double> queue(3);
+
+ // Insert three elements into the buffer
+ queue.push_back(1.0);
+ queue.push_back(2.0);
+ queue.push_back(3.0);
+
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+ EXPECT_EQ(3.0, queue[2]);
+
+ /*
+ * The buffer is full now, so pushing subsequent elements will overwrite the
+ * front-most elements.
+ */
+
+ queue.push_back(4.0); // Overwrite 1 with 4
+
+ // The buffer now contains 2, 3 and 4
+ EXPECT_EQ(2.0, queue[0]);
+ EXPECT_EQ(3.0, queue[1]);
+ EXPECT_EQ(4.0, queue[2]);
+
+ queue.push_back(5.0); // Overwrite 2 with 5
+
+ // The buffer now contains 3, 4 and 5
+ EXPECT_EQ(3.0, queue[0]);
+ EXPECT_EQ(4.0, queue[1]);
+ EXPECT_EQ(5.0, queue[2]);
+
+ EXPECT_EQ(5.0, queue.pop_back()); // 5 is removed
+
+ // The buffer now contains 3 and 4
+ EXPECT_EQ(3.0, queue[0]);
+ EXPECT_EQ(4.0, queue[1]);
+
+ EXPECT_EQ(3.0, queue.pop_front()); // 3 is removed
+
+ // Leaving only one element with value == 4
+ EXPECT_EQ(4.0, queue[0]);
+}
+
+TEST(CircularBufferTest, ResetTest) {
+ circular_buffer<double> queue(5);
+
+ for (size_t i = 1; i < 6; i++) {
+ queue.push_back(i);
+ }
+
+ queue.reset();
+
+ for (size_t i = 0; i < 5; i++) {
+ EXPECT_EQ(0.0, queue[i]);
+ }
+}
+
+TEST(CircularBufferTest, ResizeTest) {
+ circular_buffer<double> queue(5);
+
+ /* Buffer contains {1, 2, 3, _, _}
+ * ^ front
+ */
+ queue.push_back(1.0);
+ queue.push_back(2.0);
+ queue.push_back(3.0);
+
+ queue.resize(2);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.resize(5);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.reset();
+
+ /* Buffer contains {_, 1, 2, 3, _}
+ * ^ front
+ */
+ queue.push_back(0.0);
+ queue.push_back(1.0);
+ queue.push_back(2.0);
+ queue.push_back(3.0);
+ queue.pop_front();
+
+ queue.resize(2);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.resize(5);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.reset();
+
+ /* Buffer contains {_, _, 1, 2, 3}
+ * ^ front
+ */
+ queue.push_back(0.0);
+ queue.push_back(0.0);
+ queue.push_back(1.0);
+ queue.push_back(2.0);
+ queue.push_back(3.0);
+ queue.pop_front();
+ queue.pop_front();
+
+ queue.resize(2);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.resize(5);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.reset();
+
+ /* Buffer contains {3, _, _, 1, 2}
+ * ^ front
+ */
+ queue.push_back(3.0);
+ queue.push_front(2.0);
+ queue.push_front(1.0);
+
+ queue.resize(2);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.resize(5);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.reset();
+
+ /* Buffer contains {2, 3, _, _, 1}
+ * ^ front
+ */
+ queue.push_back(2.0);
+ queue.push_back(3.0);
+ queue.push_front(1.0);
+
+ queue.resize(2);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ queue.resize(5);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+
+ // Test push_back() after resize
+ queue.push_back(3.0);
+ EXPECT_EQ(1.0, queue[0]);
+ EXPECT_EQ(2.0, queue[1]);
+ EXPECT_EQ(3.0, queue[2]);
+
+ // Test push_front() after resize
+ queue.push_front(4.0);
+ EXPECT_EQ(4.0, queue[0]);
+ EXPECT_EQ(1.0, queue[1]);
+ EXPECT_EQ(2.0, queue[2]);
+ EXPECT_EQ(3.0, queue[3]);
+}
diff --git a/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp
new file mode 100644
index 0000000..ea14f32
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-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/filters/LinearDigitalFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+#include <thread>
+
+#include "frc/Base.h"
+#include "gtest/gtest.h"
+
+/* Filter constants */
+static constexpr double kFilterStep = 0.005;
+static constexpr double kFilterTime = 2.0;
+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;
+static constexpr double kPi = 3.14159265358979323846;
+
+using namespace frc;
+
+enum FilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
+
+std::ostream& operator<<(std::ostream& os, const FilterNoiseTestType& type) {
+ switch (type) {
+ case TEST_SINGLE_POLE_IIR:
+ os << "LinearDigitalFilter SinglePoleIIR";
+ break;
+ case TEST_MOVAVG:
+ os << "LinearDigitalFilter MovingAverage";
+ break;
+ }
+
+ return os;
+}
+
+constexpr double kStdDev = 10.0;
+
+/**
+ * Adds Gaussian white noise to a function returning data. The noise will have
+ * the standard deviation provided in the constructor.
+ */
+class NoiseGenerator : public PIDSource {
+ public:
+ NoiseGenerator(double (*dataFunc)(double), double stdDev)
+ : m_distr(0.0, stdDev) {
+ m_dataFunc = dataFunc;
+ }
+
+ void SetPIDSourceType(PIDSourceType pidSource) override {}
+
+ double Get() { return m_dataFunc(m_count) + m_noise; }
+
+ double PIDGet() override {
+ m_noise = m_distr(m_gen);
+ m_count += kFilterStep;
+ return m_dataFunc(m_count) + m_noise;
+ }
+
+ void Reset() { m_count = -kFilterStep; }
+
+ private:
+ std::function<double(double)> m_dataFunc;
+ double m_noise = 0.0;
+
+ // Make sure first call to PIDGet() uses m_count == 0
+ double m_count = -kFilterStep;
+
+ std::random_device m_rd;
+ std::mt19937 m_gen{m_rd()};
+ std::normal_distribution<double> m_distr;
+};
+
+/**
+ * A fixture that includes a noise generator wrapped in a filter
+ */
+class FilterNoiseTest : public testing::TestWithParam<FilterNoiseTestType> {
+ protected:
+ std::unique_ptr<PIDSource> m_filter;
+ std::shared_ptr<NoiseGenerator> m_noise;
+
+ static double GetData(double t) { return 100.0 * std::sin(2.0 * kPi * t); }
+
+ void SetUp() override {
+ m_noise = std::make_shared<NoiseGenerator>(GetData, kStdDev);
+
+ switch (GetParam()) {
+ case TEST_SINGLE_POLE_IIR: {
+ m_filter = std::make_unique<LinearDigitalFilter>(
+ LinearDigitalFilter::SinglePoleIIR(
+ m_noise, kSinglePoleIIRTimeConstant, kFilterStep));
+ break;
+ }
+
+ case TEST_MOVAVG: {
+ m_filter = std::make_unique<LinearDigitalFilter>(
+ LinearDigitalFilter::MovingAverage(m_noise, kMovAvgTaps));
+ break;
+ }
+ }
+ }
+};
+
+/**
+ * Test if the filter reduces the noise produced by a signal generator
+ */
+TEST_P(FilterNoiseTest, NoiseReduce) {
+ double theoryData = 0.0;
+ double noiseGenError = 0.0;
+ double filterError = 0.0;
+
+ m_noise->Reset();
+ for (double t = 0; t < kFilterTime; t += kFilterStep) {
+ theoryData = GetData(t);
+ filterError += std::abs(m_filter->PIDGet() - theoryData);
+ noiseGenError += std::abs(m_noise->Get() - theoryData);
+ }
+
+ 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_CASE_P(Test, FilterNoiseTest,
+ testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG), );
diff --git a/wpilibc/src/test/native/cpp/FilterOutputTest.cpp b/wpilibc/src/test/native/cpp/FilterOutputTest.cpp
new file mode 100644
index 0000000..ec71760
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/FilterOutputTest.cpp
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-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/filters/LinearDigitalFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+#include <thread>
+
+#include "frc/Base.h"
+#include "gtest/gtest.h"
+
+/* Filter constants */
+static constexpr double kFilterStep = 0.005;
+static constexpr double kFilterTime = 2.0;
+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;
+static constexpr double kPi = 3.14159265358979323846;
+
+using namespace frc;
+
+enum FilterOutputTestType {
+ TEST_SINGLE_POLE_IIR,
+ TEST_HIGH_PASS,
+ TEST_MOVAVG,
+ TEST_PULSE
+};
+
+std::ostream& operator<<(std::ostream& os, const FilterOutputTestType& type) {
+ switch (type) {
+ case TEST_SINGLE_POLE_IIR:
+ os << "LinearDigitalFilter SinglePoleIIR";
+ break;
+ case TEST_HIGH_PASS:
+ os << "LinearDigitalFilter HighPass";
+ break;
+ case TEST_MOVAVG:
+ os << "LinearDigitalFilter MovingAverage";
+ break;
+ case TEST_PULSE:
+ os << "LinearDigitalFilter Pulse";
+ break;
+ }
+
+ return os;
+}
+
+class DataWrapper : public PIDSource {
+ public:
+ explicit DataWrapper(double (*dataFunc)(double)) { m_dataFunc = dataFunc; }
+
+ virtual void SetPIDSourceType(PIDSourceType pidSource) {}
+
+ virtual double PIDGet() {
+ m_count += kFilterStep;
+ return m_dataFunc(m_count);
+ }
+
+ void Reset() { m_count = -kFilterStep; }
+
+ private:
+ std::function<double(double)> m_dataFunc;
+
+ // Make sure first call to PIDGet() uses m_count == 0
+ double m_count = -kFilterStep;
+};
+
+/**
+ * A fixture that includes a consistent data source wrapped in a filter
+ */
+class FilterOutputTest : public testing::TestWithParam<FilterOutputTestType> {
+ protected:
+ std::unique_ptr<PIDSource> m_filter;
+ std::shared_ptr<DataWrapper> m_data;
+ double m_expectedOutput = 0.0;
+
+ static double GetData(double t) {
+ return 100.0 * std::sin(2.0 * kPi * t) + 20.0 * std::cos(50.0 * kPi * t);
+ }
+
+ static double GetPulseData(double t) {
+ if (std::abs(t - 1.0) < 0.001) {
+ return 1.0;
+ } else {
+ return 0.0;
+ }
+ }
+
+ void SetUp() override {
+ switch (GetParam()) {
+ case TEST_SINGLE_POLE_IIR: {
+ m_data = std::make_shared<DataWrapper>(GetData);
+ m_filter = std::make_unique<LinearDigitalFilter>(
+ LinearDigitalFilter::SinglePoleIIR(
+ m_data, kSinglePoleIIRTimeConstant, kFilterStep));
+ m_expectedOutput = kSinglePoleIIRExpectedOutput;
+ break;
+ }
+
+ case TEST_HIGH_PASS: {
+ m_data = std::make_shared<DataWrapper>(GetData);
+ m_filter =
+ std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::HighPass(
+ m_data, kHighPassTimeConstant, kFilterStep));
+ m_expectedOutput = kHighPassExpectedOutput;
+ break;
+ }
+
+ case TEST_MOVAVG: {
+ m_data = std::make_shared<DataWrapper>(GetData);
+ m_filter = std::make_unique<LinearDigitalFilter>(
+ LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps));
+ m_expectedOutput = kMovAvgExpectedOutput;
+ break;
+ }
+
+ case TEST_PULSE: {
+ m_data = std::make_shared<DataWrapper>(GetPulseData);
+ m_filter = std::make_unique<LinearDigitalFilter>(
+ LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps));
+ m_expectedOutput = 0.0;
+ break;
+ }
+ }
+ }
+};
+
+/**
+ * Test if the linear digital filters produce consistent output
+ */
+TEST_P(FilterOutputTest, FilterOutput) {
+ m_data->Reset();
+
+ double filterOutput = 0.0;
+ for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
+ filterOutput = m_filter->PIDGet();
+ }
+
+ RecordProperty("FilterOutput", filterOutput);
+
+ EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+ << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_CASE_P(Test, FilterOutputTest,
+ testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
+ TEST_MOVAVG, TEST_PULSE), );
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/RobotDriveTest.cpp b/wpilibc/src/test/native/cpp/RobotDriveTest.cpp
new file mode 100644
index 0000000..464a707
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/RobotDriveTest.cpp
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-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"
+#include "frc/RobotDrive.h"
+#include "frc/drive/DifferentialDrive.h"
+#include "frc/drive/MecanumDrive.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class RobotDriveTest : 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(RobotDriveTest, 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(RobotDriveTest, 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(RobotDriveTest, 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(RobotDriveTest, 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(RobotDriveTest, 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(RobotDriveTest, 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/SpeedControllerGroupTest.cpp b/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
new file mode 100644
index 0000000..a6f5028
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "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_CASE_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..141c72e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Watchdog.h" // NOLINT(build/include_order)
+
+#include <stdint.h>
+
+#include <thread>
+
+#include <wpi/raw_ostream.h>
+
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(WatchdogTest, EnableDisable) {
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(0.4, [&] { 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";
+}
+
+TEST(WatchdogTest, Reset) {
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(0.4, [&] { 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";
+}
+
+TEST(WatchdogTest, SetTimeout) {
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(1.0, [&] { watchdogCounter++; });
+
+ watchdog.Enable();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ watchdog.SetTimeout(0.2);
+
+ 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";
+}
+
+TEST(WatchdogTest, IsExpired) {
+ Watchdog watchdog(0.2, [] {});
+ watchdog.Enable();
+
+ EXPECT_FALSE(watchdog.IsExpired());
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+ EXPECT_TRUE(watchdog.IsExpired());
+}
+
+TEST(WatchdogTest, Epochs) {
+ uint32_t watchdogCounter = 0;
+
+ Watchdog watchdog(0.4, [&] { 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";
+}
+
+TEST(WatchdogTest, MultiWatchdog) {
+ uint32_t watchdogCounter1 = 0;
+ uint32_t watchdogCounter2 = 0;
+
+ Watchdog watchdog1(0.2, [&] { watchdogCounter1++; });
+ Watchdog watchdog2(0.6, [&] { 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/main.cpp b/wpilibc/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..0e00efa
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/main.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-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 <hal/HAL.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..3c9e411
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "shuffleboard/MockActuatorSendable.h"
+
+using namespace frc;
+
+MockActuatorSendable::MockActuatorSendable(wpi::StringRef name)
+ : SendableBase(false) {
+ SetName(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/ShuffleboardTabTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
new file mode 100644
index 0000000..23f3e3a
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <array>
+#include <memory>
+#include <string>
+
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "frc/commands/InstantCommand.h"
+#include "frc/shuffleboard/ShuffleboardInstance.h"
+#include "frc/shuffleboard/ShuffleboardTab.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class ShuffleboardTabTest : 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(ShuffleboardTabTest, AddDouble) {
+ auto entry = m_tab->Add("Double", 1.0).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/Double", entry.GetName());
+ EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
+}
+
+TEST_F(ShuffleboardTabTest, AddInteger) {
+ auto entry = m_tab->Add("Int", 1).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/Int", entry.GetName());
+ EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
+}
+
+TEST_F(ShuffleboardTabTest, AddBoolean) {
+ auto entry = m_tab->Add("Bool", false).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/Bool", entry.GetName());
+ EXPECT_FALSE(entry.GetValue()->GetBoolean());
+}
+
+TEST_F(ShuffleboardTabTest, AddString) {
+ auto entry = m_tab->Add("String", "foobar").GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/String", entry.GetName());
+ EXPECT_EQ("foobar", entry.GetValue()->GetString());
+}
+
+TEST_F(ShuffleboardTabTest, AddNamedSendableWithProperties) {
+ InstantCommand sendable("Command");
+ std::string widgetType = "Command Widget";
+ wpi::StringMap<std::shared_ptr<nt::Value>> map;
+ map.try_emplace("foo", nt::Value::MakeDouble(1234));
+ map.try_emplace("bar", nt::Value::MakeString("baz"));
+ m_tab->Add(sendable).WithWidget(widgetType).WithProperties(map);
+
+ m_instance->Update();
+ std::string meta = "/Shuffleboard/.metadata/Tab/Command";
+
+ EXPECT_EQ(1234, m_ntInstance.GetEntry(meta + "/Properties/foo").GetDouble(-1))
+ << "Property 'foo' not set correctly";
+ EXPECT_EQ("baz",
+ m_ntInstance.GetEntry(meta + "/Properties/bar").GetString(""))
+ << "Property 'bar' not set correctly";
+ EXPECT_EQ(widgetType,
+ m_ntInstance.GetEntry(meta + "/PreferredComponent").GetString(""))
+ << "Preferred component not set correctly";
+}
+
+TEST_F(ShuffleboardTabTest, AddNumberArray) {
+ std::array<double, 3> expect = {{1.0, 2.0, 3.0}};
+ auto entry = m_tab->Add("DoubleArray", expect).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/DoubleArray", entry.GetName());
+
+ auto actual = entry.GetValue()->GetDoubleArray();
+ EXPECT_EQ(expect.size(), actual.size());
+ for (size_t i = 0; i < expect.size(); i++) {
+ EXPECT_FLOAT_EQ(expect[i], actual[i]);
+ }
+}
+
+TEST_F(ShuffleboardTabTest, AddBooleanArray) {
+ std::array<bool, 2> expect = {{true, false}};
+ auto entry = m_tab->Add("BoolArray", expect).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/BoolArray", entry.GetName());
+
+ auto actual = entry.GetValue()->GetBooleanArray();
+ EXPECT_EQ(expect.size(), actual.size());
+ for (size_t i = 0; i < expect.size(); i++) {
+ EXPECT_EQ(expect[i], actual[i]);
+ }
+}
+
+TEST_F(ShuffleboardTabTest, AddStringArray) {
+ std::array<std::string, 2> expect = {{"foo", "bar"}};
+ auto entry = m_tab->Add("StringArray", expect).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/StringArray", entry.GetName());
+
+ auto actual = entry.GetValue()->GetStringArray();
+ EXPECT_EQ(expect.size(), actual.size());
+ for (size_t i = 0; i < expect.size(); i++) {
+ EXPECT_EQ(expect[i], actual[i]);
+ }
+}
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";
+}