commit 47e0d03b6c0a347d1257391d04fec2658ed88831
author Milind Upadhyay <milind.upadhyay@gmail.com> Sat Mar 05 23:06:25 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Mar 05 23:06:25 2022 -0800
tree dbed02642e908accab5454b46088ad32f8120442
parent f9a166e775b0ed606f3027d8c3f777d97c104718

Tune up the catapult state machine to work

This now holds the ball in the intake, and will transfer it when the
turret is in position.

Change-Id: I2e9aa11cae014b56679edaeb9f71a7c32664094c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index affc25b..013db30 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -162,8 +162,6 @@
       }
 
       state_ = SuperstructureState::TRANSFERRING;
-      transferring_timer_ = timestamp;
-
       // Save the side the ball is on for later
 
       break;
@@ -171,8 +169,9 @@
     case SuperstructureState::TRANSFERRING: {
       // If we've been transferring for too long, the ball probably got lost
       if (timestamp >
-          transferring_timer_ + constants::Values::kBallLostTime()) {
+          intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
         intake_state_ = IntakeState::NO_BALL;
+        state_ = SuperstructureState::IDLE;
         break;
       }
 
@@ -219,26 +218,13 @@
 
       // Keep feeding for kExtraLoadingTime
 
-      can_position_fetcher_.Fetch();
-      const bool flipper_arm_roller_is_stopped =
-          can_position_fetcher_.get() != nullptr &&
-          std::abs(
-              can_position_fetcher_->flipper_arm_integrated_sensor_velocity()) <
-              0.01;
-
-      const bool reading_is_recent =
-          can_position_fetcher_.get() != nullptr &&
-          (timestamp < can_position_fetcher_.context().monotonic_event_time +
-                           constants::Values::kFlipperVelocityValidTime());
-
       // 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.
-      // TODO(milind): maybe it's better to update loading_timer_ as long as the
-      // turret beambreak is triggered.
-      if (timestamp > loading_timer_ + constants::Values::kExtraLoadingTime() &&
-          !position->turret_beambreak() &&
-          (flipper_arm_roller_is_stopped || !reading_is_recent)) {
+      if (position->turret_beambreak()) {
+        loading_timer_ = timestamp;
+      } else if (timestamp >
+                 loading_timer_ + constants::Values::kExtraLoadingTime()) {
         state_ = SuperstructureState::LOADED;
         reseating_in_catapult_ = false;
       }