commit 20aa8ae5414ad8e231b7af4b63f233f916845623
author Milind Upadhyay <milind.upadhyay@gmail.com> Mon Mar 07 08:37:46 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Fri Mar 11 17:11:44 2022 -0800
tree 7c623219774413d278e51cb190ff2ad6d3d072cd
parent 170521e54dd424c7afd50ed5d04e089e9eb596c6

Test 2022 superstructure starting off collided

Also, actually call Test() with negative angles.

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I5661147060098cdb69e8eef5332446bcdcc1d0c0

y2022/control_loops/superstructure/collision_avoidance_test.cc

diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index cd54fea..7c53be3 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -87,19 +87,19 @@
 
   void Simulate() {
     FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
-
-    // Don't simulate if already collided
-    if (avoidance_.IsCollided(status_)) {
-      return;
-    }
+    bool was_collided = avoidance_.IsCollided(status_);
 
     bool moving = true;
     while (moving) {
       // Compute the safe goal
       avoidance_.UpdateGoal(status_, unsafe_goal_.message().turret());
 
-      // The system should never be collided
-      ASSERT_FALSE(avoidance_.IsCollided(status_));
+      if (!was_collided) {
+        // The system should never be collided if it didn't start off collided
+        EXPECT_FALSE(avoidance_.IsCollided(status_));
+      } else {
+        was_collided = avoidance_.IsCollided(status_);
+      }
 
       safe_goal.mutable_message()->mutable_intake_front()->mutate_unsafe_goal(
           ::aos::Clip(intake_front_goal(), avoidance_.min_intake_front_goal(),
@@ -129,6 +129,8 @@
       }
     }
 
+    EXPECT_FALSE(avoidance_.IsCollided(status_));
+
     CheckGoals();
   }
 
@@ -336,8 +338,13 @@
            {TurretState::kSafeFront, TurretState::kSafeBack,
             TurretState::kSafeFrontWrapped, TurretState::kSafeBackWrapped,
             TurretState::kUnsafeFront, TurretState::kUnsafeBack,
-            TurretState::kUnsafeFrontWrapped,
-            TurretState::kUnsafeBackWrapped}) {
+            TurretState::kUnsafeFrontWrapped, TurretState::kUnsafeBackWrapped,
+            TurretState::kNegativeSafeFront, TurretState::kNegativeSafeBack,
+            TurretState::kNegativeSafeFrontWrapped,
+            TurretState::kNegativeSafeBackWrapped,
+            TurretState::kNegativeUnsafeFront, TurretState::kNegativeUnsafeBack,
+            TurretState::kNegativeUnsafeFrontWrapped,
+            TurretState::kNegativeUnsafeBackWrapped}) {
         // Intake front goal
         for (IntakeState intake_front_goal :
              {IntakeState::kSafe, IntakeState::kUnsafe}) {
@@ -350,7 +357,15 @@
                   TurretState::kSafeFrontWrapped, TurretState::kSafeBackWrapped,
                   TurretState::kUnsafeFront, TurretState::kUnsafeBack,
                   TurretState::kUnsafeFrontWrapped,
-                  TurretState::kUnsafeBackWrapped}) {
+                  TurretState::kUnsafeBackWrapped,
+                  TurretState::kNegativeSafeFront,
+                  TurretState::kNegativeSafeBack,
+                  TurretState::kNegativeSafeFrontWrapped,
+                  TurretState::kNegativeSafeBackWrapped,
+                  TurretState::kNegativeUnsafeFront,
+                  TurretState::kNegativeUnsafeBack,
+                  TurretState::kNegativeUnsafeFrontWrapped,
+                  TurretState::kNegativeUnsafeBackWrapped}) {
               // Catapult state
               for (CatapultState catapult_state :
                    {CatapultState::kIdle, CatapultState::kShooting}) {