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/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_);