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>
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());
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 48d07f1..edb3831 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -33,6 +33,7 @@
           ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
 
   static constexpr double kTurretGoalThreshold = 0.05;
+  static constexpr double kTurretGoalLoadingThreshold = 0.70;
   static constexpr double kCatapultGoalThreshold = 0.05;
   // potentiometer will be more noisy
   static constexpr double kFlipperGoalThreshold = 0.05;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index e18c2d9..fef165b 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -764,17 +764,26 @@
   SendRobotVelocity(1.0);
 
   constexpr double kTurretGoal = 2.0;
+  constexpr double kCatapultReturnPosition = -0.87;
   {
     auto builder = superstructure_goal_sender_.MakeBuilder();
     flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
         turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
             *builder.fbb(), kTurretGoal);
+    const auto catapult_return_offset =
+        CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), kCatapultReturnPosition);
+    auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
+    catapult_builder.add_return_position(catapult_return_offset);
+    const auto catapult_offset = catapult_builder.Finish();
+
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_roller_speed_front(12.0);
     goal_builder.add_roller_speed_back(12.0);
     goal_builder.add_roller_speed_compensation(0.0);
     goal_builder.add_turret(turret_offset);
     goal_builder.add_turret_intake(RequestedIntake::kFront);
+    goal_builder.add_catapult(catapult_offset);
     builder.CheckOk(builder.Send(goal_builder.Finish()));
   }
   RunFor(std::chrono::seconds(2));
@@ -924,8 +933,8 @@
             *builder.fbb(), kTurretGoal);
 
     const auto catapult_return_offset =
-        CreateStaticZeroingSingleDOFProfiledSubsystemGoal(*builder.fbb(),
-                                                          -0.87);
+        CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), kCatapultReturnPosition);
     auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
     catapult_builder.add_shot_position(0.3);
     catapult_builder.add_shot_velocity(15.0);
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index f8c3a7c..ec36dd4 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -32,8 +32,8 @@
       .setColor(RED)
       .setPointSize(4.0);
   positionPlot.addMessageLine(status, ['state'])
-      .setColor(CYAN)
-      .setPointSize(1.0);
+      .setColor(PINK)
+      .setPointSize(4.0);
   positionPlot.addMessageLine(status, ['flippers_open'])
       .setColor(WHITE)
       .setPointSize(1.0);
@@ -60,6 +60,9 @@
   intakePlot.addMessageLine(position, ['intake_beambreak_back'])
       .setColor(PINK)
       .setPointSize(1.0);
+  intakePlot.addMessageLine(output, ['transfer_roller_voltage'])
+      .setColor(BROWN)
+      .setPointSize(3.0);
 
 
   const otherPlot =
@@ -72,6 +75,9 @@
   otherPlot.addMessageLine(status, ['catapult', 'position'])
       .setColor(PINK)
       .setPointSize(4.0);
+  otherPlot.addMessageLine(status, ['turret', 'position'])
+      .setColor(WHITE)
+      .setPointSize(4.0);
   otherPlot.addMessageLine(position, ['flipper_arm_left', 'encoder'])
       .setColor(BLUE)
       .setPointSize(4.0);