Merge remote-tracking branch 'origin/chris_substeps' into main_nav

# Conflicts:
#	navigation/repulsorFieldPlanner.py

drivetrain/controlStrategies/autoDrive.py

@@ -18,11 +18,11 @@ 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()
+        self._prevCmd:DrivetrainCommand|None = None
 
     def setRequest(self, toSpeaker, toPickup) -> None:
         self._toSpeaker = toSpeaker
@@ -32,40 +32,52 @@ 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)
+
+            if(self._prevCmd is None or self._prevCmd.desPose is None):
+                olCmd = self.rfp.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR)
+            else:
+                olCmd = self.rfp.update(self._prevCmd.desPose, 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")
+
             if( olCmd.desPose is not None):
                 retCmd = self._trajCtrl.update2(olCmd.velX, olCmd.velY, olCmd.velT, olCmd.desPose, curPose)
+
+            self._prevCmd = retCmd
+        else:
+            self._prevCmd = None
 
 
         return retCmd

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

@@ -9,8 +9,6 @@
 from navigation.obstacles import HorizontalObstacle, Obstacle, PointObstacle, VerticalObstacle
 from navigation.navConstants import FIELD_X_M, FIELD_Y_M
 
-
-
 # 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")
@@ -58,12 +56,16 @@
     (0.0, 0.0)
 ])
 
-# These define how far in advance we attempt to plan for telemetry and stuck-detection purposes
+# These define how far in advance we attempt to plan for telemetry purposes
 # Increasing the distance causes us to look further ahead
-TELEM_LOOKAHEAD_DIST_M = 3.0
+LOOKAHEAD_DIST_M = 1.5
 # Increasing the number of steps increases the accuracy of our look-ahead prediction
 # but also increases CPU load on the RIO.
-TELEM_LOOKAHEAD_STEPS = 7
+LOOKAHEAD_STEPS = 4
+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:
     """
@@ -86,6 +88,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):
         """
@@ -117,7 +120,7 @@ def getGoalForce(self, curLocation:Translation2d) -> Force:
             # no goal, no force
             return Force()
 
-    def decay_observations(self):
+    def _decay_observations(self):
         """
         Transient obstacles decay in strength over time. 
         This function decays the obstacle's strength, and removes obstacles which have zero strength.
@@ -147,6 +150,19 @@ def getObstacleTransList(self) -> list[Translation2d]:
 
         return retArr
 
+    def isStuck(self) -> bool:
+        """
+        Based on current lookahead trajectory, see if we expect to make progress in the near future,
+        or if we're stuck in ... basically ... the same spot. IE, at a local minima
+        """
+        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:
         """
@@ -157,7 +173,7 @@ def atGoal(self, trans:Translation2d)->bool:
         else:
             return (self.goal.translation() - trans).norm() < GOAL_MARGIN_M
 
-    def getForceAtTrans(self, trans:Translation2d)->Force:
+    def _getForceAtTrans(self, trans:Translation2d)->Force:
         """
         Calculates the total force at a given X/Y point on the field.
         This is the sum of:
@@ -181,8 +197,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:
         """
         Given a starting pose, and a maximum step size to take, produce a drivetrain command which moves the robot in the best direction
         """
@@ -198,13 +218,22 @@ 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())
 
-                # Calculate a substep in the direction of the force
-                step = Translation2d(stepSize_m*netForce.unitX(), stepSize_m*netForce.unitY()) 
+                nextTrans = curTrans
+
+                for _ in range(4):
+
+                    if (nextTrans - curTrans).norm() >= stepSize_m:
+                        break
+
+                    netForce = self._getForceAtTrans(nextTrans)
+
+                    # Calculate a substep in the direction of the force
+                    step = Translation2d(stepSize_m*netForce.unitX()*0.5, stepSize_m*netForce.unitY()*0.5) 
+
+                    # Take that step
+                    nextTrans += step
 
-                # Take that substep
-                nextTrans = curTrans + step
 
                 # Assemble velocity commands based on the step we took
                 retVal.velX = (nextTrans - curTrans).X()/0.02 * slowFactor
@@ -213,28 +242,41 @@ 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]:
+    def _doLookahead(self, curPose):
         """
-        Perform all telemetry-related tasks.
-        This includes logging data, and performing the lookahead operation
-        Returns the list of Pose2D's that are the result of the lookahead operation
+        Perform the lookahead operation - Plan ahead out to a maximum distance, or until we detect we are stuck
         """
-        telemTraj = []
-        stepsize = TELEM_LOOKAHEAD_DIST_M/TELEM_LOOKAHEAD_STEPS
+        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]:
+        """
+        Perform all telemetry-related tasks.
+        This includes logging data
+        Returns the list of Pose2D's that are the result of the lookahead operation
+        """
         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