Added code to handle wrapping values.
Change-Id: Ie6729956df08aec824a3cc9a04c5ff014a6c0909
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index c63d97d..0b8799f 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -56,3 +56,24 @@
'//frc971/control_loops:queues',
],
)
+
+cc_library(
+ name = 'wrap',
+ srcs = [
+ 'wrap.cc',
+ ],
+ hdrs = [
+ 'wrap.h',
+ ],
+)
+
+cc_test(
+ name = 'wrap_test',
+ srcs = [
+ 'wrap_test.cc',
+ ],
+ deps = [
+ '//aos/testing:googletest',
+ ':wrap',
+ ],
+)
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
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_
diff --git a/frc971/zeroing/wrap_test.cc b/frc971/zeroing/wrap_test.cc
new file mode 100644
index 0000000..e195385
--- /dev/null
+++ b/frc971/zeroing/wrap_test.cc
@@ -0,0 +1,44 @@
+#include <random>
+
+#include "frc971/zeroing/wrap.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+// Tests some various positive and negative values for wrap.
+TEST(WrapTest, TestWrap) {
+ EXPECT_NEAR(1.0, Wrap(0.0, 1.0, 10.0), 1e-6);
+ EXPECT_NEAR(-1.0, Wrap(0.0, -1.0, 10.0), 1e-6);
+
+ EXPECT_NEAR(1.0, Wrap(5.0, 1.0, 10.0), 1e-6);
+ EXPECT_NEAR(9.0, Wrap(5.0, -1.0, 10.0), 1e-6);
+
+ EXPECT_NEAR(10.0, Wrap(5.0, 10.0, 10.0), 1e-6);
+ EXPECT_NEAR(1.0, Wrap(5.0, -9.0, 10.0), 1e-6);
+}
+
+// Try a bunch of combinations and verify that the result has the right
+// properties. We can be really inefficient here since it is a test.
+TEST(WrapTest, ExhaustiveWrap) {
+ for (double i = -20; i < 20; ++i) {
+ for (double j = -20; j < 20; ++j) {
+ EXPECT_NEAR(i, Wrap(i, j, 10.0), 5.0);
+ const double wrapped_val = Wrap(i, j, 10.0);
+ bool found_interval = false;
+ for (int k = -5; k < 5; ++k) {
+ if (::std::abs(k * 10 + wrapped_val - j) < 1e-6) {
+ found_interval = true;
+ break;
+ }
+ }
+ EXPECT_TRUE(found_interval) << ": Wrap(" << i << ", " << j
+ << ") = " << wrapped_val;
+ }
+ }
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971