Override Hood and Shooter goals with vision distance
Change-Id: I9741880fe7f96cf4208ca2cf75d584d683a30cea
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 5892282..2e97b9e 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -90,9 +90,10 @@
*shot_interpolation_table =
::frc971::shooter_interpolation::InterpolationTable(
{// { distance_to_target, { shot_angle, shot_power }},
- {100.1, {20.0 * M_PI / 180.0, 335.0}},
- {150.2, {35.0 * M_PI / 180.0, 384.0}},
- {200.3, {40.0 * M_PI / 180.0, 417.0}},
+ {1.67, {0.31, 320.0}},
+ {1.90, {0.33, 330.0}},
+ {2.15, {0.33, 347.0}},
+ {2.45, {0.33, 361.0}},
});
switch (team) {
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
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index 31623d0..5b13b7b 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -73,7 +73,7 @@
const ButtonLocation kIntakeIn(3, 12);
const ButtonLocation kIntakeOut(3, 8);
const ButtonLocation kFire(3, 3);
-const ButtonLocation kCloseShot(3, 7);
+const ButtonLocation kVisionDistanceShot(3, 7);
const ButtonLocation kMiddleShot(3, 6);
const POVLocation kFarShot(3, 270);
@@ -88,7 +88,7 @@
public:
Reader() {}
- enum class ShotDistance { CLOSE_SHOT, MIDDLE_SHOT, FAR_SHOT };
+ enum class ShotDistance { VISION_SHOT, MIDDLE_SHOT, FAR_SHOT };
ShotDistance last_shot_distance_ = ShotDistance::FAR_SHOT;
@@ -104,8 +104,6 @@
}
}
- vision_valid_ = false;
-
if (!auto_running_) {
HandleDrivetrain(data);
HandleTeleop(data);
@@ -262,36 +260,40 @@
if (data.PosEdge(kMiddleShot)) {
turret_goal_ = -M_PI;
}
- if (data.PosEdge(kCloseShot)) {
- turret_goal_ = -M_PI;
- }
if (data.PosEdge(kFarShot)) {
turret_goal_ = 0.0;
}
+ if (data.PosEdge(kVisionDistanceShot)) {
+ turret_goal_ = 0.0;
+ }
- if (data.IsPressed(kCloseShot)) {
- last_shot_distance_ = ShotDistance::CLOSE_SHOT;
+ if (data.IsPressed(kVisionDistanceShot)) {
+ last_shot_distance_ = ShotDistance::VISION_SHOT;
} else if (data.IsPressed(kMiddleShot)) {
last_shot_distance_ = ShotDistance::MIDDLE_SHOT;
} else if (data.IsPressed(kFarShot)) {
last_shot_distance_ = ShotDistance::FAR_SHOT;
}
- if (data.IsPressed(kVisionAlign) || data.IsPressed(kCloseShot) ||
+ if (data.IsPressed(kVisionAlign) ||
data.IsPressed(kMiddleShot) || data.IsPressed(kFarShot) ||
data.IsPressed(kFire)) {
switch (last_shot_distance_) {
- case ShotDistance::CLOSE_SHOT:
- hood_goal_ = 0.30;
- shooter_velocity_ = 322.0;
+ case ShotDistance::VISION_SHOT:
+ hood_goal_ = 0.31;
+ shooter_velocity_ = 320.0;
+ vision_distance_shot_ = true;
+ turret_goal_ = 0.0;
break;
case ShotDistance::MIDDLE_SHOT:
hood_goal_ = 0.43 - 0.00;
shooter_velocity_ = 361.0;
+ vision_distance_shot_ = false;
break;
case ShotDistance::FAR_SHOT:
hood_goal_ = 0.43 - 0.01;
shooter_velocity_ = 365.0;
+ vision_distance_shot_ = false;
break;
}
} else {
@@ -349,6 +351,12 @@
new_superstructure_goal->intake.voltage_rollers = 0.0;
new_superstructure_goal->lights_on = lights_on;
+ if (data.IsPressed(kVisionAlign) && vision_distance_shot_) {
+ new_superstructure_goal->use_vision_for_shots = true;
+ } else {
+ new_superstructure_goal->use_vision_for_shots = false;
+ }
+
if (superstructure_queue.status->intake.position >
superstructure_queue.status->intake.unprofiled_goal_position + 0.01) {
intake_accumulator_ = 10;
@@ -384,8 +392,8 @@
} else if (fire_) {
new_superstructure_goal->indexer.voltage_rollers = 12.0;
switch (last_shot_distance_) {
- case ShotDistance::CLOSE_SHOT:
- new_superstructure_goal->indexer.angular_velocity = -0.90 * M_PI;
+ case ShotDistance::VISION_SHOT:
+ new_superstructure_goal->indexer.angular_velocity = -2.25 * M_PI;
break;
case ShotDistance::MIDDLE_SHOT:
case ShotDistance::FAR_SHOT:
@@ -437,7 +445,7 @@
bool was_running_ = false;
bool auto_running_ = false;
- bool vision_valid_ = false;
+ bool vision_distance_shot_ = false;
bool fire_ = false;
double robot_velocity_ = 0.0;