PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mode auto manuel

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-03-04 09:34:50 -05:00
parent 9f4142d7aa
commit b1124bd3f0
5 changed files with 70 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -49,14 +49,16 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
try {
var config = RobotConfig.fromGUISettings();
AutoBuilder.configure(
() -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
() -> getState().Speeds, // Supplier of current robot speeds
// Consumer of ChassisSpeeds and feedforwards to drive the robot
(speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(speeds)
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
),
new PPHolonomicDriveController(
// PID constants for translation
@@ -70,6 +72,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this // Subsystem for requirements
);
} catch (Exception ex) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());