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/collision_avoidance.cc

diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index d87054a..0c13295 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -33,6 +33,25 @@
     return true;
   }
 
+  // If we aren't firing, no need to check the catapult
+  if (!status.shooting) {
+    return false;
+  }
+
+  // Checks if intake front is collided with catapult.
+  if (TurretCollided(
+          status.intake_front_position, status.turret_position + M_PI,
+          kMinCollisionZoneFrontTurret, kMaxCollisionZoneFrontTurret)) {
+    return true;
+  }
+
+  // Checks if intake back is collided with catapult.
+  if (TurretCollided(status.intake_back_position, status.turret_position + M_PI,
+                     kMinCollisionZoneBackTurret,
+                     kMaxCollisionZoneBackTurret)) {
+    return true;
+  }
+
   return false;
 }
 
@@ -96,20 +115,39 @@
   // Calculating the avoidance with either intake, and when the turret is
   // wrapped.
 
-  CalculateAvoidance(true, intake_front_position, turret_goal,
-                     kMinCollisionZoneFrontTurret, kMaxCollisionZoneFrontTurret,
-                     turret_position);
-  CalculateAvoidance(false, intake_back_position, turret_goal,
-                     kMinCollisionZoneBackTurret, kMaxCollisionZoneBackTurret,
-                     turret_position);
+  CalculateAvoidance(true, false, intake_front_position, turret_position,
+                     turret_goal, kMinCollisionZoneFrontTurret,
+                     kMaxCollisionZoneFrontTurret);
+  CalculateAvoidance(false, false, intake_back_position, turret_position,
+                     turret_goal, kMinCollisionZoneBackTurret,
+                     kMaxCollisionZoneBackTurret);
+
+  // If we aren't firing, no need to check the catapult
+  if (!status.shooting) {
+    return;
+  }
+
+  CalculateAvoidance(true, true, intake_front_position, turret_position,
+                     turret_goal, kMinCollisionZoneFrontTurret,
+                     kMaxCollisionZoneFrontTurret);
+  CalculateAvoidance(false, true, intake_back_position, turret_position,
+                     turret_goal, kMinCollisionZoneBackTurret,
+                     kMaxCollisionZoneBackTurret);
 }
 
-void CollisionAvoidance::CalculateAvoidance(bool intake_front,
+void CollisionAvoidance::CalculateAvoidance(bool intake_front, bool catapult,
                                             double intake_position,
+                                            double turret_position,
                                             double turret_goal,
                                             double min_turret_collision_goal,
-                                            double max_turret_collision_goal,
-                                            double turret_position) {
+                                            double max_turret_collision_goal) {
+  // If we are checking the catapult, offset the turret angle to represent where
+  // the catapult is
+  if (catapult) {
+    turret_position += M_PI;
+    turret_goal += M_PI;
+  }
+
   auto [turret_position_wrapped, turret_position_wraps] =
       WrapTurretAngle(turret_position);
 
@@ -161,12 +199,15 @@
     // If the intake is in the way, limit the turret until moved. Otherwise,
     // let'errip!
     if (!turret_pos_unsafe && (intake_position > kCollisionZoneIntake)) {
+      // If we were comparing the position of the catapult,
+      // remove that offset of pi to get the turret position
+      const double bounds_offset = (catapult ? -M_PI : 0);
       if (turret_position < min_turret_collision_goal_unwrapped) {
-        update_max_turret_goal(min_turret_collision_goal_unwrapped -
-                               kEpsTurret);
+        update_max_turret_goal(min_turret_collision_goal_unwrapped +
+                               bounds_offset - kEpsTurret);
       } else {
         update_min_turret_goal(max_turret_collision_goal_unwrapped +
-                               kEpsTurret);
+                               bounds_offset + kEpsTurret);
       }
     }
   }