Lookahead runs in simulation, but not on real robot

RobotCasserole1736 · Nov 15, 2024 · f1cc8fe · f1cc8fe
1 parent 91c152b
commit f1cc8fe
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 5 deletions.
diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py
@@ -82,10 +82,6 @@ def update(self, estPose:Pose2d, moduleAngles):
         self.field.getObject("curObstaclesFull").setPoses([Pose2d(x, Rotation2d()) for x in self.fullObstacles])
         self.field.getObject("curObstaclesThird").setPoses([Pose2d(x, Rotation2d()) for x in self.thirdObstacles])
         self.field.getObject("curObstaclesAlmostGone").setPoses([Pose2d(x, Rotation2d()) for x in self.almostGoneObstacles])
-        #print("Full: " + str(len(self.fullObstacles)))
-        #print("Third: " + str(len(self.thirdObstacles)))
-        #print("Almost gone: " + str(len(self.almostGoneObstacles)))
-
 
         self.field.getObject("visionObservations").setPoses(self.visionPoses)
         self.visionPoses = []

diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py
@@ -1,3 +1,4 @@
+from wpilib import TimedRobot
 from wpimath.geometry import Pose2d, Translation2d, Rotation2d
 
 from utils.mapLookup2d import MapLookup2D
@@ -261,7 +262,13 @@ def update(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainComman
         self.startSlowFactor = min(self.startSlowFactor, 1.0)
 
         nextCmd = self._getCmd(curCmd, stepSize_m)
-        # self._doLookahead(curCmd)
+
+        if(TimedRobot.isSimulation()):
+            # Lookahead is for telemetry and debug only, and is very
+            # time expensive. We'll do it in simulation, but not on the
+            # real robot.
+            self._doLookahead(curCmd)
+
         return nextCmd
 
     def _getCmd(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainCommand: