Flash the light white when vision is locked
Flashing is easier to see, and we need feedback.
Change-Id: I5e827912947e7068feccafdc46eec49374087db8
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 524a1ba..87afdfa 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -27,6 +27,7 @@
":superstructure_queue",
":vacuum",
"//aos/controls:control_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2019:constants",
"//y2019:status_light",
],
@@ -48,6 +49,7 @@
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2019:status_light",
"//y2019/control_loops/superstructure/intake:intake_plants",
],
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index f62ba37..a9c7ce1 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -2,6 +2,7 @@
#include "aos/controls/control_loops.q.h"
#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/status_light.q.h"
@@ -92,22 +93,48 @@
intake_.set_min_position(collision_avoidance_.min_intake_goal());
intake_.set_max_position(collision_avoidance_.max_intake_goal());
+ ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
+
if (status && unsafe_goal) {
// Light Logic
if (status->estopped) {
// Estop is red
SendColors(1.0, 0.0, 0.0);
- } else if (status->has_piece) {
- // Having suction is green
- SendColors(0.0, 1.0, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 0 && !status->has_piece) {
- // Ball mode is orange
- SendColors(1.0, 0.1, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 1 && !status->has_piece) {
- // Disk mode is deep blue
- SendColors(0.05, 0.1, 0.5);
+ } else if (::frc971::control_loops::drivetrain_queue.status.get() &&
+ ::frc971::control_loops::drivetrain_queue.status
+ ->line_follow_logging.frozen) {
+ // Vision align is flashing white.
+ ++line_blink_count_;
+ if (line_blink_count_ < 20) {
+ SendColors(1.0, 1.0, 1.0);
+ } else {
+ // And then flash with green if we have a game piece.
+ if (status->has_piece) {
+ SendColors(0.0, 1.0, 0.0);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+ }
+
+ if (line_blink_count_ > 40) {
+ line_blink_count_ = 0;
+ }
} else {
- SendColors(0.0, 0.0, 0.0);
+ line_blink_count_ = 0;
+ if (status->has_piece) {
+ // Green if we have a game piece.
+ SendColors(0.0, 1.0, 0.0);
+ } else if (unsafe_goal->suction.gamepiece_mode == 0 &&
+ !status->has_piece) {
+ // Ball mode is orange
+ SendColors(1.0, 0.1, 0.0);
+ } else if (unsafe_goal->suction.gamepiece_mode == 1 &&
+ !status->has_piece) {
+ // Disk mode is deep blue
+ SendColors(0.05, 0.1, 0.5);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
}
}
}
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 626c84b..2df9026 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -50,6 +50,8 @@
CollisionAvoidance collision_avoidance_;
+ int line_blink_count_ = 0;
+
static constexpr double kMinIntakeAngleForRollers = -0.7;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 4d3de51..dd327ee 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -6,16 +6,17 @@
#include "aos/controls/control_loop_test.h"
#include "aos/queue.h"
#include "frc971/control_loops/capped_test_plant.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"
#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"
#include "y2019/control_loops/superstructure/superstructure.h"
#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
+#include "y2019/status_light.q.h"
namespace y2019 {
namespace control_loops {
@@ -290,6 +291,7 @@
"position"),
superstructure_(&event_loop_) {
status_light.Clear();
+ ::frc971::control_loops::drivetrain_queue.status.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}