Added code to handle wrapping values.
Change-Id: Ie6729956df08aec824a3cc9a04c5ff014a6c0909
diff --git a/frc971/zeroing/wrap.cc b/frc971/zeroing/wrap.cc
new file mode 100644
index 0000000..2698928
--- /dev/null
+++ b/frc971/zeroing/wrap.cc
@@ -0,0 +1,13 @@
+#include "frc971/zeroing/wrap.h"
+
+#include <cmath>
+
+namespace frc971 {
+namespace zeroing {
+
+double Wrap(double nearest, double value, double period) {
+ return ::std::remainder(value - nearest, period) + nearest;
+}
+
+} // namespace zeroing
+} // namespace frc971