Adds the sensor unwrap class and tests.
Added the Reset function call to the constructor. Cleaned up some comments.
Change-Id: Ib6bc3cacfe586890b7294f11a1a721a14fd0f989
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index dde7c63..ea44c7a 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -39,6 +39,7 @@
visibility = ["//visibility:public"],
deps = [
":intake_plants",
+ ":sensor_unwrap",
"//aos:math",
"//aos/controls:control_loop",
"//frc971/control_loops:control_loops_fbs",
@@ -49,3 +50,25 @@
"//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.cc b/y2018/control_loops/superstructure/intake/intake.cc
index dffebbc..edefca4 100644
--- a/y2018/control_loops/superstructure/intake/intake.cc
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -4,7 +4,6 @@
#include "aos/commonmath.h"
#include "aos/logging/logging.h"
-
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
@@ -91,8 +90,9 @@
IntakeSide::IntakeSide(
const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
- &zeroing_constants)
- : zeroing_estimator_(zeroing_constants) {}
+ &zeroing_constants,
+ const double spring_offset)
+ : zeroing_estimator_(zeroing_constants), spring_offset_(spring_offset) {}
void IntakeSide::Reset() { state_ = State::UNINITIALIZED; }
@@ -109,6 +109,7 @@
AOS_LOG(DEBUG, "Uninitialized, waiting for intake\n");
zeroing_estimator_.Reset();
controller_.Reset();
+ spring_unwrap_.Reset();
state_ = State::ZEROING;
break;
@@ -131,8 +132,10 @@
state_ = State::UNINITIALIZED;
}
// ESTOP if we hit the hard limits.
- if ((controller_.motor_position()) > controller_.intake_range().upper_hard ||
- (controller_.motor_position()) < controller_.intake_range().lower_hard) {
+ if ((controller_.motor_position()) >
+ controller_.intake_range().upper_hard ||
+ (controller_.motor_position()) <
+ controller_.intake_range().lower_hard) {
AOS_LOG(ERROR, "Hit hard limits\n");
state_ = State::ESTOP;
}
@@ -144,7 +147,7 @@
}
const bool disable = (output == nullptr) || state_ != State::RUNNING;
- controller_.set_position(position->spring_angle(),
+ controller_.set_position(spring_unwrap_.Unwrap(position->spring_angle()),
position->motor_position()->encoder());
controller_.Update(disable, unsafe_goal);
@@ -159,6 +162,8 @@
superstructure::IntakeSideStatus::Builder status_builder(*fbb);
// Save debug/internal state.
status_builder.add_estimator_state(estimator_state);
+ // Save the spring wrapped status.
+ status_builder.add_spring_wrapped(spring_unwrap_.sensor_wrapped());
controller_.SetStatus(&status_builder, unsafe_goal);
status_builder.add_calculated_velocity(
@@ -167,7 +172,7 @@
controller_.kDt);
status_builder.add_zeroed(zeroing_estimator_.zeroed());
status_builder.add_estopped(estopped());
- status_builder.add_state ( static_cast<int32_t>(state_));
+ status_builder.add_state(static_cast<int32_t>(state_));
intake_last_position_ =
zeroing_estimator_.offset() + position->motor_position()->encoder();
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 7f0ec9f..ac4f971 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -1,12 +1,15 @@
#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
+#include <math.h>
+
#include "aos/commonmath.h"
#include "aos/controls/control_loop.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"
@@ -76,7 +79,8 @@
class IntakeSide {
public:
IntakeSide(const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
- &zeroing_constants);
+ &zeroing_constants,
+ const double spring_offset);
// The operating voltage.
static constexpr double kOperatingVoltage() { return 12.0; }
@@ -99,6 +103,7 @@
State state() const { return state_; }
bool estopped() const { return state_ == State::ESTOP; }
+
bool zeroed() const { return zeroing_estimator_.zeroed(); }
bool clear_of_box() const { return controller_.output_position() < -0.1; }
@@ -108,10 +113,18 @@
private:
IntakeController controller_;
- State state_ = State::UNINITIALIZED;
-
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator zeroing_estimator_;
+ const double spring_offset_;
+
+ double spring_range() const {
+ return ::y2018::constants::Values::kIntakeSpringRatio() * (2 * M_PI);
+ }
+
+ UnwrapSensor spring_unwrap_{spring_offset_, spring_range()};
+
+ State state_ = State::UNINITIALIZED;
+
double intake_last_position_ = 0.0;
};
diff --git a/y2018/control_loops/superstructure/intake/sensor_unwrap.cc b/y2018/control_loops/superstructure/intake/sensor_unwrap.cc
new file mode 100644
index 0000000..c7d2a03
--- /dev/null
+++ b/y2018/control_loops/superstructure/intake/sensor_unwrap.cc
@@ -0,0 +1,64 @@
+#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
new file mode 100644
index 0000000..ccb1b52
--- /dev/null
+++ b/y2018/control_loops/superstructure/intake/sensor_unwrap.h
@@ -0,0 +1,48 @@
+#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
new file mode 100644
index 0000000..6211846
--- /dev/null
+++ b/y2018/control_loops/superstructure/intake/unwrap_test.cc
@@ -0,0 +1,71 @@
+#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
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 70af274..9f5b9fe 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -35,8 +35,10 @@
drivetrain_output_fetcher_(
event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
"/drivetrain")),
- intake_left_(constants::GetValues().left_intake.zeroing),
- intake_right_(constants::GetValues().right_intake.zeroing) {}
+ intake_left_(constants::GetValues().left_intake.zeroing,
+ constants::GetValues().left_intake.spring_offset),
+ intake_right_(constants::GetValues().right_intake.zeroing,
+ constants::GetValues().right_intake.spring_offset) {}
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
diff --git a/y2018/control_loops/superstructure/superstructure_status.fbs b/y2018/control_loops/superstructure/superstructure_status.fbs
index af2d1ab..8baf40e 100644
--- a/y2018/control_loops/superstructure/superstructure_status.fbs
+++ b/y2018/control_loops/superstructure/superstructure_status.fbs
@@ -16,6 +16,8 @@
spring_position:float;
// Estimated velocity of the spring in units/second.
spring_velocity:float;
+ // Reported wrapping of the spring
+ spring_wrapped:int;
// Estimated position of the joint.
motor_position:float;