WIP - the isStuck isn't working super great. It isn't actually stopping

RobotCasserole1736 · Oct 11, 2024 · 7506b58 · 7506b58
1 parent 4fe0d93
commit 7506b58
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py
@@ -18,14 +18,16 @@ class AutoDrive(metaclass=Singleton):
     def __init__(self):
         self._toSpeaker = False
         self._toPickup = False
+        self._isStuck = False
         self.rfp = RepulsorFieldPlanner()
         self._trajCtrl = HolonomicDriveController()
         self._telemTraj = []
         self._obsDet = ObstacleDetector()
 
-    def setRequest(self, toSpeaker, toPickup) -> None:
+    def setRequest(self, toSpeaker, toPickup, isStuck) -> None:
         self._toSpeaker = toSpeaker
         self._toPickup = toPickup
+        self._isStuck = isStuck
 
     def getTrajectory(self) -> Trajectory|None:
         return None # TODO
@@ -52,13 +54,14 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
         self.rfp._decay_observations()
 
         # Handle command changes
+
         if(self._toPickup):
             self.rfp.setGoal(transform(GOAL_PICKUP))
         elif(self._toSpeaker):
             self.rfp.setGoal(transform(GOAL_SPEAKER))
         elif(not self._toSpeaker and not self._toPickup):
             self.rfp.setGoal(None)
-
+        
         # If being asked to auto-align, use the command from the dynamic path planner
         if(self._toPickup or self._toSpeaker):
             olCmd = self.rfp.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR)
@@ -67,6 +70,8 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
             log("AutoDrive Rot Cmd", olCmd.velT, "radpers")
             if( olCmd.desPose is not None):
                 retCmd = self._trajCtrl.update2(olCmd.velX, olCmd.velY, olCmd.velT, olCmd.desPose, curPose)
-
+            if(self._isStuck):
+                olCmd = self.rfp.update(curPose,0)
+                retCmd = self._trajCtrl.update2(0,0,0,curPose,curPose)
 
         return retCmd
diff --git a/robot.py b/robot.py
@@ -129,7 +129,7 @@ def teleopPeriodic(self):
                 obs = PointObstacle(location=(ct+tf), strength=0.7)
                 self.autodrive.rfp.add_obstcale_observaton(obs)
 
-        self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
+        self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup(), self.autodrive.rfp.isStuck())
 
         # No trajectory in Teleop
         Trajectory().setCmd(None)