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) {

y2022/control_loops/superstructure/superstructure.h

diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 11cd38f..14fa8ab 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -94,6 +94,7 @@
   SuperstructureState state_ = SuperstructureState::IDLE;
   bool front_intake_has_ball_ = false;
   bool back_intake_has_ball_ = false;
+  std::optional<double> last_shot_angle_ = std::nullopt;
   RequestedIntake turret_intake_state_ = RequestedIntake::kFront;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);

y2022/joystick_reader.cc

diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 653e1c1..fe52db7 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -55,7 +55,7 @@
 const ButtonLocation kCatapultPos(4, 3);
 const ButtonLocation kFire(4, 1);
 const ButtonLocation kTurret(4, 15);
-const ButtonLocation kAutoAim(4, 2);
+const ButtonLocation kAutoAim(4, 16);
 
 const ButtonLocation kClimberExtend(4, 6);
 const ButtonLocation kClimberIntakes(4, 5);
@@ -173,7 +173,7 @@
     double roller_front_speed = 0.0;
     double roller_back_speed = 0.0;
 
-    std::optional<double> turret_pos = 0.0;
+    std::optional<double> turret_pos = std::nullopt;
 
     double climber_position = 0.01;
 
@@ -261,7 +261,6 @@
     if (data.IsPressed(kFire)) {
       fire = true;
       // Provide a default turret goal.
-      turret_pos = 0.0;
     }
 
     if (data.IsPressed(kClimberIntakes)) {