diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15afb32..8cc022a 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; @@ -38,6 +43,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(); @@ -48,6 +55,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(); @@ -89,6 +100,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 1ba174f..27d9e5d 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/AdvantageKit.json b/vendordeps/AdvantageKit.json index 868ae9d..b4e470d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.1", + "version": "26.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.1" + "version": "26.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.1", + "version": "26.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ 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 diff --git a/vendordeps/Phoenix6-26.1.1.json b/vendordeps/Phoenix6-26.1.2.json similarity index 92% rename from vendordeps/Phoenix6-26.1.1.json rename to vendordeps/Phoenix6-26.1.2.json index 7a0eca0..dcf2db8 100644 --- a/vendordeps/Phoenix6-26.1.1.json +++ b/vendordeps/Phoenix6-26.1.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.1.json", + "fileName": "Phoenix6-26.1.2.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.1", + "version": "26.1.2", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.1" + "version": "26.1.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index e8196c3..c6b1b9e 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.4", + "version": "2026.0.5", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.4" + "version": "2026.0.5" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.4", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.4", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.4", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.4", + "version": "2026.0.5", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.4", + "version": "2026.0.5", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.4", + "version": "2026.0.5", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.4", + "version": "2026.0.5", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true,