Added detection of getting stuck, and led to indicate state

drivetrain/controlStrategies/autoDrive.py

@@ -18,9 +18,8 @@ class AutoDrive(metaclass=Singleton):
     def __init__(self):
         self._toSpeaker = False
         self._toPickup = False
-        self._rfp = RepulsorFieldPlanner()
+        self.rfp = RepulsorFieldPlanner()
         self._trajCtrl = HolonomicDriveController()
-        self._curPose = Pose2d()
         self._telemTraj = []
         self._obsDet = ObstacleDetector()
 
@@ -32,35 +31,37 @@ def getTrajectory(self) -> Trajectory|None:
         return None # TODO
 
     def updateTelemetry(self) -> None:        
-        self._telemTraj = self._rfp.updateTelemetry(self._curPose)
+        self._telemTraj = self.rfp.updateTelemetry()
 
     def getWaypoints(self) -> list[Pose2d]:
         return self._telemTraj
 
     def getObstacles(self) -> list[Translation2d]:
-        return self._rfp.getObstacleTransList()
+        return self.rfp.getObstacleTransList()
+
+    def isRunning(self)->bool:
+        return self.rfp.goal != None
 
     def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand:
 
         retCmd = cmdIn # default - no auto driving
-        self._curPose = curPose
 
         for obs in self._obsDet.getObstacles(curPose):
-            self._rfp.add_obstcale_observaton(obs)
+            self.rfp.add_obstcale_observaton(obs)
 
-        self._rfp.decay_observations()
+        self.rfp._decay_observations()
 
         # Handle command changes
         if(self._toPickup):
-            self._rfp.setGoal(transform(GOAL_PICKUP))
+            self.rfp.setGoal(transform(GOAL_PICKUP))
         elif(self._toSpeaker):
-            self._rfp.setGoal(transform(GOAL_SPEAKER))
+            self.rfp.setGoal(transform(GOAL_SPEAKER))
         elif(not self._toSpeaker and not self._toPickup):
-            self._rfp.setGoal(None)
+            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.getCmd(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR)
+            olCmd = self.rfp.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR)
             log("AutoDrive FwdRev Cmd", olCmd.velX, "mps")
             log("AutoDrive Strafe Cmd", olCmd.velY, "mps")
             log("AutoDrive Rot Cmd", olCmd.velT, "radpers")

humanInterface/ledControl.py

@@ -0,0 +1,38 @@
+from wpilib import PWMMotorController
+from utils.constants import LED_STACK_LIGHT_CTRL_PWM
+from utils.singleton import Singleton
+from wpimath.filter import Debouncer
+
+BLINK = -1.0
+GREEN = 0.35
+RED = 0.03
+BLUE = 0.85
+OFF = 0.0
+
+class LEDControl(metaclass=Singleton):
+    def __init__(self):
+
+        self._isAutoDrive = False
+        self._isStuck = False
+        self.stuckDebounce = Debouncer(0.3, Debouncer.DebounceType.kFalling)
+        self.ctrlBlock = PWMMotorController("LEDCtrl", LED_STACK_LIGHT_CTRL_PWM)
+
+    def update(self):
+
+        stuckDebounced = self.stuckDebounce.calculate(self._isStuck)
+
+        if(self._isAutoDrive):
+            if(stuckDebounced):
+                pwmVal = RED * BLINK
+            else:
+                pwmVal = BLUE
+        else:
+            pwmVal = GREEN
+
+        self.ctrlBlock.set(pwmVal)
+
+    def setAutoDrive(self, isAutoDrive:bool):
+        self._isAutoDrive = isAutoDrive
+
+    def setStuck(self, isStuck:bool):
+        self._isStuck = isStuck

navigation/repulsorFieldPlanner.py

@@ -1,5 +1,3 @@
-from dataclasses import dataclass
-import math
 from wpimath.geometry import Pose2d, Translation2d
 from navigation.navConstants import FIELD_X_M, FIELD_Y_M
 
