commit 28288cc139453f8740033f4bd43a1962c9fa5951
author Henry Speiser <henry@speiser.net> Wed Mar 09 22:59:24 2022 -0800
committer Henry Speiser <henry@speiser.net> Wed Mar 09 22:59:24 2022 -0800
tree c7583e170584777ad1b24433a4154d3e51a3a603
parent 8e2582bb4dfe257becac52e97063d02a1b3bbd1e

Add catapult to collision avoidance

Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: I504fbfc23d549f25c8e345818f7ee84f183508c6
Signed-off-by: Milind Upadhyay <milind.upadhyay@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 b9e0698..bb8d4d0 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -279,9 +279,15 @@
           turret_goal != nullptr &&
           std::abs(turret_goal->unsafe_goal() - turret_.position()) <
               kTurretGoalThreshold;
+      const bool collided = collision_avoidance_.IsCollided(
+          {.intake_front_position = intake_front_.estimated_position(),
+           .intake_back_position = intake_back_.estimated_position(),
+           .turret_position = turret_.estimated_position(),
+           .shooting = true});
 
-      // If the turret reached the aiming goal, fire!
-      if (flippers_open_ && turret_near_goal) {
+      // If the turret reached the aiming goal and the catapult is safe to move
+      // up, fire!
+      if (flippers_open_ && turret_near_goal && !collided) {
         fire_ = true;
       }
 
@@ -312,7 +318,8 @@
   collision_avoidance_.UpdateGoal(
       {.intake_front_position = intake_front_.estimated_position(),
        .intake_back_position = intake_back_.estimated_position(),
-       .turret_position = turret_.estimated_position()},
+       .turret_position = turret_.estimated_position(),
+       .shooting = state_ == SuperstructureState::SHOOTING},
       turret_goal);
 
   turret_.set_min_position(collision_avoidance_.min_turret_goal());