Working?? stuck detection. Currently doesn't print or log anything wh…

…en it gets stuck. Anyone else wanna try and break it before I merge this branch and main?

Noticed an issue in sim that I will look into soon.

drivetrain/controlStrategies/autoDrive.py

@@ -10,6 +10,7 @@
 from navigation.navConstants import GOAL_PICKUP, GOAL_SPEAKER
 from drivetrain.drivetrainPhysical import MAX_DT_LINEAR_SPEED_MPS
 from utils.allianceTransformUtils import transform
+import math
 
 # Maximum speed that we'll attempt to path plan at. Needs to be at least 
 # slightly less than the maximum physical speed, so the robot can "catch up" 
@@ -27,13 +28,25 @@ def __init__(self):
         self._olCmd = DrivetrainCommand()
         self._prevCmd:DrivetrainCommand|None = None
         self._plannerDur:float = 0.0
+        self.autoPrevEnabled = False #This name might be a wee bit confusing. It just keeps track if we were in auto last refresh.
+        self.stuckTracker = 0 
+        self.prevPose = Pose2d()
+
 
         addLog("AutoDrive Proc Time", lambda:(self._plannerDur * 1000.0), "ms")
 
 
     def setRequest(self, toSpeaker, toPickup) -> None:
         self._toSpeaker = toSpeaker
         self._toPickup = toPickup
+        #The following if statement is just logic to enable self.autoPrevEnabled when the driver enables an auto.
+        if self._toSpeaker == True or self._toPickup == True:
+            if self.autoPrevEnabled == False:
+                self.stuckTracker = 0 
+            self.autoPrevEnabled = True
+        if self._toSpeaker == False and self._toPickup == False:
+            self.autoPrevEnabled = False
+
 
     def updateTelemetry(self) -> None:        
         self._telemTraj = self.rfp.getLookaheadTraj()
@@ -48,6 +61,7 @@ def isRunning(self)->bool:
         return self.rfp.goal != None
 
     def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand:
+
 
         startTime = Timer.getFPGATimestamp()
 
@@ -69,6 +83,13 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
         # If being asked to auto-align, use the command from the dynamic path planner
         if(self._toPickup or self._toSpeaker):
 
+            #This checks how much we moved, and if we moved less than one cm it increments the counter by one.
+            if math.sqrt(((curPose.X() - self.prevPose.X()) ** 2) + ((curPose.Y() - self.prevPose.Y()) ** 2)) < .01:
+                self.stuckTracker += 1
+            else:
+                if self.stuckTracker > 0:
+                    self.stuckTracker -= 1
+
             # Open Loop - Calculate the new desired pose and velocity to get there from the 
             # repulsor field path planner
             if(self._prevCmd is None or self._prevCmd.desPose is None):
@@ -89,4 +110,10 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
 
         self._plannerDur = Timer.getFPGATimestamp() - startTime
 
+        #Set our curPos as the new old pose
+        self.prevPose = curPose
+        #assume that we are either stuck or done if the counter reaches above 10. (sometimes it will get to like 4 when we are accelerating or taking a sharp turn)
+        if self.stuckTracker >= 10:
+            retCmd = cmdIn #set the returned cmd to the cmd that we were originally given.
+
         return retCmd

drivetrain/drivetrainCommand.py

@@ -12,4 +12,4 @@ class DrivetrainCommand:
     velX:float = 0.0  # Field X velocity in meters/sec
     velY:float = 0.0  # Field Y velocity in meters/sec
     velT:float = 0.0  # Rotational speed in rad/sec
-    desPose: Pose2d | None = None  # Current desired pose of the drivetrain, nor None if no pose is specified.
+    desPose: Pose2d | None = None  # Current desired pose of the drivetrain, or None if no pose is specified.

navigation/repulsorFieldPlanner.py

@@ -36,15 +36,7 @@
     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)),
+    PointObstacle(location=Translation2d(0, 0))
 ]
 
 # Fixed Obstacles - Outer walls of the field 
@@ -304,23 +296,10 @@ 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)
@@ -329,17 +308,9 @@ def _doLookahead(self, curPose):
                         # 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]:
         """