Refactored unwrap class from y2018 code to frc971 directory.
Change-Id: I375e1a2cf875e89944d514f333295867d1a3437b
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index ea44c7a..dde7c63 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -39,7 +39,6 @@
visibility = ["//visibility:public"],
deps = [
":intake_plants",
- ":sensor_unwrap",
"//aos:math",
"//aos/controls:control_loop",
"//frc971/control_loops:control_loops_fbs",
@@ -50,25 +49,3 @@
"//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
-
-cc_library(
- name = "sensor_unwrap",
- srcs = [
- "sensor_unwrap.cc",
- ],
- hdrs = [
- "sensor_unwrap.h",
- ],
- visibility = ["//visibility:public"],
-)
-
-cc_test(
- name = "unwrap_test",
- srcs = [
- "unwrap_test.cc",
- ],
- deps = [
- ":sensor_unwrap",
- "//aos/testing:googletest",
- ],
-)
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index ac4f971..e064c0e 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -5,11 +5,11 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop.h"
+#include "frc971/zeroing/wrap.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
-#include "y2018/control_loops/superstructure/intake/sensor_unwrap.h"
#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
@@ -121,7 +121,8 @@
return ::y2018::constants::Values::kIntakeSpringRatio() * (2 * M_PI);
}
- UnwrapSensor spring_unwrap_{spring_offset_, spring_range()};
+ ::frc971::zeroing::UnwrapSensor spring_unwrap_{spring_offset_,
+ spring_range()};
State state_ = State::UNINITIALIZED;
diff --git a/y2018/control_loops/superstructure/intake/sensor_unwrap.cc b/y2018/control_loops/superstructure/intake/sensor_unwrap.cc
deleted file mode 100644
index c7d2a03..0000000
--- a/y2018/control_loops/superstructure/intake/sensor_unwrap.cc
+++ /dev/null
@@ -1,64 +0,0 @@
-#include "y2018/control_loops/superstructure/intake/sensor_unwrap.h"
-
-#include <cmath>
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace intake {
-
-UnwrapSensor::UnwrapSensor(double sensor_offset, double sensor_range)
- : sensor_offset_(sensor_offset), sensor_range_(sensor_range) {
- Reset();
-}
-
-double UnwrapSensor::Unwrap(double current_sensor_value) {
- // First time the function is called it will use that value to initialize the
- // wrap calculation. This catches cases where the offset and first value
- // difference triggers an unwanted wrap at the first calculation.
- if (uninitialized_ == true) {
- uninitialized_ = false;
- } else {
- // Calculates the lower sensor value and set the max sensor range
- // If offset is not 0, this will correct the zeroing offset
- const double sensor_min_value = sensor_offset_;
- const double sensor_max_value = sensor_range_ + sensor_min_value;
-
- // Check if provided sensor value is within the range. This to prevent the
- // function to get out of sync. Will not throw an error, but continue and
- // return the value + wrapped factor and not process this value.
- if (current_sensor_value < sensor_min_value ||
- current_sensor_value > sensor_max_value) {
- return current_sensor_value + (sensor_range_ * wrap_count_);
- }
-
- // Calculate the positive or negative movement
- const double sensor_move = current_sensor_value - sensor_last_value_;
-
- // Function assumes that a movement of more then 1/2 of the range
- // indicates that we wrapped, instead of moved very fast.
- if (std::abs(sensor_move) > (sensor_range_ / 2)) {
- if (sensor_move >= 0) {
- // sensor moved past the sensor_min_value
- wrap_count_ -= 1;
- } else {
- // sensor moved past the sensor_max_value
- wrap_count_ += 1;
- }
- }
- }
- sensor_last_value_ = current_sensor_value;
- // return the unwrapped sensor value
- return current_sensor_value + (sensor_range_ * wrap_count_);
-}
-
-void UnwrapSensor::Reset() {
- wrap_count_ = 0;
- sensor_last_value_ = sensor_offset_;
- uninitialized_ = true;
-}
-
-} // namespace intake
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/intake/sensor_unwrap.h b/y2018/control_loops/superstructure/intake/sensor_unwrap.h
deleted file mode 100644
index ccb1b52..0000000
--- a/y2018/control_loops/superstructure/intake/sensor_unwrap.h
+++ /dev/null
@@ -1,48 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_SENSOR_UNWRAP_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_SENSOR_UNWRAP_H_
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace intake {
-
-// UnwrapSensor takes in a sensor value from a sensor that loops in a certain
-// interval. ex(the sensor moves from 0 to 10 and back to 0 while moving the
-// same direction) By checking for big gaps in sensor readings it assumes you
-// have wrapped either back or forwards and handles accordingly. It returns the
-// overall sensor value.
-
-class UnwrapSensor {
- public:
- // The sensor_offset (+ or -) present the sensor value that is 'zero'
- // The sensor_range presents the absolute value of the sensor range from 0 to
- // sensor_range. This will be adjusted using the sensor_offset
- UnwrapSensor(double sensor_offset, double sensor_range);
-
- // Takes a wrapped sensor value and unwraps it to give you its total position.
- double Unwrap(double current_sensor_value);
-
- void Reset();
-
- int sensor_wrapped() const { return wrap_count_; }
-
- private:
- const double sensor_offset_, sensor_range_;
-
- // The last value given from set_position, starts at offset
- double sensor_last_value_ = sensor_offset_;
-
- // Log if sensor is in wrapped state in either direction
- int wrap_count_ = 0;
-
- // function waits for first call with a value to set sensor_last_value_. Will
- // start to calculate the spring unwrap at the second function call.
- bool uninitialized_ = true;
-};
-
-} // namespace intake
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_SENSOR_UNWRAP_H_
diff --git a/y2018/control_loops/superstructure/intake/unwrap_test.cc b/y2018/control_loops/superstructure/intake/unwrap_test.cc
deleted file mode 100644
index 6211846..0000000
--- a/y2018/control_loops/superstructure/intake/unwrap_test.cc
+++ /dev/null
@@ -1,71 +0,0 @@
-#include "gtest/gtest.h"
-#include "y2018/control_loops/superstructure/intake/sensor_unwrap.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace intake {
-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 intake
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018