Make LED indicator show intaking state cleaner
This lets our drivers know if they got a cone/cube a lot easier by
removing the other states that it could be confused with.
Change-Id: I2bd4b72c95742efd1c33ac96b94b78bd7fc3b356
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index e5bc830..afca18b 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -166,6 +166,7 @@
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
index afd0201..5cee266 100644
--- a/y2023/control_loops/superstructure/led_indicator.cc
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -1,6 +1,7 @@
#include "y2023/control_loops/superstructure/led_indicator.h"
namespace led = ctre::phoenix::led;
+namespace chrono = std::chrono;
namespace y2023::control_loops::superstructure {
@@ -13,6 +14,8 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_position_fetcher_(
event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_goal_fetcher_(
+ event_loop_->MakeFetcher<Goal>("/superstructure")),
server_statistics_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/roborio/aos")),
@@ -35,7 +38,9 @@
candle_.ConfigAllSettings(config, 0);
event_loop_->AddPhasedLoop([this](int) { DecideColor(); },
- std::chrono::milliseconds(20));
+ chrono::milliseconds(20));
+ event_loop_->OnRun(
+ [this]() { startup_time_ = event_loop_->monotonic_now(); });
}
// This method will be called once per scheduler run
@@ -75,6 +80,7 @@
drivetrain_output_fetcher_.Fetch();
drivetrain_status_fetcher_.Fetch();
client_statistics_fetcher_.Fetch();
+ superstructure_goal_fetcher_.Fetch();
gyro_reading_fetcher_.Fetch();
localizer_output_fetcher_.Fetch();
@@ -96,15 +102,18 @@
// Not zeroed
if (superstructure_status_fetcher_.get() &&
!superstructure_status_fetcher_->zeroed()) {
- DisplayLed(255, 0, 255);
+ DisplayLed(255, 255, 0);
return;
}
- // If the imu gyro readings are not being sent/updated recently
- if (!gyro_reading_fetcher_.get() ||
- gyro_reading_fetcher_.context().monotonic_event_time <
- event_loop_->monotonic_now() -
- frc971::controls::kLoopFrequency * 10) {
+ // If the imu gyro readings are not being sent/updated recently. Only do this
+ // after we've been on for a bit.
+ if (event_loop_->context().monotonic_event_time >
+ startup_time_ + chrono::seconds(5) &&
+ (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time +
+ frc971::controls::kLoopFrequency * 10 <
+ event_loop_->monotonic_now())) {
if (flash_counter_.Flash()) {
DisplayLed(255, 0, 0);
} else {
@@ -127,54 +136,44 @@
return;
}
- if (superstructure_status_fetcher_.get()) {
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_goal_fetcher_.get()) {
+ const bool cone = (superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_UP ||
+ superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_DOWN);
+ const bool intaking = (superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_UP ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CUBE ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_LAST ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_DOWN);
// Check if end effector is intaking.
if (superstructure_status_fetcher_->end_effector_state() ==
- EndEffectorState::INTAKING) {
+ EndEffectorState::LOADED &&
+ intaking) {
if (flash_counter_.Flash()) {
- DisplayLed(255, 165, 0);
- } else {
- DisplayLed(0, 0, 0);
+ if (cone) {
+ DisplayLed(255, 165, 0);
+ } else {
+ DisplayLed(138, 43, 226);
+ }
+ return;
}
-
- return;
- }
- // Check if end effector is spitting.
- if (superstructure_status_fetcher_->end_effector_state() ==
- EndEffectorState::SPITTING) {
- if (flash_counter_.Flash()) {
- DisplayLed(0, 255, 0);
- } else {
- DisplayLed(0, 0, 0);
- }
-
- return;
- }
-
- // Check the if there is a cone in the end effector.
- if (superstructure_status_fetcher_->game_piece() ==
- vision::Class::CONE_UP ||
- superstructure_status_fetcher_->game_piece() ==
- vision::Class::CONE_DOWN) {
- DisplayLed(255, 255, 0);
- return;
- }
- // Check if the cube beam break is triggered.
- if (superstructure_status_fetcher_->game_piece() == vision::Class::CUBE) {
- DisplayLed(138, 43, 226);
- return;
}
// Check if there is a target that is in sight
- if (drivetrain_status_fetcher_.get() != nullptr &&
- drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
- DisplayLed(255, 165, 0);
- return;
- }
-
if (event_loop_->monotonic_now() <
- last_accepted_time_ + std::chrono::milliseconds(500)) {
- DisplayLed(0, 0, 255);
+ last_accepted_time_ + chrono::milliseconds(100)) {
+ if (drivetrain_status_fetcher_.get() != nullptr &&
+ drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
+ DisplayLed(0, 255, 0);
+ return;
+ } else {
+ DisplayLed(0, 0, 255);
+ }
return;
}
}
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
index 256de30..dfdf4b1 100644
--- a/y2023/control_loops/superstructure/led_indicator.h
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -12,6 +12,7 @@
#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/queues/gyro_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
@@ -76,6 +77,7 @@
drivetrain_output_fetcher_;
aos::Fetcher<Status> superstructure_status_fetcher_;
aos::Fetcher<Position> superstructure_position_fetcher_;
+ aos::Fetcher<Goal> superstructure_goal_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics>
server_statistics_fetcher_;
aos::Fetcher<aos::message_bridge::ClientStatistics>
@@ -89,6 +91,9 @@
aos::monotonic_clock::time_point last_accepted_time_ =
aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point startup_time_ =
+ aos::monotonic_clock::min_time;
+
FlashCounter flash_counter_{kFlashIterations};
};
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 7ac7000..2ee2d2b 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -28,5 +28,4 @@
}
-
root_type Goal;