Added status light and logic.
Change-Id: Idf192f1e6af3cb4f8a8e659fce5269cbd78bd4bd
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 3443d63..dd332b8 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -4,10 +4,26 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2019/status_light.q.h"
+
namespace y2019 {
namespace control_loops {
namespace superstructure {
+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");
+ }
+}
+} // namespace
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
@@ -75,6 +91,22 @@
wrist_.set_max_position(collision_avoidance_.max_wrist_goal());
intake_.set_min_position(collision_avoidance_.min_intake_goal());
intake_.set_max_position(collision_avoidance_.max_intake_goal());
+
+ if (status && unsafe_goal) {
+ // Light Logic
+ if (status->estopped) {
+ // Estop is red
+ SendColors(0.5, 0.0, 0.0);
+ } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ // Ball mode is blue
+ SendColors(0.0, 0.0, 0.5);
+ } else if (unsafe_goal->suction.gamepiece_mode == 1) {
+ // Disk mode is yellow
+ SendColors(0.5, 0.5, 0.0);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+ }
}
} // namespace superstructure