Merge branch 'KylesStuckDetector' into main_nav

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,23 @@ def __init__(self):
         self._olCmd = DrivetrainCommand()
         self._prevCmd:DrivetrainCommand|None = None
         self._plannerDur:float = 0.0
+        self.autoSpeakerPrevEnabled = False #This name might be a wee bit confusing. It just keeps track if we were in auto targeting the speaker last refresh.
+        self.autoPickupPrevEnabled = False #This name might be a wee bit confusing. It just keeps track if we were in auto targeting the speaker 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.autoSpeakerPrevEnabled != self._toSpeaker or self.autoPickupPrevEnabled != self._toPickup:
+            self.stuckTracker = 0
+        self.autoPickupPrevEnabled = self._toPickup
+        self.autoSpeakerPrevEnabled = self._toSpeaker
+
 
     def updateTelemetry(self) -> None:        
         self._telemTraj = self.rfp.getLookaheadTraj()
@@ -48,6 +59,7 @@ def isRunning(self)->bool:
         return self.rfp.goal != None
 
     def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand:
+
 
         startTime = Timer.getFPGATimestamp()
 
@@ -68,25 +80,40 @@ 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):
-
-            # Open Loop - Calculate the new desired pose and velocity to get there from the 
-            # repulsor field path planner
-            if(self._prevCmd is None):
-                initCmd = DrivetrainCommand(0,0,0,curPose) # TODO - init this from current odometry vel
-                self._olCmd = self.rfp.update(initCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
-            else:
-                self._olCmd = self.rfp.update(self._prevCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
-
-            # Add closed loop - use the trajectory controller to add in additional 
-            # velocity if we're currently far away from the desired pose
-            retCmd = self._trajCtrl.update2(self._olCmd.velX, 
-                                            self._olCmd.velY, 
-                                            self._olCmd.velT, 
-                                            self._olCmd.desPose, curPose)    
-            self._prevCmd = retCmd
+            if self.stuckTracker < 10: #Only run if the robot isn't stuck
+                #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):
+                    initCmd = DrivetrainCommand(0,0,0,curPose) # TODO - init this from current odometry vel
+                    self._olCmd = self.rfp.update(initCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
+                else:
+                    self._olCmd = self.rfp.update(self._prevCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
+
+                # Add closed loop - use the trajectory controller to add in additional 
+                # velocity if we're currently far away from the desired pose
+                retCmd = self._trajCtrl.update2(self._olCmd.velX, 
+                                                self._olCmd.velY, 
+                                                self._olCmd.velT, 
+                                                self._olCmd.desPose, curPose)    
+                self._prevCmd = retCmd
         else:
             self._prevCmd = None
 
         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.
+            self._prevCmd = None
+
         return retCmd

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,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))
 ]
 
 # Fixed Obstacles - Outer walls of the field