Fix oscillating velocity for differential drive (#1052)

mjansen4857 · Feb 6, 2025 · ce94d1e · ce94d1e
1 parent 1027141
commit ce94d1e
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 8 deletions.
diff --git a/lib/trajectory/trajectory.dart b/lib/trajectory/trajectory.dart
@@ -219,7 +219,7 @@ class PathPlannerTrajectory {
       var accelStates =
           robotConfig.kinematics.toSwerveModuleStates(chassisAccel);
       for (int m = 0; m < numModules; m++) {
-        num moduleAcceleration = accelStates[m].speedMetersPerSecond;
+        num moduleAcceleration = accelStates[m].speedMetersPerSecond.abs();
 
         // Calculate the module velocity at the current state
         // vf^2 = v0^2 + 2ad
@@ -371,7 +371,7 @@ class PathPlannerTrajectory {
 
       // Use the robot accelerations to calculate how each module should decelerate
       for (int m = 0; m < numModules; m++) {
-        num moduleAcceleration = accelStates[m].speedMetersPerSecond;
+        num moduleAcceleration = accelStates[m].speedMetersPerSecond.abs();
 
         // Calculate the module velocity at the current state
         // vf^2 = v0^2 + 2ad

diff --git a/pathplannerlib-python/pathplannerlib/trajectory.py b/pathplannerlib-python/pathplannerlib/trajectory.py
@@ -564,7 +564,7 @@ def _forwardAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon
                                                              state.pose.rotation())
         accelStates = config.toSwerveModuleStates(chassisAccel)
         for m in range(config.numModules):
-            moduleAcceleration = accelStates[m].speed
+            moduleAcceleration = abs(accelStates[m].speed)
 
             # Calculate the module velocity at the current state
             # vf^2 = v0^2 + 2ad
@@ -669,7 +669,7 @@ def _reverseAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon
                                                              state.pose.rotation())
         accelStates = config.toSwerveModuleStates(chassisAccel)
         for m in range(config.numModules):
-            moduleAcceleration = accelStates[m].speed
+            moduleAcceleration = abs(accelStates[m].speed)
 
             # Calculate the module velocity at the current state
             # vf^2 = v0^2 + 2ad

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java
@@ -382,7 +382,7 @@ private static void forwardAccelPass(
               accelVec.getX(), accelVec.getY(), angularAccel, state.pose.getRotation());
       var accelStates = config.toSwerveModuleStates(chassisAccel);
       for (int m = 0; m < config.numModules; m++) {
-        double moduleAcceleration = accelStates[m].speedMetersPerSecond;
+        double moduleAcceleration = Math.abs(accelStates[m].speedMetersPerSecond);
 
         // Calculate the module velocity at the current state
         // vf^2 = v0^2 + 2ad
@@ -527,7 +527,7 @@ private static void reverseAccelPass(
               state.pose.getRotation());
       var accelStates = config.toSwerveModuleStates(chassisAccel);
       for (int m = 0; m < config.numModules; m++) {
-        double moduleAcceleration = accelStates[m].speedMetersPerSecond;
+        double moduleAcceleration = Math.abs(accelStates[m].speedMetersPerSecond);
 
         // Calculate the module velocity at the current state
         // vf^2 = v0^2 + 2ad

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp
@@ -420,7 +420,7 @@ void PathPlannerTrajectory::forwardAccelPass(
 		auto accelStates = config.toSwerveModuleStates(chassisAccel);
 		for (size_t m = 0; m < config.numModules; m++) {
 			units::meters_per_second_squared_t moduleAcceleration {
-					accelStates[m].speed() };
+					units::math::abs(accelStates[m].speed)() };
 
 			// Calculate the module velocity at the current state
 			// vf^2 = v0^2 + 2ad
@@ -580,7 +580,7 @@ void PathPlannerTrajectory::reverseAccelPass(
 		auto accelStates = config.toSwerveModuleStates(chassisAccel);
 		for (size_t m = 0; m < config.numModules; m++) {
 			units::meters_per_second_squared_t moduleAcceleration {
-					accelStates[m].speed() };
+					units::math::abs(accelStates[m].speed)() };
 
 			// Calculate the module velocity at the current state
 			// vf^2 = v0^2 + 2ad