PathPlanner Implementation

lib/RoboLib

@@ -1 +1 @@
-Subproject commit 915b14b4c7023240ac1e70d6af3558d38770153c
+Subproject commit 425745f11cfde280f18c3e029f021f867139e119

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

@@ -4,8 +4,13 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.AutoBuilderException;
+
 import edu.wpi.first.units.Unit;
 import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -37,6 +42,8 @@ public class RobotContainer {
 
   // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
 
+  private final SendableChooser<Command> autoChooser;
+
   public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
 
   public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
@@ -47,6 +54,10 @@ public class RobotContainer {
   
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+
     // Configure the trigger bindings
     configureBindings();
 
@@ -82,6 +93,6 @@ public class RobotContainer {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return Autos.exampleAuto(m_exampleSubsystem);
+    return autoChooser.getSelected();
   }
 }

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -9,8 +9,13 @@ import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
 import org.littletonrobotics.junction.Logger;
 
 import com.fasterxml.jackson.annotation.JsonCreator.Mode;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
@@ -132,11 +137,37 @@ public class DriveSubsystem extends TestableSubsystem {
 
                 Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
                 Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
-                Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose());
+                Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());
             }
 
         }, this));
 
+        RobotConfig config;
+        try {
+            config = RobotConfig.fromGUISettings();
+
+            AutoBuilder.configure(
+                    driveSystem::getRobotPose2d,
+                    driveSystem::setPose2d,
+                    driveSystem::getRobotRelativeSpeeds,
+                    (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
+                    new PPHolonomicDriveController(
+                            new PIDConstants(5.0, 0, 0),
+                            new PIDConstants(5.0, 0, 0)),
+                    config,
+                    () -> {
+                        var alliance = DriverStation.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == DriverStation.Alliance.Red;
+                        }
+                        return false;
+                    },
+                    this);
+
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+
     }
 
     @Override
@@ -157,9 +188,12 @@ public class DriveSubsystem extends TestableSubsystem {
         SwerveModule[] modules = driveSystem.getModules();
 
         for (int i = 0; i < modules.length; i++) {
-            assertThat(modules[i].getDriveMotor().isConnected()).as(modules[i].getModuleName() + " drive motor is not connected").isTrue();
+            assertThat(modules[i].getDriveMotor().isConnected())
-            assertThat(modules[i].getSteerMotor().isConnected()).as(modules[i].getModuleName() + " steer motor is not connected").isTrue();
+                    .as(modules[i].getModuleName() + " drive motor is not connected").isTrue();
-            assertThat(modules[i].getSteerEncoder().isConnected()).as(modules[i].getModuleName() + " CANCoder is not connected").isTrue();
+            assertThat(modules[i].getSteerMotor().isConnected())
+                    .as(modules[i].getModuleName() + " steer motor is not connected").isTrue();
+            assertThat(modules[i].getSteerEncoder().isConnected())
+                    .as(modules[i].getModuleName() + " CANCoder is not connected").isTrue();
         }
 
         assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue();

vendordeps/PathplannerLib-2026.1.2.json

@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib-2026.1.2.json",
+    "name": "PathplannerLib",
+    "version": "2026.1.2",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2026",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2026.1.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2026.1.2",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}