it drives! too spinny but we can fix that!

Author: gerth2

drivetrain/controlStrategies/autoDrive.py

@@ -45,7 +45,7 @@ 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):
-            cmdIn = self._dpp.get()
+            retCmd = self._dpp.get()
 
         self._toSpeakerPrev = self._toSpeaker
         self._toPickupPrev = self._toPickup

navigation/dynamicPathPlanner.py

@@ -61,21 +61,26 @@ def get(self) -> DrivetrainCommand:
             # We'll sample the path before and after the current timestanp
             # and use finite differences to derive a drivetrain velocity command
             curTime = Timer.getFPGATimestamp() - self.trajStartTime_s
-            prevTime = max(0.0, curTime - 0.01)
-            nextTime = curTime + 0.01
-            deltaTime = nextTime - prevTime
-
-            prevSample = self.curTraj.sample(prevTime).pose
-            curSample = self.curTraj.sample(curTime).pose
-            nextSample = self.curTraj.sample(nextTime).pose
-
-            xVel = (nextSample.translation().X() - prevSample.translation().X())/deltaTime
-            yVel = (nextSample.translation().Y() - prevSample.translation().Y())/deltaTime
-            rotVel = (nextSample.rotation() - prevSample.rotation()).radians()/deltaTime
-
-            return DrivetrainCommand(
-                xVel,yVel,rotVel,curSample
-            )
+            if(curTime < self.curTraj.totalTime()):
+                prevTime = max(0.0, curTime - 0.01)
+                nextTime = curTime + 0.01
+                deltaTime = nextTime - prevTime
+
+                prevSample = self.curTraj.sample(prevTime).pose
+                curSample = self.curTraj.sample(curTime).pose
+                nextSample = self.curTraj.sample(nextTime).pose
+
+                xVel = (nextSample.translation().X() - prevSample.translation().X())/deltaTime
+                yVel = (nextSample.translation().Y() - prevSample.translation().Y())/deltaTime
+                rotVel = (nextSample.rotation() - prevSample.rotation()).radians()/deltaTime
+
+                return DrivetrainCommand(
+                    xVel,yVel,rotVel,curSample
+                )
+            else:
+                return DrivetrainCommand(
+                    0,0,0,self.curGoal.endPose
+                )
 
         else:
             # No path, no motion

navigation/gradientDescentCostMap.py

@@ -90,7 +90,10 @@ def _gradient_descent_on_function(self, start: tuple[int, int], step_size: float
         max_iter = int(math.sqrt(straightline_x ** 2 + straightline_y ** 2) / step_size * 20.0)
 
         path = []
-       
+
+        # First point is always part of the path
+        path.append(np.array([x, y]))
+
         for step in range(max_iter):
 
             # Constrain x/y to be on the field
@@ -128,12 +131,12 @@ def _gradient_descent_on_function(self, start: tuple[int, int], step_size: float
                 path.append(np.array([x_end, y_end]))
                 break
 
-            x, y = new_x, new_y
+            if step == max_iter - 1:
+                # If max iterations reached, return path without the final point
+                print("Max iterations reached. Stopping.")
+                break
 
-       
-        # If max iterations reached, return path without the final point
-        if step == max_iter - 1:
-            print("Max iterations reached. Stopping.")
+            x, y = new_x, new_y
 
         # Simplify the path
         return self._resample_path(path, 2.5)

simgui.json

@@ -73,6 +73,18 @@
           "selectable": false,
           "style": "Track"
         },
+        "desTrajWaypoints": {
+          "arrows": false,
+          "color": [
+            0.5709201693534851,
+            0.20304691791534424,
+            0.8284313678741455,
+            255.0
+          ],
+          "length": 0.20000000298023224,
+          "weight": 6.0,
+          "width": 0.20000000298023224
+        },
         "height": 8.210550308227539,
         "left": 150,
         "right": 2961,