Override Hood and Shooter goals with vision distance
Change-Id: I9741880fe7f96cf4208ca2cf75d584d683a30cea
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 797791f..307b8c5 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -23,6 +23,7 @@
'superstructure.h',
],
deps = [
+ ':vision_distance_average',
':superstructure_queue',
'//aos/common/controls:control_loop',
'//y2017/control_loops/superstructure/column',
@@ -96,3 +97,27 @@
'//aos/testing:test_shm',
],
)
+
+cc_library(
+ name = 'vision_distance_average',
+ hdrs = [
+ 'vision_distance_average.h',
+ ],
+ deps = [
+ '//aos/common:time',
+ '//aos/common:ring_buffer',
+ '//y2017/vision:vision_queue',
+ ],
+)
+
+cc_test(
+ name = 'vision_distance_average_test',
+ srcs = [
+ 'vision_distance_average_test.cc',
+ ],
+ deps = [
+ ':vision_distance_average',
+ '//aos/common:time',
+ '//aos/testing:googletest',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 63ab691..2de0ab8 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -42,11 +42,33 @@
vision_status = vision::vision_status.get();
}
- hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
- &(position->hood),
- output != nullptr ? &(output->voltage_hood) : nullptr,
- &(status->hood));
- shooter_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->shooter) : nullptr,
+ // Create a copy of the goals so that we can modify them.
+ HoodGoal hood_goal;
+ ShooterGoal shooter_goal;
+ if (unsafe_goal != nullptr) {
+ hood_goal = unsafe_goal->hood;
+ shooter_goal = unsafe_goal->shooter;
+
+ distance_average_.Tick(::aos::monotonic_clock::now(), vision_status);
+ status->vision_distance = distance_average_.Get();
+ if (distance_average_.Valid()) {
+ LOG(DEBUG, "VisionDistance %f\n", status->vision_distance);
+ if (unsafe_goal->use_vision_for_shots) {
+ auto shot_params =
+ constants::GetValues().shot_interpolation_table.GetShooterData(
+ distance_average_.Get());
+ hood_goal.angle = shot_params.angle;
+ shooter_goal.angular_velocity = shot_params.power;
+ }
+ } else {
+ LOG(DEBUG, "VisionNotValid %f\n", status->vision_distance);
+ }
+ }
+
+ hood_.Iterate(
+ unsafe_goal != nullptr ? &hood_goal : nullptr, &(position->hood),
+ output != nullptr ? &(output->voltage_hood) : nullptr, &(status->hood));
+ shooter_.Iterate(unsafe_goal != nullptr ? &shooter_goal : nullptr,
&(position->theta_shooter), position->sent_time,
output != nullptr ? &(output->voltage_shooter) : nullptr,
&(status->shooter));
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index a605db0..e15bfe3 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -5,11 +5,12 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2017/control_loops/superstructure/column/column.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/shooter/shooter.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
-#include "y2017/control_loops/superstructure/column/column.h"
+#include "y2017/control_loops/superstructure/vision_distance_average.h"
namespace y2017 {
namespace control_loops {
@@ -48,6 +49,8 @@
// If true, we ignore collisions.
bool ignore_collisions_ = false;
+ VisionDistanceAverage distance_average_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index a771f2e..bac9051 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -189,6 +189,7 @@
HoodGoal hood;
ShooterGoal shooter;
bool lights_on;
+ bool use_vision_for_shots;
};
message Status {
@@ -205,6 +206,8 @@
TurretProfiledSubsystemStatus turret;
IndexerStatus indexer;
+
+ float vision_distance;
};
message Position {
diff --git a/y2017/control_loops/superstructure/vision_distance_average.h b/y2017/control_loops/superstructure/vision_distance_average.h
new file mode 100644
index 0000000..fbe2f00
--- /dev/null
+++ b/y2017/control_loops/superstructure/vision_distance_average.h
@@ -0,0 +1,61 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_VISION_DISTANCE_AVERAGE_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_VISION_DISTANCE_AVERAGE_H_
+
+#include <stdint.h>
+#include <chrono>
+
+#include "aos/common/ring_buffer.h"
+#include "aos/common/time.h"
+#include "y2017/vision/vision.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+
+// Averages out the distance from the vision system by a factor of 25.
+class VisionDistanceAverage {
+ public:
+ // Call on every tick of the control loop to update the state.
+ void Tick(::aos::monotonic_clock::time_point monotonic_now,
+ const vision::VisionStatus *vision_status) {
+ auto cull_time = monotonic_now - std::chrono::seconds(2);
+ while (data_.size() > 0 && data_[0].time < cull_time) {
+ data_.Shift();
+ cached_value_ = ComputeValue();
+ }
+
+ if (vision_status != nullptr && vision_status->image_valid) {
+ data_.Push({monotonic_now, vision_status->distance});
+ cached_value_ = ComputeValue();
+ }
+ }
+
+ // Does not update while not seeing new images.
+ double Get() { return cached_value_; }
+
+ // Valid gives a sense of how recent the data is.
+ bool Valid() { return data_.size() > 4; }
+
+ private:
+ double ComputeValue() {
+ double result = 0.0;
+ for (size_t i = 0; i < data_.size(); ++i) {
+ result += data_[i].value;
+ }
+ return result / data_.size();
+ }
+
+ struct DistanceEvent {
+ ::aos::monotonic_clock::time_point time;
+ double value;
+ };
+
+ double cached_value_ = 0.0;
+ ::aos::RingBuffer<DistanceEvent, 25> data_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_VISION_DISTANCE_AVERAGE_H_
diff --git a/y2017/control_loops/superstructure/vision_distance_average_test.cc b/y2017/control_loops/superstructure/vision_distance_average_test.cc
new file mode 100644
index 0000000..70f2f64
--- /dev/null
+++ b/y2017/control_loops/superstructure/vision_distance_average_test.cc
@@ -0,0 +1,70 @@
+#include "y2017/control_loops/superstructure/vision_distance_average.h"
+
+#include "gtest/gtest.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+
+class VisionDistanceAverageTest : public ::testing::Test {
+ public:
+ ::aos::monotonic_clock::time_point tick_time() {
+ current_time_ += std::chrono::milliseconds(60);
+ return current_time_;
+ }
+
+ VisionDistanceAverage* average() {
+ return &average_;
+ }
+
+ void TickInvalid() {
+ vision::VisionStatus status;
+ status.image_valid = false;
+
+ average_.Tick(tick_time(), &status);
+ }
+
+ void TickValid(double distance) {
+ vision::VisionStatus status;
+ status.image_valid = true;
+ status.distance = distance;
+
+ average_.Tick(tick_time(), &status);
+ }
+
+ void TickNullptr() {
+ average_.Tick(tick_time(), nullptr);
+ }
+
+ private:
+ ::aos::monotonic_clock::time_point current_time_ =
+ ::aos::monotonic_clock::epoch() + std::chrono::seconds(300);
+
+ VisionDistanceAverage average_;
+};
+
+TEST_F(VisionDistanceAverageTest, AverageTest) {
+ EXPECT_FALSE(average()->Valid());
+ for (int i = 0; i < 100; ++i) {
+ TickValid(2.0 + i);
+ }
+ EXPECT_TRUE(average()->Valid());
+ EXPECT_EQ(89.0, average()->Get());
+}
+
+TEST_F(VisionDistanceAverageTest, AverageDropCount) {
+ EXPECT_FALSE(average()->Valid());
+ for (int i = 0; i < 100; ++i) {
+ TickValid(2.0 + i);
+ }
+ EXPECT_TRUE(average()->Valid());
+ for (int i = 0; i < 100; ++i) {
+ TickInvalid();
+ }
+ TickNullptr();
+ EXPECT_FALSE(average()->Valid());
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017