Reset the vision averagers and trackers on button release.
Change-Id: If07ea4fdc9ab26752e6d7664998a69ddc84643b1
diff --git a/aos/common/ring_buffer.h b/aos/common/ring_buffer.h
index 693ac95..2e44a5e 100644
--- a/aos/common/ring_buffer.h
+++ b/aos/common/ring_buffer.h
@@ -52,6 +52,9 @@
bool full() const { return size_ == buffer_size; }
+ // Clears all the data out of the buffer.
+ void Reset() { size_ = 0; }
+
private:
::std::array<Data, buffer_size> data_;
diff --git a/aos/common/ring_buffer_test.cc b/aos/common/ring_buffer_test.cc
index bfaecba..5aa7327 100644
--- a/aos/common/ring_buffer_test.cc
+++ b/aos/common/ring_buffer_test.cc
@@ -89,5 +89,34 @@
}
}
+// Test that the buffer works after Reset.
+TEST_F(RingBufferTest, ResetWorks) {
+ // Over fill it, and then clear it out.
+ ASSERT_TRUE(buffer_.empty());
+
+ for (size_t i = 0; i < 53; ++i) {
+ buffer_.Push(i);
+ }
+ ASSERT_TRUE(buffer_.full());
+
+ buffer_.Reset();
+
+ ASSERT_TRUE(buffer_.empty());
+
+ // Now, add numbers 0-9 to the RingBuffer.
+ for (int i = 0; i < 10; ++i) {
+ buffer_.Push(i);
+ }
+
+ // It should now be full.
+ ASSERT_TRUE(buffer_.full());
+
+ // The last 10 numbers were added 0-9, so verify that is what is in the
+ // buffer.
+ for (size_t i = 0; i < buffer_.size(); ++i) {
+ ASSERT_EQ(i, buffer_[i]);
+ }
+}
+
} // namespace testing
} // namespace aos
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index cbc76ab..9ee07b5 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -536,6 +536,8 @@
profiled_subsystem_.set_turret_unprofiled_goal(
vision_time_adjuster_.goal() + vision_error_);
}
+ } else {
+ vision_time_adjuster_.ResetTime();
}
if (freeze_) {
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 1faed36..95c771b 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -51,6 +51,10 @@
shooter_goal = unsafe_goal->shooter;
indexer_goal = unsafe_goal->indexer;
+ if (!unsafe_goal->use_vision_for_shots) {
+ distance_average_.Reset();
+ }
+
distance_average_.Tick(::aos::monotonic_clock::now(), vision_status);
status->vision_distance = distance_average_.Get();
if (distance_average_.Valid()) {
diff --git a/y2017/control_loops/superstructure/vision_distance_average.h b/y2017/control_loops/superstructure/vision_distance_average.h
index fbe2f00..7cd1ee1 100644
--- a/y2017/control_loops/superstructure/vision_distance_average.h
+++ b/y2017/control_loops/superstructure/vision_distance_average.h
@@ -36,6 +36,9 @@
// Valid gives a sense of how recent the data is.
bool Valid() { return data_.size() > 4; }
+ // Clears all the saved samples.
+ void Reset() { data_.Reset(); }
+
private:
double ComputeValue() {
double result = 0.0;
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.h b/y2017/control_loops/superstructure/vision_time_adjuster.h
index a7b79f3..46c7f89 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.h
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.h
@@ -43,6 +43,10 @@
double right;
};
+ void ResetTime() {
+ most_recent_vision_time_ = ::aos::monotonic_clock::min_time;
+ }
+
private:
// Buffer space to store the most recent drivetrain and turret messages from
// the last second.