Added code to handle wrapping values.
Change-Id: Ie6729956df08aec824a3cc9a04c5ff014a6c0909
diff --git a/frc971/zeroing/wrap.h b/frc971/zeroing/wrap.h
new file mode 100644
index 0000000..84b049d
--- /dev/null
+++ b/frc971/zeroing/wrap.h
@@ -0,0 +1,14 @@
+#ifndef FRC971_ZEROING_WRAP_H_
+#define FRC971_ZEROING_WRAP_H_
+
+namespace frc971 {
+namespace zeroing {
+
+// Returns a modified value which has been wrapped such that it is +- period/2
+// away from nearest.
+double Wrap(double nearest, double value, double period);
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_WRAP_H_