@@ -42,8 +40,12 @@
 ])
 
 # These define how far in advance we attempt to plan for telemetry purposes
-TELEM_LOOKAHEAD_DIST_M = 3.0
-TELEM_LOOKAHEAD_STEPS = 7
+LOOKAHEAD_DIST_M = 3.0
+LOOKAHEAD_STEPS = 7
+LOOKAHEAD_STEP_SIZE = LOOKAHEAD_DIST_M/LOOKAHEAD_STEPS
+
+# If the lookahead routine's end poit is within this, we declare ourselves stuck.
+STUCK_DIST = LOOKAHEAD_DIST_M/4
 
 class RepulsorFieldPlanner:
     def __init__(self):
@@ -53,6 +55,7 @@ def __init__(self):
         self.transientObstcales:list[Obstacle] = []
         self.distToGo:float = 0.0
         self.goal:Pose2d|None = None
+        self.lookaheadTraj:list[Pose2d] = []
 
     def setGoal(self, goal:Pose2d|None):
         self.goal = goal
@@ -70,7 +73,7 @@ def getGoalForce(self, curLocation:Translation2d) -> Force:
             # no goal, no force
             return Force()
 
-    def decay_observations(self):
+    def _decay_observations(self):
         # Linear decay of each transient obstacle observation
         for obs in self.transientObstcales:
             obs.strength -= 0.01
@@ -87,14 +90,23 @@ def getObstacleTransList(self) -> list[Translation2d]:
 
         return retArr
 
+    def isStuck(self) -> bool:
+        if(len(self.lookaheadTraj) > 3 and self.goal is not None):
+            start = self.lookaheadTraj[0]
+            end = self.lookaheadTraj[-1]
+            dist = (end-start).translation().norm()
+            distToGoal = (end - self.goal).translation().norm()
+            return dist < STUCK_DIST and distToGoal > LOOKAHEAD_DIST_M * 2
+        else:
+            return False
 
     def atGoal(self, trans:Translation2d)->bool:
         if(self.goal is None):
             return True
         else:
             return (self.goal.translation() - trans).norm() < GOAL_MARGIN_M
 
-    def getForceAtTrans(self, trans:Translation2d)->Force:
+    def _getForceAtTrans(self, trans:Translation2d)->Force:
 
         goalForce = self.getGoalForce(trans)
 
@@ -112,8 +124,12 @@ def getForceAtTrans(self, trans:Translation2d)->Force:
 
         return netForce
 
