commit ac4d88767f9e92e57d88f112abe49ef237f75da0
author Niko Sohmers <nikolai@sohmers.com> Fri Feb 23 13:55:47 2024 -0800
committer Niko Sohmers <nikolai@sohmers.com> Fri Feb 23 13:55:47 2024 -0800
tree 31a16451943a44659899c88a9cf083f765626535
parent 0b3f5f68208b99becc5b242dcd6270004e858d0a

Add turret to intake collision avoidance

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Idca062b4ff7ddca289581baf032736dadbf71e8c

y2024/control_loops/superstructure/shooter.cc

diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 0666772..a0dd234 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -4,6 +4,7 @@
 #include "aos/flatbuffers/base.h"
 #include "frc971/control_loops/aiming/aiming.h"
 #include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
+#include "y2024/control_loops/superstructure/collision_avoidance.h"
 
 namespace y2024::control_loops::superstructure {
 
@@ -43,6 +44,8 @@
     double *catapult_output, double *altitude_output, double *turret_output,
     double *retention_roller_output, double /*battery_voltage*/,
     aos::monotonic_clock::time_point current_timestamp,
+    CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
+    double *max_intake_pivot_position, double *min_intake_pivot_position,
     flatbuffers::FlatBufferBuilder *fbb) {
   superstructure_can_position_fetcher_.Fetch();
   drivetrain_status_fetcher_.Fetch();
@@ -146,9 +149,32 @@
        altitude_.estimated_position() >
            robot_constants_->common()->min_altitude_shooting_angle());
 
+  const bool disabled = turret_.Correct(turret_goal, position->turret(),
+                                        turret_output == nullptr);
+
+  collision_avoidance->UpdateGoal(
+      {.intake_pivot_position = intake_pivot_position,
+       .turret_position = turret_.estimated_position()},
+      turret_goal->unsafe_goal());
+
+  turret_.set_min_position(collision_avoidance->min_turret_goal());
+  turret_.set_max_position(collision_avoidance->max_turret_goal());
+
+  *max_intake_pivot_position = collision_avoidance->max_intake_pivot_goal();
+  *min_intake_pivot_position = collision_avoidance->min_intake_pivot_goal();
+
+  // Calculate the loops for a cycle.
+  const double voltage = turret_.UpdateController(disabled);
+
+  turret_.UpdateObserver(voltage);
+
+  // Write out all the voltages.
+  if (turret_output) {
+    *turret_output = voltage;
+  }
+
   const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
-      turret_status_offset =
-          turret_.Iterate(turret_goal, position->turret(), turret_output, fbb);
+      turret_status_offset = turret_.MakeStatus(fbb);
 
   const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
       altitude_status_offset = altitude_.Iterate(