Retrieve VisionStatus in the superstructure
This will let us access the VisionStatus message from several parts of
the superstructure. Using FetchLatest() within several parts of the
code within the same binary would cause some logical errors that I'd
rather avoid. I want to access the VisionStatus in the Hood and the
Turret for automatically selecting a good shot angle and shot power.
Change-Id: I00dbc6a91a7dbd959f8720ab9735b06dc4bfe99b
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index e248739..cffc7ad 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -389,15 +389,17 @@
void Column::Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position, double *indexer_output,
- double *turret_output, IndexerStatus *indexer_status,
+ const ColumnPosition *position,
+ const vision::VisionStatus *vision_status,
+ double *indexer_output, double *turret_output,
+ IndexerStatus *indexer_status,
TurretProfiledSubsystemStatus *turret_status,
intake::Intake *intake) {
bool disable = turret_output == nullptr || indexer_output == nullptr;
profiled_subsystem_.Correct(*position);
vision_time_adjuster_.Tick(::aos::monotonic_clock::now(),
- profiled_subsystem_.X_hat(2, 0));
+ profiled_subsystem_.X_hat(2, 0), vision_status);
switch (state_) {
case State::UNINITIALIZED:
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 092e253..6bfe1c9 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -16,6 +16,7 @@
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
+#include "y2017/vision/vision.q.h"
namespace y2017 {
namespace control_loops {
@@ -181,8 +182,10 @@
void Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position, double *indexer_output,
- double *turret_output, IndexerStatus *indexer_status,
+ const ColumnPosition *position,
+ const vision::VisionStatus *vision_status,
+ double *indexer_output, double *turret_output,
+ IndexerStatus *indexer_status,
TurretProfiledSubsystemStatus *turret_status,
intake::Intake *intake);
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
index c58822a..f19bbf7 100644
--- a/y2017/control_loops/superstructure/hood/hood.cc
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -12,6 +12,8 @@
constexpr double Hood::kZeroingVoltage;
constexpr double Hood::kOperatingVoltage;
+constexpr ::aos::monotonic_clock::duration Hood::kTimeTillNotMoving;
+
// The tracking error to allow before declaring that we are stuck and reversing
// direction while zeroing.
constexpr double kStuckZeroingTrackingError = 0.02;
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 2a04ef4..e0017c1 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -7,6 +7,7 @@
#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/vision/vision.q.h"
namespace y2017 {
namespace control_loops {
@@ -36,6 +37,11 @@
column_.Reset();
}
+ const vision::VisionStatus *vision_status = nullptr;
+ if (vision::vision_status.FetchLatest()) {
+ vision_status = vision::vision_status.get();
+ }
+
hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
&(position->hood),
output != nullptr ? &(output->voltage_hood) : nullptr,
@@ -91,7 +97,7 @@
column_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
- &(position->column),
+ &(position->column), vision_status,
output != nullptr ? &(output->voltage_indexer) : nullptr,
output != nullptr ? &(output->voltage_turret) : nullptr,
&(status->indexer), &(status->turret), &intake_);
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.cc b/y2017/control_loops/superstructure/vision_time_adjuster.cc
index b93e1d5..1bd61f8 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.cc
@@ -5,7 +5,6 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
-#include "y2017/vision/vision.q.h"
namespace y2017 {
namespace control_loops {
@@ -110,7 +109,8 @@
VisionTimeAdjuster::VisionTimeAdjuster() {}
void VisionTimeAdjuster::Tick(monotonic_clock::time_point monotonic_now,
- double turret_position) {
+ double turret_position,
+ const vision::VisionStatus *vision_status) {
// We have new column data, store it.
column_data_.Push({.time = monotonic_now, .turret = turret_position});
@@ -126,10 +126,9 @@
// If we have new vision data, compute the newest absolute angle at which the
// target is.
- if (vision::vision_status.FetchLatest() &&
- vision::vision_status->image_valid) {
+ if (vision_status != nullptr && vision_status->image_valid) {
monotonic_clock::time_point last_target_time(
- monotonic_clock::duration(vision::vision_status->target_time));
+ monotonic_clock::duration(vision_status->target_time));
double column_angle = 0;
double drivetrain_angle = 0;
@@ -143,12 +142,11 @@
if (column_angle_is_valid && drivetrain_angle_is_valid) {
LOG(INFO, "Accepting Vision angle of %f, age %f\n",
most_recent_vision_angle_,
- chrono::duration_cast<chrono::duration<double>>(monotonic_now -
- last_target_time)
- .count());
- most_recent_vision_reading_ = vision::vision_status->angle;
+ chrono::duration_cast<chrono::duration<double>>(
+ monotonic_now - last_target_time).count());
+ most_recent_vision_reading_ = vision_status->angle;
most_recent_vision_angle_ =
- vision::vision_status->angle + column_angle + drivetrain_angle;
+ vision_status->angle + column_angle + drivetrain_angle;
most_recent_vision_time_ = monotonic_now;
}
}
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.h b/y2017/control_loops/superstructure/vision_time_adjuster.h
index 19307e3..a7b79f3 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.h
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.h
@@ -5,6 +5,7 @@
#include "aos/common/ring_buffer.h"
#include "aos/common/time.h"
+#include "y2017/vision/vision.q.h"
namespace y2017 {
namespace control_loops {
@@ -17,7 +18,7 @@
// This needs to be called at the same interval as the control loops so that
// it can attempt to make accurate goal recommendations.
void Tick(::aos::monotonic_clock::time_point monotonic_now,
- double turret_position);
+ double turret_position, const vision::VisionStatus *vision_status);
// Returns true if we have enough data to recommend a goal for the turret.
bool valid() const { return valid_; }
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
index a499836..c6dcd25 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
@@ -56,7 +56,11 @@
void RunIteration() {
SendMessages();
- adjuster_.Tick(::aos::monotonic_clock::now(), turret_angle_);
+ const vision::VisionStatus *vision_status = nullptr;
+ if (vision::vision_status.FetchLatest()) {
+ vision_status = vision::vision_status.get();
+ }
+ adjuster_.Tick(::aos::monotonic_clock::now(), turret_angle_, vision_status);
TickTime();
}