diff --git a/lib/trajectory/trajectory.dart b/lib/trajectory/trajectory.dart index 55820611..8ff8301f 100644 --- 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 index 024f00e1..92f2671f 100644 --- 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 index 7f3d3f64..089a02bc 100644 --- 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 index aa61f475..12ed45e8 100644 --- 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