Adding rudimentary PathPlanner stuff

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

@@ -4,10 +4,13 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
@@ -18,11 +21,20 @@ public class RobotContainer {
   private CommandXboxController primary;
   private CommandXboxController secondary;
 
+  private SendableChooser<Command> autoChooser;
+
   public RobotContainer() {
     // TODO Need to provide a real initial pose
     // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
     drivetrain = new Drivetrain(new Pose2d(), null);
 
+    // An example Named Command, doesn't need to remain once we start actually adding real things
+    // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
+    NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
+
+    // TODO Specify a default auto string once we have one
+    autoChooser = AutoBuilder.buildAutoChooser();
+
     primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
     secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
@@ -60,6 +72,6 @@ public class RobotContainer {
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return autoChooser.getSelected();
   }
 }

src/main/java/frc/robot/constants/AutoConstants.java

@@ -1,6 +1,11 @@
 package frc.robot.constants;
 
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 
 public final class AutoConstants {
     public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6
@@ -15,6 +20,15 @@ public final class AutoConstants {
     public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
     public static final double kDHeadingController = 0.0025;
 
+    // TODO Is the robot module radius correct?
+    public static final HolonomicPathFollowerConfig kPFConfig = new HolonomicPathFollowerConfig(
+        new PIDConstants(kPXController), 
+        new PIDConstants(kPThetaController), 
+        kMaxSpeedMetersPerSecond, 
+        Units.inchesToMeters(36.77), 
+        new ReplanningConfig()
+    );
+
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);

src/main/java/frc/robot/constants/SwerveModuleConstants.java

@@ -2,7 +2,7 @@ package frc.robot.constants;
 
 import com.revrobotics.CANSparkBase.IdleMode;
 
-public class ModuleConstants {
+public class SwerveModuleConstants {
     // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T.
     // This changes the drive speed of the module (a pinion gear with more teeth will result in a
     // robot that drives faster).

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

@@ -8,14 +8,17 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import com.kauailabs.navx.frc.AHRS;
+import com.pathplanner.lib.auto.AutoBuilder;
+
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SPI;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
@@ -99,6 +102,22 @@ public class Drivetrain extends SubsystemBase {
         m_currentTranslationDir = 0.0;
         m_currentTranslationMag = 0.0;
         m_prevTime =  WPIUtilJNI.now() * 1e-6;
+
+        AutoBuilder.configureHolonomic(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getCurrentChassisSpeeds, 
+            this::driveWithChassisSpeeds, 
+            AutoConstants.kPFConfig, 
+            () -> {
+                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+            }, 
+            this
+        );
     }
 
     @Override
@@ -157,6 +176,15 @@ public class Drivetrain extends SubsystemBase {
         );
     }
 
+    public ChassisSpeeds getCurrentChassisSpeeds() {
+        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
+            m_frontLeft.getState(),
+            m_frontRight.getState(),
+            m_rearLeft.getState(),
+            m_rearRight.getState()
+        );
+    }
+
     /**
      * Method to drive the robot using joystick info.
      *
@@ -232,6 +260,15 @@ public class Drivetrain extends SubsystemBase {
         m_rearRight.setDesiredState(swerveModuleStates[3]);
     }
 
+    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
+        SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds);
+
+        m_frontLeft.setDesiredState(states[0]);
+        m_frontRight.setDesiredState(states[1]);
+        m_rearLeft.setDesiredState(states[2]);
+        m_rearRight.setDesiredState(states[3]);
+    }
+
     public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) {
         return run(() -> {
             drive(

vendordeps/PathplannerLib.json

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