Fixed an issue where the robot wouldn't recalculate it's position whe…

…n re-enabling auto drive.

Also corrected the code so it doesn't constantly calculate autodrive after assuming its stuck just to ignore the calculations.

drivetrain/controlStrategies/autoDrive.py

@@ -80,29 +80,30 @@ 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):
-                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
 
@@ -113,6 +114,6 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
         #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.stuckTracker += 1
+            self._prevCmd = None
 
         return retCmd