FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

PathPlanner Implementation

This commit is contained in:
Tyler Wilcox 2026-03-20 17:02:22 -04:00
parent e703a054fc
commit fe7d188314
4 changed files with 92 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

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

13
src/main/java/frc/robot/RobotContainer.java
View File

@ -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();
}
}

48
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -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();

38
vendordeps/PathplannerLib-2026.1.2.json Normal file
View File

@ -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"
]
}
]
}