commit c63d0949159ff2ee0e215d6e7dd368af4ed6cdb4
author milind-u <milind.upadhyay@gmail.com> Fri Apr 15 12:07:42 2022 -0700
committer milind-u <milind.upadhyay@gmail.com> Fri Apr 15 12:07:42 2022 -0700
tree 8ea98cceec8ae35bb53971be9d7f9122f3ad0d3a
parent a2dba0be70009d961b315399c5d458bf114203f4

Speed up catapult

We got a new gear ratio.  Fix some state machine transitions as well to
let us reload the catapult while grabbing the next ball, and to fire
immediately too.

Change-Id: I930af58db609815d4fa639fa37b66caa011b6b94
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Signed-off-by: milind-u <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 cee13bf..6577bff 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -299,6 +299,12 @@
       frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
           *turret_loading_goal_buffer.fbb(), turret_loading_position));
 
+  const bool catapult_near_return_position =
+      (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
+       unsafe_goal->catapult()->has_return_position() &&
+       std::abs(unsafe_goal->catapult()->return_position()->unsafe_goal() -
+                catapult_.estimated_position()) < kCatapultGoalThreshold);
+
   const bool turret_near_goal =
       turret_goal != nullptr &&
       std::abs(turret_goal->unsafe_goal() - turret_.position()) <
@@ -354,8 +360,8 @@
 
       const bool turret_near_goal =
           std::abs(turret_.estimated_position() - turret_loading_position) <
-          kTurretGoalThreshold;
-      if (!turret_near_goal) {
+          kTurretGoalLoadingThreshold;
+      if (!turret_near_goal || !catapult_near_return_position) {
         break;  // Wait for turret to reach the chosen intake
       }
 
@@ -418,6 +424,7 @@
 
           // Reset opening timeout
           flipper_opening_start_time_ = timestamp;
+          loading_timer_ = timestamp;
         }
       }
       break;
@@ -486,15 +493,9 @@
         fire_ = true;
       }
 
-      const bool near_return_position =
-          (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
-           unsafe_goal->catapult()->has_return_position() &&
-           std::abs(unsafe_goal->catapult()->return_position()->unsafe_goal() -
-                    catapult_.estimated_position()) < kCatapultGoalThreshold);
-
       // Once the shot is complete and the catapult is back to its return
       // position, go back to IDLE
-      if (catapult_.shot_count() > prev_shot_count_ && near_return_position) {
+      if (catapult_.shot_count() > prev_shot_count_ ) {
         prev_shot_count_ = catapult_.shot_count();
         fire_ = false;
         discarding_ball_ = false;
@@ -509,7 +510,8 @@
       {.intake_front_position = intake_front_.estimated_position(),
        .intake_back_position = intake_back_.estimated_position(),
        .turret_position = turret_.estimated_position(),
-       .shooting = state_ == SuperstructureState::SHOOTING},
+       .shooting = (state_ == SuperstructureState::SHOOTING) ||
+                   !catapult_near_return_position},
       turret_goal);
 
   turret_.set_min_position(collision_avoidance_.min_turret_goal());