Refactored unwrap class from y2018 code to frc971 directory.
Change-Id: I375e1a2cf875e89944d514f333295867d1a3437b
diff --git a/frc971/zeroing/unwrap_test.cc b/frc971/zeroing/unwrap_test.cc
new file mode 100644
index 0000000..d1e6db9
--- /dev/null
+++ b/frc971/zeroing/unwrap_test.cc
@@ -0,0 +1,67 @@
+#include "frc971/zeroing/wrap.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+TEST(SensorTest, UnwrapOnce) {
+ // Test the sensor moving over the maximum range value and wrapping once
+ // then move sensor in oppsite direction to unwrap and test result.
+
+ // Initialize with the offset and range
+ UnwrapSensor sensor(-1.5, 6); // min = -1.5 & max = 4.5 & move > 3
+ EXPECT_EQ(sensor.Unwrap(1.5), 1.5); // move n/a
+ EXPECT_EQ(sensor.sensor_wrapped(), 0);
+ EXPECT_EQ(sensor.Unwrap(4.0), 4.0); // move 2.5
+ EXPECT_EQ(sensor.Unwrap(-1.0), 5.0); // move -5.0 -> wrap+
+ EXPECT_EQ(sensor.sensor_wrapped(), 1);
+ EXPECT_EQ(sensor.Unwrap(-1.5), 4.5); // move -0.5
+ EXPECT_EQ(sensor.Unwrap(4.0), 4.0); // move 5.5 -> wrap-
+ EXPECT_EQ(sensor.sensor_wrapped(), 0);
+ sensor.Reset();
+}
+
+TEST(SensorTest, UnwrapTwice) {
+ // Test the sensor wrapping twice over the lower value of the range.
+
+ // Initialize with the offset and range
+ UnwrapSensor sensor(-1.5, 6);
+ EXPECT_EQ(sensor.Unwrap(1.0), 1.0);
+ EXPECT_EQ(sensor.Unwrap(-1.0), -1.0);
+ EXPECT_EQ(sensor.Unwrap(4.0), -2.0);
+ EXPECT_EQ(sensor.sensor_wrapped(), -1);
+ EXPECT_EQ(sensor.Unwrap(2.0), -4.0);
+ EXPECT_EQ(sensor.Unwrap(-1.0), -7.0);
+ EXPECT_EQ(sensor.Unwrap(4.0), -8.0);
+ EXPECT_EQ(sensor.sensor_wrapped(), -2);
+}
+
+TEST(SensorTest, UnwrapOutRange) {
+ // Test if values out side range are handled proporly.
+ // Not wrapped scenario only.
+
+ UnwrapSensor sensor(-1.5, 6);
+ EXPECT_EQ(sensor.Unwrap(-3.0), -3.0); // Passed by the init stage
+ EXPECT_EQ(sensor.Unwrap(-3.0), -3.0); // Caught by the exeption handler
+ EXPECT_EQ(sensor.Unwrap(6.5), 6.5);
+}
+
+TEST(SensorTest, UnwrapInit) {
+ // Test the case where the start value and offset will be far enough apart to
+ // trigger a wrap. By ignoring the fisrt value for evaluation and set that for
+ // the next evaluation, this should not trigger the wrap.
+
+ UnwrapSensor sensor(-0.6, 1.0); // min = -0.6 & max = 0.4 & move > 0.5
+ EXPECT_EQ(sensor.Unwrap(0.0), 0.0); // move = n/a
+ EXPECT_EQ(sensor.sensor_wrapped(), 0);
+ EXPECT_EQ(sensor.Unwrap(0.0), 0.0);
+ sensor.Reset();
+ EXPECT_EQ(sensor.Unwrap(0.4), 0.4); // move = n/a
+ EXPECT_EQ(sensor.Unwrap(-0.4), 0.6); // move = -0.8, wrap 1
+ EXPECT_EQ(sensor.Unwrap(0.2), 0.2); // move = 1.0, wrap -1
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971