From fe7d188314c312ac7c7b0bc40f11489bcf6ba3dc Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 20 Mar 2026 17:02:22 -0400 Subject: [PATCH] PathPlanner Implementation --- lib/RoboLib | 2 +- src/main/java/frc/robot/RobotContainer.java | 13 ++++- .../frc/robot/subsystems/DriveSubsystem.java | 48 ++++++++++++++++--- vendordeps/PathplannerLib-2026.1.2.json | 38 +++++++++++++++ 4 files changed, 92 insertions(+), 9 deletions(-) create mode 100644 vendordeps/PathplannerLib-2026.1.2.json diff --git a/lib/RoboLib b/lib/RoboLib index 915b14b..425745f 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit 915b14b4c7023240ac1e70d6af3558d38770153c +Subproject commit 425745f11cfde280f18c3e029f021f867139e119 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42d7e0a..7eeabe5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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 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(); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index c332dd2..20846d8 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/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; @@ -50,7 +55,7 @@ public class DriveSubsystem extends TestableSubsystem { .withSteerKI(steerI) .withSteerKD(steerD) .withSteerKV(steerKV) - //If the offsets don't work, contact Tyler W or Dom G + // If the offsets don't work, contact Tyler W or Dom G .withSteerOffset(Constants.SwerveDriveOffsets.FRONT_LEFT_MOTOR_OFFSET) .withSteerEncoderId(Constants.SwerveDriveIDs.FRONT_LEFT_ENCODER_ID) .withModuleLocation(new Translation2d(1, 1)) @@ -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 @@ -152,14 +183,17 @@ public class DriveSubsystem extends TestableSubsystem { } @PostTest() - public void DriveSubsystem_CanDevicesConnected(){ + public void DriveSubsystem_CanDevicesConnected() { 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].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(); + 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].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(); diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLib-2026.1.2.json new file mode 100644 index 0000000..f72fa41 --- /dev/null +++ b/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" + ] + } + ] +} \ No newline at end of file