Add code for status led
Changes color based on the status of the robot.
Change-Id: Id8817f360fc1063f19117a020ed9a8210eb04f3c
Signed-off-by: Henry Speiser <henry@speiser.net>
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index 4739cd1..669e691 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -159,6 +159,29 @@
],
)
+cc_library(
+ name = "led_indicator_lib",
+ srcs = ["led_indicator.cc"],
+ hdrs = ["led_indicator.h"],
+ data = [
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":superstructure_output_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//third_party:phoenix",
+ "//third_party:wpilib",
+ ],
+)
+
ts_library(
name = "superstructure_plotter",
srcs = ["superstructure_plotter.ts"],
diff --git a/y2022/control_loops/superstructure/led_indicator.cc b/y2022/control_loops/superstructure/led_indicator.cc
new file mode 100644
index 0000000..4ec934d
--- /dev/null
+++ b/y2022/control_loops/superstructure/led_indicator.cc
@@ -0,0 +1,125 @@
+#include "y2022/control_loops/superstructure/led_indicator.h"
+
+namespace led = ctre::phoenix::led;
+
+namespace y2022::control_loops::superstructure {
+
+LedIndicator::LedIndicator(aos::EventLoop *event_loop)
+ : drivetrain_output_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<Status>("/superstructure")),
+ server_statistics_fetcher_(
+ event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/roborio/aos")) {
+ led::CANdleConfiguration config;
+ config.statusLedOffWhenActive = true;
+ config.disableWhenLOS = false;
+ config.brightnessScalar = 1.0;
+ candle_.ConfigAllSettings(config, 0);
+
+ event_loop->AddPhasedLoop([&](int) { DecideColor(); },
+ std::chrono::milliseconds(20));
+}
+
+// This method will be called once per scheduler run
+void LedIndicator::DisplayLed(uint8_t r, uint8_t g, uint8_t b) {
+ candle_.SetLEDs(static_cast<int>(r), static_cast<int>(g),
+ static_cast<int>(b));
+}
+
+namespace {
+bool DisconnectedPi(const aos::message_bridge::ServerStatistics &server_stats) {
+ for (const auto *pi_status : *server_stats.connections()) {
+ if (pi_status->state() == aos::message_bridge::State::DISCONNECTED) {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool DrivingFast(
+ const frc971::control_loops::drivetrain::Output &drivetrain_out) {
+ return (drivetrain_out.left_voltage() >= 11.5 ||
+ drivetrain_out.right_voltage() >= 11.5);
+}
+} // namespace
+
+void LedIndicator::DecideColor() {
+ superstructure_status_fetcher_.Fetch();
+ server_statistics_fetcher_.Fetch();
+ drivetrain_output_fetcher_.Fetch();
+
+ // Estopped
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->estopped()) {
+ DisplayLed(255, 0, 0);
+ return;
+ }
+
+ // Not zeroed
+ if (superstructure_status_fetcher_.get() &&
+ !superstructure_status_fetcher_->zeroed()) {
+ DisplayLed(255, 255, 0);
+ return;
+ }
+
+ // Pi disconnected
+ if (server_statistics_fetcher_.get() &&
+ DisconnectedPi(*server_statistics_fetcher_)) {
+ if (disconnected_flash_) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(0, 0, 255);
+ }
+
+ if (disconnected_counter_ % kFlashIterations == 0) {
+ disconnected_flash_ = !disconnected_flash_;
+ }
+ disconnected_counter_++;
+ return;
+ }
+
+ // Driving fast
+ if (drivetrain_output_fetcher_.get() &&
+ DrivingFast(*drivetrain_output_fetcher_)) {
+ DisplayLed(138, 43, 226);
+ return;
+ }
+
+ // Statemachine
+ if (superstructure_status_fetcher_.get()) {
+ switch (superstructure_status_fetcher_->state()) {
+ case (SuperstructureState::IDLE):
+ DisplayLed(0, 0, 0);
+ break;
+ case (SuperstructureState::TRANSFERRING):
+ DisplayLed(0, 0, 255);
+ break;
+ case (SuperstructureState::LOADING):
+ DisplayLed(255, 255, 255);
+ break;
+ case (SuperstructureState::LOADED):
+ if (!superstructure_status_fetcher_->ready_to_fire()) {
+ DisplayLed(255, 140, 0);
+ } else if (superstructure_status_fetcher_->front_intake_has_ball() ||
+ superstructure_status_fetcher_->back_intake_has_ball()) {
+ DisplayLed(165, 42, 42);
+ } else {
+ DisplayLed(0, 255, 0);
+ }
+ break;
+ case (SuperstructureState::SHOOTING):
+ if (!superstructure_status_fetcher_->flippers_open()) {
+ DisplayLed(255, 105, 180);
+ } else {
+ DisplayLed(0, 255, 255);
+ }
+ break;
+ }
+ return;
+ }
+}
+
+} // namespace y2022::control_loops::superstructure
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
new file mode 100644
index 0000000..bdcee6c
--- /dev/null
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -0,0 +1,62 @@
+#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "ctre/phoenix/led/CANdle.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2022::control_loops::superstructure {
+
+class LedIndicator {
+ public:
+ LedIndicator(aos::EventLoop *event_loop);
+
+ // Colors in order of priority:
+ //
+ // Red: estopped
+ // Yellow: not zeroed
+ // Flash blue/red: pi disconnected
+ // Purple: driving fast
+ //
+ // Statemachine:
+ // IDLE:
+ // Off
+ // TRANSFERRING:
+ // Blue
+ // LOADING:
+ // White
+ // LOADED:
+ // Green: ready to fire
+ // Brown: intaked another ball
+ // Orange: loaded
+ // SHOOTING:
+ // Pink: flippers opening
+ // Cyan: superstructure shooting
+ void DecideColor();
+
+ private:
+ static constexpr size_t kFlashIterations = 5;
+
+ void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
+
+ ctre::phoenix::led::CANdle candle_{0, ""};
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Output>
+ drivetrain_output_fetcher_;
+ aos::Fetcher<Status> superstructure_status_fetcher_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+
+ size_t disconnected_counter_ = 0;
+ bool disconnected_flash_ = false;
+};
+
+} // namespace y2022::control_loops::superstructure
+
+#endif // Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 63fa5e2..63f3d60 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -234,6 +234,16 @@
frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*turret_loading_goal_buffer.fbb(), turret_loading_position));
+ const bool turret_near_goal =
+ turret_goal != nullptr &&
+ std::abs(turret_goal->unsafe_goal() - turret_.position()) <
+ kTurretGoalThreshold;
+ const bool collided = collision_avoidance_.IsCollided(
+ {.intake_front_position = intake_front_.estimated_position(),
+ .intake_back_position = intake_back_.estimated_position(),
+ .turret_position = turret_.estimated_position(),
+ .shooting = true});
+
switch (state_) {
case SuperstructureState::IDLE: {
// Only change the turret's goal loading position when idle, to prevent us
@@ -335,16 +345,6 @@
break;
}
case SuperstructureState::SHOOTING: {
- const bool turret_near_goal =
- turret_goal != nullptr &&
- std::abs(turret_goal->unsafe_goal() - turret_.position()) <
- kTurretGoalThreshold;
- const bool collided = collision_avoidance_.IsCollided(
- {.intake_front_position = intake_front_.estimated_position(),
- .intake_back_position = intake_back_.estimated_position(),
- .turret_position = turret_.estimated_position(),
- .shooting = true});
-
// Don't open the flippers until the turret's ready: give them as little
// time to get bumped as possible.
if (!turret_near_goal || collided) {
@@ -509,6 +509,8 @@
status_builder.add_flippers_open(flippers_open_);
status_builder.add_reseating_in_catapult(reseating_in_catapult_);
status_builder.add_fire(fire_);
+ status_builder.add_ready_to_fire(state_ == SuperstructureState::LOADED &&
+ turret_near_goal && !collided);
status_builder.add_state(state_);
if (!front_intake_has_ball_ && !back_intake_has_ball_) {
status_builder.add_intake_state(IntakeState::NO_BALL);
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index a1817a7..06fddd0 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -55,6 +55,8 @@
flippers_open:bool (id: 12);
// Whether the flippers failed to open and we are retrying
reseating_in_catapult:bool (id: 13);
+ // Whether the turret is ready for firing
+ ready_to_fire:bool (id: 20);
// Whether the catapult was told to fire,
// meaning that the turret and flippers are ready for firing
// and we were asked to fire. Different from fire flag in goal.