Implemented 5th order gaussian quadrature.
Change-Id: Id7023b4f175c60b6930b5cb8aa829636604b2d25
diff --git a/frc971/control_loops/fixed_quadrature_test.cc b/frc971/control_loops/fixed_quadrature_test.cc
new file mode 100644
index 0000000..f842519
--- /dev/null
+++ b/frc971/control_loops/fixed_quadrature_test.cc
@@ -0,0 +1,21 @@
+#include "frc971/control_loops/fixed_quadrature.h"
+
+#include <cmath>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Tests that integrating y = cos(x) works.
+TEST(GaussianQuadratureTest, Cos) {
+ double y1 =
+ GaussianQuadrature5([](double x) { return ::std::cos(x); }, 0.0, 0.5);
+
+ EXPECT_NEAR(y1, ::std::sin(0.5), 1e-15);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971