commit 027fd62f24fc1c49c447b4a600489ab54ade861b
author Austin Schuh <austin.linux@gmail.com> Fri Mar 01 21:26:07 2024 -0800
committer Austin Schuh <austin.linux@gmail.com> Fri Mar 01 22:41:38 2024 -0800
tree 289ed513f3227d38fef5f5bc2903b6fc1ec71462
parent 5006fc43b943b9fe0f8ef3fe2411d4f691aaa03f

Add extend collision avoidance

If extend wants to go up, move turret to 0 and lock it out.

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I13101a0c10d430436e765250b70a1257601680b2

y2024/control_loops/superstructure/shooter.cc

diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 07b7ce1..cc00454 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -40,9 +40,11 @@
     bool fire, double *catapult_output, double *altitude_output,
     double *turret_output, double *retention_roller_output,
     double *retention_roller_stator_current_limit, double /*battery_voltage*/,
-    CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
+    CollisionAvoidance *collision_avoidance, const double extend_position,
+    const double extend_goal, double *max_extend_position,
+    double *min_extend_position, const double intake_pivot_position,
     double *max_intake_pivot_position, double *min_intake_pivot_position,
-    bool standby, flatbuffers::FlatBufferBuilder *fbb,
+    flatbuffers::FlatBufferBuilder *fbb,
     aos::monotonic_clock::time_point monotonic_now) {
   drivetrain_status_fetcher_.Fetch();
 
@@ -94,18 +96,8 @@
 
   bool aiming = false;
 
-  if (standby) {
-    // Note is going to AMP or TRAP, move turret to extend
-    // collision avoidance position.
-    PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
-        turret_goal_builder.get(),
-        robot_constants_->common()->turret_avoid_extend_collision_position());
-
-    PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
-        altitude_goal_builder.get(),
-        robot_constants_->common()->altitude_loading_position());
-  } else if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
-             (!piece_loaded && state_ == CatapultState::READY)) {
+  if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
+      (!piece_loaded && state_ == CatapultState::READY)) {
     // We don't have the note so we should be ready to intake it.
     PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
         turret_goal_builder.get(),
@@ -138,7 +130,7 @@
 
   const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
       *turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
-                      !standby && shooter_goal->has_turret_position())
+                      shooter_goal->has_turret_position())
                          ? shooter_goal->turret_position()
                          : &turret_goal_builder->AsFlatbuffer();
 
@@ -161,8 +153,9 @@
 
   collision_avoidance->UpdateGoal(
       {.intake_pivot_position = intake_pivot_position,
-       .turret_position = turret_.estimated_position()},
-      turret_goal->unsafe_goal());
+       .turret_position = turret_.estimated_position(),
+       .extend_position = extend_position},
+      turret_goal->unsafe_goal(), extend_goal);
 
   turret_.set_min_position(collision_avoidance->min_turret_goal());
   turret_.set_max_position(collision_avoidance->max_turret_goal());
@@ -170,6 +163,9 @@
   *max_intake_pivot_position = collision_avoidance->max_intake_pivot_goal();
   *min_intake_pivot_position = collision_avoidance->min_intake_pivot_goal();
 
+  *max_extend_position = collision_avoidance->max_extend_goal();
+  *min_extend_position = collision_avoidance->min_extend_goal();
+
   // Calculate the loops for a cycle.
   const double voltage = turret_.UpdateController(disabled);