From be5ac2ca7179e4b128a74a952fa7d7aee66e66ce Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Mon, 16 Feb 2026 20:23:14 -0800 Subject: [PATCH] Add new pose reset for initial paths --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/drive/DriveCommands.java | 8 +++++++- src/main/java/frc/robot/util/MapleSimUtil.java | 4 ++++ 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3738558..36619f4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -288,8 +288,8 @@ public RobotContainer() { autoChooser_ = new LoggedDashboardChooser<>("Auto Choices"); autoChooser_.onChange(auto -> { - System.err.println("Auto \"" + auto.getName() + "\" selected!"); - // This should be used to set up robot position setting, initialization, etc. + System.out.println("Auto \"" + auto.getName() + "\" selected!"); + // Anything you may want to do when the auto is selected. }); // Test Bindings diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 167fbcf..db9befe 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -62,6 +62,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.drive.Drive; +import frc.robot.util.MapleSimUtil; public class DriveCommands { private static final double kStoppedVelocity = 0.15 ; @@ -593,10 +594,15 @@ public static Command initialFollowPathCommand(Drive drive, String pathName, boo return Commands.none(); } + var startingPose = initPosePath.getStartingHolonomicPose().orElseThrow(); + return Commands.sequence( setPoseCommand( drive, - initPosePath.getStartingHolonomicPose().orElseThrow(), false), + startingPose, + false + ), + Commands.runOnce(() -> MapleSimUtil.placeRobotOnField(startingPose)), AutoBuilder.followPath(path.get())); } diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 4645c33..286406f 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -137,6 +137,10 @@ public static Pose2d getPosition() { return drivebaseSimulation.getSimulatedDriveTrainPose(); } + public static void placeRobotOnField(Pose2d pose) { + drivebaseSimulation.setSimulationWorldPose(pose); + } + public static ChassisSpeeds getFieldChassisSpeeds() { return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); }