refactor: path constraints are in constants

flamingchickens1540 · Feb 15, 2024 · 3518c81 · 3518c81
1 parent 84960a3
commit 3518c81
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 9 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -1,5 +1,6 @@
 package org.team1540.robot2024;
 
+import com.pathplanner.lib.path.PathConstraints;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -65,8 +66,15 @@ public static class Drivetrain {
         public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
         public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
         public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
+    }
 
-        public static final TrajectoryConfig trajectoryConfig = new TrajectoryConfig(MAX_LINEAR_SPEED, 8.269);
+    public static class Auto {
+        public static final double LINEAR_ACCEL_TIME_SECS = 4.0/3.0;
+        public static final double ANGULAR_ACCEL_TIME_SECS = 4.0/3.0;
+        public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(
+                Drivetrain.MAX_LINEAR_SPEED, Drivetrain.MAX_LINEAR_SPEED * LINEAR_ACCEL_TIME_SECS,
+                Constants.Drivetrain.MAX_ANGULAR_SPEED,
+                Constants.Drivetrain.MAX_ANGULAR_SPEED * ANGULAR_ACCEL_TIME_SECS);
     }
 
     public static class Indexer {

diff --git a/src/main/java/org/team1540/robot2024/util/PathHelper.java b/src/main/java/org/team1540/robot2024/util/PathHelper.java
@@ -1,7 +1,6 @@
 package org.team1540.robot2024.util;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.GeometryUtil;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -17,12 +16,6 @@
 import java.util.function.Supplier;
 
 public class PathHelper {
-    private static final PathConstraints constraints = new PathConstraints(
-            3.0, 4.0,
-            Constants.Drivetrain.MAX_ANGULAR_SPEED,
-            Constants.Drivetrain.MAX_ANGULAR_SPEED * (4.0/3.0)
-    );
-
     final boolean isResetting;
     final Pose2d initialPose;
     final String pathname;
@@ -73,7 +66,7 @@ public Command getCommand(Drivetrain drivetrain, boolean shouldRealign) {
         BooleanSupplier shouldFlip = () -> DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red;
         Supplier<Pose2d> startingPose = () -> shouldFlip.getAsBoolean() ? GeometryUtil.flipFieldPose(initialPose) : initialPose;
         Command command = new ConditionalCommand(
-                AutoBuilder.pathfindThenFollowPath(path, constraints),
+                AutoBuilder.pathfindThenFollowPath(path, Constants.Auto.PATH_CONSTRAINTS),
                 AutoBuilder.followPath(path),
                 () -> drivetrain.getPose().getTranslation().getDistance((startingPose.get()).getTranslation()) > 1 && shouldRealign); //TODO tune this distance
         Command resetCommand = new InstantCommand(() -> drivetrain.setPose(startingPose.get()));