From 863ed549d4adf867bd42198389ade219a49d8503 Mon Sep 17 00:00:00 2001
From: POBots-353 <programming353@gmail.com>
Date: Mon, 29 Jan 2024 12:03:36 -0500
Subject: [PATCH] Use different drive constraints for OTF pathfinding

---
 src/main/java/frc/robot/Constants.java      |  5 +++++
 src/main/java/frc/robot/RobotContainer.java | 22 +++++++++++++--------
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6eb5784..e75390e 100644
--- 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
index b85f39e..8d1bdad 100644
--- 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() {