Use a second camera and switch between them.
Also, blink out the state over the beacon.
Change-Id: If606dfed9ae64137f71429f1190f04d5dac2c4ec
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 8fcba95..cc179d5 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -25,9 +25,12 @@
":superstructure_queue",
"//aos/common/controls:control_loop",
"//frc971/control_loops:queues",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018:constants",
+ "//y2018:status_light",
"//y2018/control_loops/superstructure/arm",
"//y2018/control_loops/superstructure/intake",
+ "//y2018/vision:vision_queue",
],
)
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 757b166..2098cd0 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -5,8 +5,11 @@
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
+#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
namespace y2018 {
namespace control_loops {
@@ -21,6 +24,17 @@
constexpr double kMaxIntakeRollerVoltage = 12.0;
} // namespace
+void SendColors(float red, float green, float blue) {
+ auto new_status_light = status_light.MakeMessage();
+ new_status_light->red = red;
+ new_status_light->green = green;
+ new_status_light->blue = blue;
+
+ if (!new_status_light.Send()) {
+ LOG(ERROR, "Failed to send lights.\n");
+ }
+}
+
Superstructure::Superstructure(
control_loops::SuperstructureQueue *superstructure_queue)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
@@ -237,6 +251,34 @@
stuck_count_ = 0;
}
status->rotation_state = static_cast<uint32_t>(rotation_state_);
+
+ ::frc971::control_loops::drivetrain_queue.output.FetchLatest();
+
+ ::y2018::vision::vision_status.FetchLatest();
+ if (status->estopped) {
+ SendColors(0.5, 0.0, 0.0);
+ } else if (!y2018::vision::vision_status.get() ||
+ y2018::vision::vision_status.Age() > chrono::seconds(1)) {
+ SendColors(0.5, 0.5, 0.0);
+ } else if (rotation_state_ == RotationState::ROTATING_LEFT ||
+ rotation_state_ == RotationState::ROTATING_RIGHT) {
+ SendColors(0.5, 0.20, 0.0);
+ } else if (rotation_state_ == RotationState::STUCK) {
+ SendColors(0.5, 0.0, 0.5);
+ } else if (position->box_back_beambreak_triggered) {
+ SendColors(0.0, 0.0, 0.5);
+ } else if (position->box_distance < 0.2) {
+ SendColors(0.0, 0.5, 0.0);
+ } else if (::frc971::control_loops::drivetrain_queue.output.get() &&
+ ::std::max(::std::abs(::frc971::control_loops::drivetrain_queue
+ .output->left_voltage),
+ ::std::abs(::frc971::control_loops::drivetrain_queue
+ .output->right_voltage)) > 11.5) {
+ SendColors(0.5, 0.0, 0.5);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+
last_box_distance_ = clipped_box_distance;
}
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 639a683..d21d1ce 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,6 +7,7 @@
#include "aos/common/controls/control_loop_test.h"
#include "aos/common/queue.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -14,6 +15,8 @@
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -271,6 +274,9 @@
".y2018.control_loops.superstructure.output",
".y2018.control_loops.superstructure.status"),
superstructure_(&superstructure_queue_) {
+ status_light.Clear();
+ ::y2018::vision::vision_status.Clear();
+ ::frc971::control_loops::drivetrain_queue.output.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}