Fix superstructure ball lost checking
Before, we were only checking if a ball was lost when we were idle or
transferring. Handle the case where we loaded one ball, but intaked
another and lost it.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I0ac657d7b1a64c8a67fd3a4c2dad8c45c8b1e019
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index a8ae718..b9e0698 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -129,6 +129,11 @@
intake_beambreak_timer_ = timestamp;
}
+ if (timestamp >
+ intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
+ intake_state_ = IntakeState::NO_BALL;
+ }
+
if (intake_state_ != IntakeState::NO_BALL) {
// Block intaking in
roller_speed_compensated_front = 0.0;
@@ -152,11 +157,6 @@
switch (state_) {
case SuperstructureState::IDLE: {
- if (timestamp >
- intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
- intake_state_ = IntakeState::NO_BALL;
- }
-
if (is_spitting) {
intake_state_ = IntakeState::NO_BALL;
}
@@ -174,13 +174,6 @@
}
case SuperstructureState::TRANSFERRING: {
// If we've been transferring for too long, the ball probably got lost
- if (timestamp >
- intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
- intake_state_ = IntakeState::NO_BALL;
- state_ = SuperstructureState::IDLE;
- break;
- }
-
if (intake_state_ == IntakeState::NO_BALL) {
state_ = SuperstructureState::IDLE;
break;
@@ -225,8 +218,8 @@
// Keep feeding for kExtraLoadingTime
// The ball should go past the turret beambreak to be loaded.
- // If we got a CAN reading not too long ago, the flippers should have also
- // stopped.
+ // If we got a CAN reading not too long ago, the flippers should have
+ // also stopped.
if (position->turret_beambreak()) {
loading_timer_ = timestamp;
} else if (timestamp >
@@ -332,7 +325,8 @@
const flatbuffers::Offset<AimerStatus> aimer_offset =
aimer_.PopulateStatus(status->fbb());
- // Disable the catapult if we want to restart to prevent damage with flippers
+ // Disable the catapult if we want to restart to prevent damage with
+ // flippers
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
catapult_status_offset =
catapult_.Iterate(unsafe_goal, position,