equivalent python fixes

mjansen4857 · Oct 5, 2024 · c149aca · c149aca
1 parent 66441af
commit c149aca
Show file tree

Hide file tree

Showing 5 changed files with 8 additions and 12 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/events.py b/pathplannerlib-python/pathplannerlib/events.py
@@ -126,6 +126,7 @@ def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:
 
 class OneShotTriggerEvent(Event):
     _name: str
+    _resetCommand: Command
 
     def __init__(self, timestamp: float, name: str):
         """
@@ -136,19 +137,15 @@ def __init__(self, timestamp: float, name: str):
         """
         super().__init__(timestamp)
         self._name = name
+        self._resetCommand = cmd.waitSeconds(0).andThen(
+            cmd.runOnce(lambda: EventScheduler.setCondition(self._name, False))).ignoringDisable(True)
 
     @override
     def handleEvent(self, eventScheduler: 'EventScheduler') -> None:
+        EventScheduler.setCondition(self._name, True)
         # We schedule this command with the main command scheduler so that it is guaranteed to be run
         # in its entirety, since the EventScheduler could cancel this command before it finishes
-        CommandScheduler.getInstance().schedule(
-            cmd.sequence(
-                cmd.runOnce(lambda: EventScheduler.setCondition(self._name, True)),
-                cmd.waitSeconds(0),  # Wait for 0 seconds to delay until next loop
-                cmd.runOnce(lambda: EventScheduler.setCondition(self._name, False))
-            ).ignoringDisable(True)
-        )
-        eventScheduler.setCondition(self._name, False)
+        CommandScheduler.getInstance().schedule(self._resetCommand)
 
     @override
     def cancelEvent(self, eventScheduler: 'EventScheduler') -> None:

diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py
@@ -886,6 +886,7 @@ def _createPath(self) -> List[PathPoint]:
             distance = points[-1].position.distance(position)
             if distance <= 0.01:
                 pos = min(pos + targetIncrement, numSegments)
+                continue
 
             prevWaypointPos = pos - targetIncrement
 

diff --git a/pathplannerlib-python/pathplannerlib/pathfinders.py b/pathplannerlib-python/pathplannerlib/pathfinders.py
@@ -402,14 +402,14 @@ def _createWaypoints(self, path: List[GridPosition], real_start_pos: Translation
             anchor1 = ((current - last) * LocalADStar._SMOOTHING_ANCHOR_PCT) + last
             heading1 = (current - last).angle()
             anchor2 = ((current - next) * LocalADStar._SMOOTHING_ANCHOR_PCT) + next
-            heading2 = (anchor2 - next).angle()
+            heading2 = (next - anchor2).angle()
 
             pathPoses.append(Pose2d(anchor1, heading1))
             pathPoses.append(Pose2d(anchor2, heading2))
 
         pathPoses.append(Pose2d(
             fieldPosPath[-1],
-            (fieldPosPath[-2] - fieldPosPath[-1]).angle()
+            (fieldPosPath[-1] - fieldPosPath[-2]).angle()
         ))
 
         return PathPlannerPath.waypointsFromPoses(pathPoses)

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/LocalADStar.java
@@ -24,7 +24,6 @@
  */
 public class LocalADStar implements Pathfinder {
   private static final double SMOOTHING_ANCHOR_PCT = 0.8;
-  private static final double SMOOTHING_CONTROL_PCT = 0.33;
   private static final double EPS = 2.5;
 
   private double fieldLength = 16.54;

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/LocalADStar.h b/pathplannerlib/src/main/native/include/pathplanner/lib/pathfinding/LocalADStar.h
@@ -69,7 +69,6 @@ class LocalADStar: public Pathfinder {
 
 private:
 	const double SMOOTHING_ANCHOR_PCT = 0.8;
-	const double SMOOTHING_CONTROL_PCT = 0.33;
 	const double EPS = 2.5;
 
 	double fieldLength;