Added status light and logic.
Change-Id: Idf192f1e6af3cb4f8a8e659fce5269cbd78bd4bd
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 00bacae..524a1ba 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -24,10 +24,11 @@
],
deps = [
":collision_avoidance",
- ":vacuum",
":superstructure_queue",
+ ":vacuum",
"//aos/controls:control_loop",
"//y2019:constants",
+ "//y2019:status_light",
],
)
@@ -47,6 +48,7 @@
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
+ "//y2019:status_light",
"//y2019/control_loops/superstructure/intake:intake_plants",
],
)
@@ -88,7 +90,7 @@
],
deps = [
":superstructure_queue",
- "//aos/controls:control_loop"
+ "//aos/controls:control_loop",
],
)
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
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index f87e4f2..d5c8a73 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -4,10 +4,13 @@
import "frc971/control_loops/profiled_subsystem.q";
struct SuctionGoal {
- // True = open solenoid (apply suction)
- // Top/bottom are when wrist is forward
- bool top;
- bool bottom;
+ // True = apply suction
+ bool grab_piece;
+
+ // 0 = ball mode
+ // 1 = disk mode
+
+ int32_t gamepiece_mode;
};
queue_group SuperstructureQueue {
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 0cce4a6..4d3de51 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -10,6 +10,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2019/constants.h"
+#include "y2019/status_light.q.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
@@ -288,6 +289,7 @@
".y2019.control_loops.superstructure.superstructure_queue."
"position"),
superstructure_(&event_loop_) {
+ status_light.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
@@ -712,8 +714,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -734,8 +735,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -761,8 +761,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -776,8 +775,7 @@
// Turn off suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = false;
- goal->suction.bottom = false;
+ goal->suction.grab_piece = false;
ASSERT_TRUE(goal.Send());
}
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 7bf2e94..497ae5b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -32,7 +32,7 @@
low_pump_voltage = *has_piece;
if (unsafe_goal && output) {
- const bool release = !unsafe_goal->top && !unsafe_goal->bottom;
+ const bool release = !unsafe_goal->grab_piece;
if (release) {
last_release_time_ = monotonic_now;
@@ -44,8 +44,16 @@
output->pump_voltage =
release ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage : kPumpVoltage);
- output->intake_suction_top = unsafe_goal->top;
- output->intake_suction_bottom = unsafe_goal->bottom;
+ if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 0) {
+ output->intake_suction_top = false;
+ output->intake_suction_bottom = true;
+ } else if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 1) {
+ output->intake_suction_top = true;
+ output->intake_suction_bottom = true;
+ } else {
+ output->intake_suction_top = false;
+ output->intake_suction_bottom = false;
+ }
// If we intend to release, or recently released, set has_piece to false so
// that we give the part of the vacuum circuit with the pressure sensor time