Created RungeKutta integrator and tests
Change-Id: I6f8587d11ad4872821456e2d1eabfe576bcaf971
diff --git a/frc971/control_loops/runge_kutta_test.cc b/frc971/control_loops/runge_kutta_test.cc
new file mode 100644
index 0000000..680715a
--- /dev/null
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -0,0 +1,26 @@
+#include "frc971/control_loops/runge_kutta.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Tests that integrating dx/dt = e^x works.
+TEST(RungeKuttaTest, Exponential) {
+ ::Eigen::Matrix<double, 1, 1> y0;
+ y0(0, 0) = 0.0;
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKutta(
+ [](::Eigen::Matrix<double, 1, 1> x) {
+ ::Eigen::Matrix<double, 1, 1> y;
+ y(0, 0) = ::std::exp(x(0, 0));
+ return y;
+ },
+ y0, 0.1);
+ EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971