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/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);