Fixed "Drive to Amp" command

DevilBotz2876 · Mar 31, 2024 · 449dd6f · 449dd6f
1 parent e120cf9
commit 449dd6f
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 41 deletions.
diff --git a/src/main/java/frc/robot/controls/DebugControls.java b/src/main/java/frc/robot/controls/DebugControls.java
@@ -1,12 +1,9 @@
 package frc.robot.controls;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.GoalEndState;
 import com.pathplanner.lib.path.PathConstraints;
-import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
@@ -175,35 +172,12 @@ public static void setupControls() {
         .withPosition(colIndex, rowIndex++)
         .withSize(2, 1);
 
-    // Create a list of bezier points from poses. Each pose represents one waypoint.
-    // The rotation component of the pose should be the direction of travel. Do not use holonomic
-    // rotation.
-    List<Translation2d> bezierPoints =
-        PathPlannerPath.bezierFromPoses(
-            RobotConfig.drive.getPose(),
-            new Pose2d(1.8, 7.5, Rotation2d.fromDegrees(90)),
-            new Pose2d(1.8, 7.7, Rotation2d.fromDegrees(90)));
-
-    // Create the path using the bezier points created above
-    PathPlannerPath path =
-        new PathPlannerPath(
-            bezierPoints,
-            new PathConstraints(
-                1.0,
-                3.0,
-                2 * Math.PI,
-                4 * Math.PI), // The constraints for this path. If using a differential drivetrain,
-            // the angular constraints have no effect.
-            new GoalEndState(
-                0.0,
-                Rotation2d.fromDegrees(
-                    -90)) // Goal end state. You can set a holonomic rotation here. If using a
-            // differential drivetrain, the rotation will have no effect.
-            );
-
-    Command driveToBlueAmp = AutoBuilder.followPath(path);
-    driveToBlueAmp.setName("Drive To Amp");
-    debugTab.add(driveToBlueAmp).withPosition(colIndex, rowIndex++).withSize(2, 1);
+    Command driveToAmp =
+        AutoBuilder.pathfindToPoseFlipped(
+            new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(90)),
+            new PathConstraints(4.0, 3.0, 2 * Math.PI, 4 * Math.PI));
+    driveToAmp.setName("Drive To Amp");
+    debugTab.add(driveToAmp).withPosition(colIndex, rowIndex++).withSize(2, 1);
     ;
     /*
         colIndex += 2;

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java
@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
-import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.filter.SlewRateLimiter;
@@ -20,7 +19,6 @@
 import org.littletonrobotics.junction.Logger;
 import swervelib.SwerveDrive;
 import swervelib.SwerveDriveTest;
-import swervelib.parser.PIDFConfig;
 import swervelib.parser.SwerveParser;
 import swervelib.telemetry.SwerveDriveTelemetry;
 
@@ -55,9 +53,6 @@ public DriveSwerveYAGSL(String configPath) {
       throw new RuntimeException(e);
     }
 
-    PIDFConfig drivePIDF = swerveDrive.getModules()[0].getDrivePIDF();
-    PIDFConfig anglePIDF = swerveDrive.getModules()[0].getAnglePIDF();
-
     AutoBuilder.configureHolonomic(
         swerveDrive::getPose, // Robot pose supplier
         swerveDrive
@@ -66,10 +61,7 @@ public DriveSwerveYAGSL(String configPath) {
         swerveDrive::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
         swerveDrive::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE
         // ChassisSpeeds
-        new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in
-            // your Constants class
-            new PIDConstants(drivePIDF.p, drivePIDF.i, drivePIDF.d), // Translation PID constants
-            new PIDConstants(anglePIDF.p, anglePIDF.i, anglePIDF.d), // Rotation PID constants
+        new HolonomicPathFollowerConfig(
             swerveDrive.getMaximumVelocity(), // Max module speed, in m/s
             swerveDrive.swerveDriveConfiguration
                 .getDriveBaseRadiusMeters(), // Drive base radius in meters. Distance from robot