Integrate PathPlannerLib and add Example Auto

Author: AudreyZ19

gradlew

@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Example Auto.auto

@@ -0,0 +1,81 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.8394326015162217,
+        "y": 4.188891299550061
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6904589462008053,
+        "y": 4.940665233394327
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.78231056307781,
+        "y": 4.370352406171121
+      },
+      "prevControl": {
+        "x": 4.860483855805502,
+        "y": 5.074315409542372
+      },
+      "nextControl": {
+        "x": 7.141587777045881,
+        "y": 3.3323255596822756
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.085944870919574,
+        "y": 0.8637961970806275
+      },
+      "prevControl": {
+        "x": 6.4054472640776625,
+        "y": 0.7463117434053165
+      },
+      "nextControl": {
+        "x": 1.7664424777614869,
+        "y": 0.9812806507559387
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.8394326015162217,
+        "y": 4.188891299550061
+      },
+      "prevControl": {
+        "x": 2.3424992022139515,
+        "y": 3.371763469412239
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Example Path.path

@@ -4,6 +4,11 @@
 
 package frc.robot;
 
+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.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -33,6 +38,20 @@ public class RobotContainer {
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     m_driveTrain.setDefaultCommand(new DriveUsingController(m_driveTrain, m_driverController));
+
+    AutoBuilder.configureHolonomic(
+          m_driveTrain::getPosition,
+          m_driveTrain::resetPosition,
+          m_driveTrain::getChassisSpeeds,
+          m_driveTrain::setChassisSpeeds,
+          new HolonomicPathFollowerConfig(
+            new PIDConstants(1.0, 0, 0), 
+            new PIDConstants(1.0,0,0),
+            m_driveTrain.getMaxSpeed(),
+            m_driveTrain.getWheelBaseRadius(),
+            new ReplanningConfig()
+          ),
+          m_driveTrain);
 
     // Configure the trigger bindings
     configureBindings();

src/main/java/frc/robot/RobotContainer.java

@@ -5,13 +5,16 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.ExampleSubsystem;
+
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 
 public final class Autos {
   /** Example static factory for an autonomous command. */
   public static Command exampleAuto(ExampleSubsystem subsystem) {
-    return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem));
+    return new PathPlannerAuto("Example Auto");
   }
 
   private Autos() {

src/main/java/frc/robot/commands/Autos.java

@@ -261,6 +261,7 @@ public double getMaxRotationalSpeed() {
     return this.maxRotationalSpeed;
   }
 
+
   /**
    * Returns the maximum rotational acceleration of the robot in rad/s^2.
    * 
@@ -270,6 +271,18 @@ public double getMaxRotationalAcceleration() {
     return this.maxRotationalAcceleration;
   }
 
+  /**
+   * Return the wheel base radius in meters.
+   * 
+   * @return The wheel base radius in meters.
+   */
+  public double getWheelBaseRadius(){
+    double halfWheelDistanceX = wheelDistanceX/2;
+    double halfWheelDistanceY = wheelDistanceY/2;
+
+    return Math.sqrt(halfWheelDistanceX*halfWheelDistanceX +halfWheelDistanceY*halfWheelDistanceY);
+  }
+
   /**
    * Returns a {@link TrapezoidProfile.Constraints} object used to enforce
    * velocity and acceleration constraints on the {@link ProfilePIDController}

src/main/java/frc/robot/parameters/SwerveDriveParameters.java

@@ -286,6 +286,15 @@ public TrapezoidProfile.Constraints getRotationalConstraints() {
     return PARAMETERS.getRotationalConstraints();
   }
 
+   /**
+   * Return the wheel base radius in meters.
+   * 
+   * @return The wheel base radius in meters.
+   */
+  public double getWheelBaseRadius(){
+    return PARAMETERS.getWheelBaseRadius();
+  }
+
   /**
    * Creates a HolonomicDriveController for the subsystem.
    *