+    def update(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
+        curCmd = self._getCmd(curPose, stepSize_m)
+        self._doLookahead(curPose)
+        return curCmd
 
-    def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
+    def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
         retVal = DrivetrainCommand() # Default, no command
 
         if(self.goal is not None):
@@ -126,7 +142,7 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
                 # Slow down when we're near the goal
                 slowFactor = GOAL_SLOW_DOWN_MAP.lookup(self.distToGo)
 
-                netForce = self.getForceAtTrans(curPose.translation())
+                netForce = self._getForceAtTrans(curPose.translation())
 
                 # Calculate a substep in the direction of the force
                 step = Translation2d(stepSize_m*netForce.unitX(), stepSize_m*netForce.unitY()) 
@@ -141,23 +157,33 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
                 retVal.desPose = Pose2d(nextTrans, self.goal.rotation())
         else:
             self.distToGo = 0.0
-
+        
         return retVal
 
-    def updateTelemetry(self, curPose:Pose2d) -> list[Pose2d]:        
-        telemTraj = []
-        stepsize = TELEM_LOOKAHEAD_DIST_M/TELEM_LOOKAHEAD_STEPS
+    def _doLookahead(self, curPose):
+        self.lookaheadTraj = []
         if(self.goal is not None):
             cp = curPose
-            for _ in range(0,TELEM_LOOKAHEAD_STEPS):
-                telemTraj.append(cp)
-                tmp = self.getCmd(cp, stepsize)
+            self.lookaheadTraj.append(cp)
+
+            for _ in range(0,LOOKAHEAD_STEPS):
+                tmp = self._getCmd(cp, LOOKAHEAD_STEP_SIZE)
                 if(tmp.desPose is not None):
                     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
                 else:
+                    # Weird, no pose given back, give up.
                     break
 
+    def updateTelemetry(self) -> list[Pose2d]:        
         log("PotentialField Num Obstacles", len(self.fixedObstacles) + len(self.transientObstcales))
         log("PotentialField Path Active", self.goal is not None)
         log("PotentialField DistToGo", self.distToGo, "m")
-        return telemTraj
+        if(self.goal is not None):
+            return self.lookaheadTraj
+        else:
+            return []

robot.py

@@ -6,6 +6,7 @@
 from drivetrain.drivetrainCommand import DrivetrainCommand
 from drivetrain.drivetrainControl import DrivetrainControl
 from humanInterface.driverInterface import DriverInterface
+from humanInterface.ledControl import LEDControl
 from navigation.obstacles import PointObstacle
 from utils.segmentTimeTracker import SegmentTimeTracker
 from utils.signalLogging import SignalWrangler
@@ -39,6 +40,7 @@ def robotInit(self):
         self.stt = SegmentTimeTracker()      
 
         self.dInt = DriverInterface()
+        self.ledCtrl = LEDControl()
 
         self.autoSequencer = AutoSequencer()
 
@@ -68,6 +70,10 @@ def robotPeriodic(self):
         self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints())
         self.driveTrain.poseEst.telemetry.setCurObstacles(self.autodrive.getObstacles())
 
+        self.ledCtrl.setAutoDrive(self.autodrive.isRunning())
+        self.ledCtrl.setStuck(self.autodrive.rfp.isStuck())
+        self.ledCtrl.update()
+
         self.stt.end()
 
     #########################################################
@@ -111,22 +117,23 @@ def teleopPeriodic(self):
             # For test purposes, inject a series of obstacles around the current pose
             ct = self.driveTrain.poseEst.getCurEstPose().translation()
             tfs = [
-                Translation2d(3.0, -0.5),
-                Translation2d(3.0, 0.0),
-                Translation2d(-3.0, 0.5),
-                Translation2d(-3.0, 0.0)
+                Translation2d(1.7, -0.5),
+                Translation2d(0.75, -0.75),
+                Translation2d(1.7, 0.5),
+                Translation2d(0.75, 0.75),
+                Translation2d(2.0, 0.0),
+                Translation2d(0.0, 1.0),
+                Translation2d(0.0, -1.0),
             ]
             for tf in tfs:
                 obs = PointObstacle(location=(ct+tf), strength=0.7)
-                self.autodrive._rfp.add_obstcale_observaton(obs)
+                self.autodrive.rfp.add_obstcale_observaton(obs)
 
         self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
 
         # No trajectory in Teleop
         Trajectory().setCmd(None)
 
-
-
     #########################################################
     ## Disabled-Specific init and update
     def disabledPeriodic(self):

utils/constants.py

@@ -64,8 +64,8 @@
 # Unused = 5
 # Unused = 6
 # Unused = 7
-# LED_STRIPS_CTRL_PWM = 8
-LED_BLOCK_CTRL_PWM = 9
+# Unused = 8
+LED_STACK_LIGHT_CTRL_PWM = 9
 
 
 #######################################################################################

vectorPlotter.py

@@ -204,7 +204,7 @@ def clear_vectors(self):
     YCount = 0.1
     while YCount < 8.1:
         while XCount < 16.4:
-            force = rpp.getForceAtTrans(Translation2d(XCount, YCount))
+            force = rpp._getForceAtTrans(Translation2d(XCount, YCount))
             plotter.add_vector(XCount, YCount, force.x, force.y)
             XCount += .2
         YCount += .2