commit 92908819e69d943efa1677f264355fd16b9f486d
author Austin Schuh <austin.linux@gmail.com> Sun Mar 27 14:14:05 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Mar 27 16:04:42 2022 -0700
tree e01a89925dfc2cb05a42c304943ba23d5c8e1b17
parent a441edbb50fdedcb89656111e9961b7f14571ea1

Make the turret go to the right intake to save time

We weren't latching which intake to go to.

Change-Id: I0e28a80838cd28ac2594d924a9c814b6ff3df8d0
Signed-off-by: Austin Schuh <austin.linux@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 0e71656..5dbe7e7 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -257,11 +257,12 @@
       }
       // When IDLE with no specific intake button pressed, allow the goal
       // message to override the intaking stuff.
-      if (have_active_intake_request || turret_goal == nullptr) {
+      if (have_active_intake_request || (turret_goal == nullptr)) {
         turret_goal = &turret_loading_goal_buffer.message();
       }
 
       if (!front_intake_has_ball_ && !back_intake_has_ball_) {
+        last_shot_angle_ = std::nullopt;
         break;
       }
 
@@ -330,6 +331,13 @@
     }
     case SuperstructureState::LOADED: {
       if (unsafe_goal != nullptr) {
+        if (turret_goal == nullptr) {
+          if (last_shot_angle_) {
+            turret_loading_goal_buffer.mutable_message()->mutate_unsafe_goal(
+                *last_shot_angle_);
+          }
+          turret_goal = &turret_loading_goal_buffer.message();
+        }
         if (unsafe_goal->cancel_shot()) {
           // Cancel the shot process
           state_ = SuperstructureState::IDLE;
@@ -345,6 +353,21 @@
       break;
     }
     case SuperstructureState::SHOOTING: {
+      if (turret_goal == nullptr) {
+        if (last_shot_angle_) {
+          turret_loading_goal_buffer.mutable_message()->mutate_unsafe_goal(
+              *last_shot_angle_);
+        }
+        turret_goal = &turret_loading_goal_buffer.message();
+        last_shot_angle_ = turret_goal->unsafe_goal();
+      } else {
+        last_shot_angle_ = std::nullopt;
+      }
+      const bool turret_near_goal =
+          turret_goal != nullptr &&
+          std::abs(turret_goal->unsafe_goal() - turret_.position()) <
+              kTurretGoalThreshold;
+
       // 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) {