commit dd72800cfa18e4b70b5f7245317e78e9334cd128
author Nathan Leong <100028864@mvla.net> Sat Feb 03 15:26:53 2024 -0800
committer Nathan Leong <nathanrleong@gmail.com> Sat Feb 03 16:34:33 2024 -0700
tree 8131f9aa1cab294b4368e5520e4f9882880613b7
parent c7879b644c08dd8e29e296879e7cbd572e4add2f

Refactor year-agnostic catapult code to frc971

This breaks up the y2022 catapult code into multiple files and moves
them into the frc971 folder. Year-specific parameters are now
provided via the constructors, and the goal message is moved into
frc971 as well.

Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I4ea720ae62a7c6c229d6c24a1f08edd7bc5b9728

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index fc6aeee..dca0a96 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -25,7 +25,8 @@
       intake_front_(values_->intake_front.subsystem_params),
       intake_back_(values_->intake_back.subsystem_params),
       turret_(values_->turret.subsystem_params),
-      catapult_(*values_),
+      catapult_(values_->catapult.subsystem_params,
+                catapult::MakeCatapultPlant()),
       drivetrain_status_fetcher_(
           event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
               "/drivetrain")),
@@ -57,8 +58,11 @@
   aos::FlatbufferFixedAllocatorArray<
       frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
       turret_loading_goal_buffer;
-  aos::FlatbufferFixedAllocatorArray<CatapultGoal, 512> catapult_goal_buffer;
-  aos::FlatbufferFixedAllocatorArray<CatapultGoal, 512>
+  aos::FlatbufferFixedAllocatorArray<
+      frc971::control_loops::catapult::CatapultGoal, 512>
+      catapult_goal_buffer;
+  aos::FlatbufferFixedAllocatorArray<
+      frc971::control_loops::catapult::CatapultGoal, 512>
       catapult_discarding_goal_buffer;
 
   const aos::monotonic_clock::time_point timestamp =
@@ -107,7 +111,7 @@
 
   const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
       *turret_goal = nullptr;
-  const CatapultGoal *catapult_goal = nullptr;
+  const frc971::control_loops::catapult::CatapultGoal *catapult_goal = nullptr;
   double roller_speed_compensated_front = 0.0;
   double roller_speed_compensated_back = 0.0;
   double transfer_roller_speed = 0.0;
@@ -149,7 +153,8 @@
         return_position_offset = {aos::CopyFlatBuffer(
             unsafe_goal->catapult()->return_position(), catapult_goal_fbb)};
       }
-      CatapultGoal::Builder builder(*catapult_goal_fbb);
+      frc971::control_loops::catapult::CatapultGoal::Builder builder(
+          *catapult_goal_fbb);
       builder.add_shot_position(shot_params.shot_angle);
       builder.add_shot_velocity(shot_params.shot_velocity);
       if (return_position_offset.has_value()) {
@@ -170,7 +175,8 @@
         return_position_offset = {aos::CopyFlatBuffer(
             unsafe_goal->catapult()->return_position(), catapult_goal_fbb)};
       }
-      CatapultGoal::Builder builder(*catapult_goal_fbb);
+      frc971::control_loops::catapult::CatapultGoal::Builder builder(
+          *catapult_goal_fbb);
       builder.add_shot_position(kDiscardingPosition);
       builder.add_shot_velocity(kDiscardingVelocity);
       if (return_position_offset.has_value()) {
@@ -538,7 +544,7 @@
   // flippers
   const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
       catapult_status_offset = catapult_.Iterate(
-          catapult_goal, position, robot_state().voltage_battery(),
+          catapult_goal, position->catapult(), robot_state().voltage_battery(),
           output != nullptr && !catapult_.estopped()
               ? &(output_struct.catapult_voltage)
               : nullptr,