Use different drive constraints for OTF pathfinding

POBots-353 · Jan 29, 2024 · 863ed54 · 863ed54
1 parent 9c1bd12
commit 863ed54
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -380,6 +380,11 @@ public static final class SwerveConstants {
   public static final class AutoConstants {
     public static final PIDConstants translationConstants = new PIDConstants(5.0, 0.0, 0.0);
     public static final PIDConstants rotationConstants = new PIDConstants(4.5, 0.0, 0.0);
+
+    public static final double pathfindingMaxVelocity = Units.feetToMeters(13.0);
+    public static final double pathfindingMaxAcceleration = Units.feetToMeters(20.0);
+    public static final double pathfindingMaxAngularVelocity = Units.degreesToRadians(180.0);
+    public static final double pathfindingMaxAngularAcceleration = Units.degreesToRadians(180.0);
   }
 
   public static final class FrontLeftModule {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -25,6 +25,7 @@
 import frc.lib.controllers.VirtualJoystick;
 import frc.lib.controllers.VirtualXboxController;
 import frc.robot.Constants.ArmConstants;
+import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.Constants.ShooterConstants;
@@ -67,12 +68,12 @@ public class RobotContainer implements Logged {
   private Climber climber = new Climber();
   private LEDs leds = new LEDs();
 
-  private PathConstraints pathConstraints =
+  private final PathConstraints pathfindingConstraints =
       new PathConstraints(
-          SwerveConstants.maxTranslationalSpeed,
-          SwerveConstants.maxTranslationalAcceleration,
-          SwerveConstants.maxAngularSpeed,
-          SwerveConstants.maxAngularAcceleration);
+          AutoConstants.pathfindingMaxVelocity,
+          AutoConstants.pathfindingMaxAcceleration,
+          AutoConstants.pathfindingMaxAngularVelocity,
+          AutoConstants.pathfindingMaxAngularAcceleration);
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final VirtualXboxController driverController =
@@ -280,19 +281,24 @@ private void configureDriveBindings() {
         .x()
         .whileTrue(
             new ProxyCommand(
-                () -> AutoBuilder.pathfindToPose(swerve.getLeftChainPose(), pathConstraints)));
+                () ->
+                    AutoBuilder.pathfindToPose(swerve.getLeftChainPose(), pathfindingConstraints)));
 
     driverController
         .y()
         .whileTrue(
             new ProxyCommand(
-                () -> AutoBuilder.pathfindToPose(swerve.getCenterChainPose(), pathConstraints)));
+                () ->
+                    AutoBuilder.pathfindToPose(
+                        swerve.getCenterChainPose(), pathfindingConstraints)));
 
     driverController
         .b()
         .whileTrue(
             new ProxyCommand(
-                () -> AutoBuilder.pathfindToPose(swerve.getRightChainPose(), pathConstraints)));
+                () ->
+                    AutoBuilder.pathfindToPose(
+                        swerve.getRightChainPose(), pathfindingConstraints)));
   }
 
   private void configureIntakeBindings() {