Make the purple light turn on when we accept images
Drop some of the other colors to make it clearer
Change-Id: I498333523de222888b29d106f276d2f1570e35e6
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index 31b35fb..f67fa12 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -181,6 +181,7 @@
"//frc971/queues:gyro_fbs",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/control_loops/superstructure/led_indicator.cc b/y2022/control_loops/superstructure/led_indicator.cc
index 94d0506..b636250 100644
--- a/y2022/control_loops/superstructure/led_indicator.cc
+++ b/y2022/control_loops/superstructure/led_indicator.cc
@@ -17,6 +17,9 @@
client_statistics_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
"/roborio/aos")),
+ localizer_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
+ "/localizer")),
gyro_reading_fetcher_(
event_loop_->MakeFetcher<frc971::sensors::GyroReading>(
"/drivetrain")) {
@@ -58,12 +61,6 @@
}
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() {
@@ -72,6 +69,15 @@
drivetrain_output_fetcher_.Fetch();
client_statistics_fetcher_.Fetch();
gyro_reading_fetcher_.Fetch();
+ localizer_output_fetcher_.Fetch();
+
+ if (localizer_output_fetcher_.get()) {
+ if (localizer_output_fetcher_->image_accepted_count() !=
+ last_accepted_count_) {
+ last_accepted_count_ = localizer_output_fetcher_->image_accepted_count();
+ last_accepted_time_ = event_loop_->monotonic_now();
+ }
+ }
// Estopped
if (superstructure_status_fetcher_.get() &&
@@ -90,7 +96,8 @@
// 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) {
+ event_loop_->monotonic_now() -
+ frc971::controls::kLoopFrequency * 10) {
if (imu_flash_) {
DisplayLed(255, 0, 0);
} else {
@@ -122,13 +129,6 @@
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()) {
@@ -147,18 +147,16 @@
} 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;
}
+
+ if (event_loop_->monotonic_now() <
+ last_accepted_time_ + std::chrono::seconds(2)) {
+ DisplayLed(255, 0, 255);
+ }
return;
}
}
diff --git a/y2022/control_loops/superstructure/led_indicator.h b/y2022/control_loops/superstructure/led_indicator.h
index 0f44788..c058254 100644
--- a/y2022/control_loops/superstructure/led_indicator.h
+++ b/y2022/control_loops/superstructure/led_indicator.h
@@ -12,6 +12,7 @@
#include "frc971/queues/gyro_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
namespace y2022::control_loops::superstructure {
@@ -58,8 +59,12 @@
server_statistics_fetcher_;
aos::Fetcher<aos::message_bridge::ClientStatistics>
client_statistics_fetcher_;
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
+ size_t last_accepted_count_ = 0;
+ aos::monotonic_clock::time_point last_accepted_time_ =
+ aos::monotonic_clock::min_time;
size_t imu_counter_ = 0;
bool imu_flash_ = false;
size_t disconnected_counter_ = 0;
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 8ca5395..4b0588c 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -1109,6 +1109,7 @@
output_builder.add_y(model_based_.xytheta()(1));
output_builder.add_theta(model_based_.xytheta()(2));
output_builder.add_zeroed(zeroer_.Zeroed());
+ output_builder.add_image_accepted_count(model_based_.total_accepted());
const Eigen::Quaterniond &orientation = model_based_.orientation();
Quaternion quaternion;
quaternion.mutate_x(orientation.x());
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index 5620a4d..fc15e9f 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -150,6 +150,8 @@
std::array<LedOutput, kNumPis> led_outputs() const { return led_outputs_; }
+ int total_accepted() const { return statistics_.total_accepted; }
+
private:
struct CombinedState {
AccelState accel_state = AccelState::Zero();
diff --git a/y2022/localizer/localizer_output.fbs b/y2022/localizer/localizer_output.fbs
index ec3302a..ff25c31 100644
--- a/y2022/localizer/localizer_output.fbs
+++ b/y2022/localizer/localizer_output.fbs
@@ -34,6 +34,9 @@
// Whether each led should be on.
// Indices correspond to pi number.
led_outputs:[LedOutput] (id: 6);
+
+ // Cumulative number of accepted images.
+ image_accepted_count:uint (id: 7);
}
root_type LocalizerOutput;