(NON-FUNCTINAL WIP) Didn't work because I was operating the test to s…

…ee if the position didn't change in the path planning stage rather than the path executing stage. Hopefully easy fix?

navigation/repulsorFieldPlanner.py

@@ -10,6 +10,8 @@
 from navigation.forceGenerators import HorizontalObstacle, ForceGenerator, PointObstacle, VerticalObstacle
 from utils.constants import FIELD_X_M, FIELD_Y_M
 
+import math
+
 # Relative strength of how hard the goal pulls the robot toward it
 # Too big and the robot will be pulled through obstacles
 # Too small and the robot will get stuck on obstacles ("local minima")
@@ -34,6 +36,15 @@
     PointObstacle(location=Translation2d(11.0, 2.74)),
     PointObstacle(location=Translation2d(13.27, 4.07)),
     PointObstacle(location=Translation2d(11.0, 5.35)),
+    PointObstacle(location=Translation2d(0, 0)),
+    PointObstacle(location=Translation2d(10.0, 5.35)),
+    PointObstacle(location=Translation2d(10.5, 6.35)),
+    PointObstacle(location=Translation2d(10.5, 5.35)),
+    PointObstacle(location=Translation2d(10.0, 5.35)),
+    PointObstacle(location=Translation2d(10.0, 5.85)),
+    PointObstacle(location=Translation2d(11.0, 5.85)),
+    PointObstacle(location=Translation2d(11.0, 6.35)),
+    PointObstacle(location=Translation2d(10.0, 6.35)),
 ]
 
 # Fixed Obstacles - Outer walls of the field 
@@ -293,19 +304,42 @@ def _doLookahead(self, curPose):
         if(self.goal is not None):
             cp = curPose
             self.lookaheadTraj.append(cp)
+            self.stuckStrikes = 0
 
+            """
+            
+            Current Solution for detecting if we are stuck doesn't work. I think we can fix it by moving it to the function that calls this one and having that function test if the average change in position is bellow a threshhold for too many attempts
+            
+            """
             for _ in range(0,LOOKAHEAD_STEPS):
                 tmp = self._getCmd(cp, LOOKAHEAD_STEP_SIZE)
                 if(tmp.desPose is not None):
+                    """
+                    Check to see if the change in position between the current position and the position of the goal of the command from the lookahead is below a certain percentage of the step size
+                        if they are similar increment a "Strike" counter
+                    
+                    """
+                    if math.sqrt(((cp.X() - tmp.desPose.X() )** 2) + ((cp.Y() - tmp.desPose.Y())** 2) ) <= LOOKAHEAD_STEP_SIZE * .5: #If the total change in position is less than five percent of the lookahead step size
+                        self.stuckStrikes += 1
+
                     cp = tmp.desPose
                     self.lookaheadTraj.append(cp)
 
                     if((cp - self.goal).translation().norm() < LOOKAHEAD_STEP_SIZE):
                         # At the goal, no need to keep looking ahead
                         break
+
+                    """
+                    If we get 3 strikes (Or some arbitrary number of strikes or a percentage of the LOOKAHEAD_STEPS value) then we're 'out' and well say that its safe to assume that we are stuck
+                    """
+
+                    if self.stuckStrikes >= 3:
+                        break
+
                 else:
                     # Weird, no pose given back, give up.
                     break
+            print(self.stuckStrikes)
 
     def getLookaheadTraj(self) -> list[Pose2d]:
         """