diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1332c365..b475beb9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.Constants.IndexerConstants.PieceState; import frc.robot.Constants.LEDsConstants.LED_STATES; import frc.robot.command.AlignToAmp; +import frc.robot.command.AmpAlign; import frc.robot.Constants.TunerConstants; import frc.robot.Constants.VisionConstants; import frc.robot.command.ChasePieces; @@ -196,7 +197,7 @@ protected void configureButtonBindings() { new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED)); - new Trigger(driver::getAButton).whileTrue(new AlignToAmp(AutonomousConstants.AMP_POSE, drivetrain)); + new Trigger(driver::getAButton).whileTrue(new AmpAlign(AutonomousConstants.AMP_POSE, drivetrain)); // Test auto align /* copilot */ diff --git a/src/main/java/frc/robot/command/AmpAlign.java b/src/main/java/frc/robot/command/AmpAlign.java index 5c08cb3e..94584967 100644 --- a/src/main/java/frc/robot/command/AmpAlign.java +++ b/src/main/java/frc/robot/command/AmpAlign.java @@ -4,18 +4,32 @@ package frc.robot.command; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.Swerve; +import frc.thunder.shuffleboard.LightningShuffleboard; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AmpAlign extends ParallelDeadlineGroup { +public class AmpAlign extends SequentialCommandGroup { /** Creates a new AmpAlign. */ - public AmpAlign() { - // Add the deadline command in the super() call. Add other commands using + public AmpAlign(Pose2d target, Swerve drivetrain) { // addCommands(). - super(new InstantCommand()); + //var roughMTP = new MoveToPose(target, drivetrain); + // var path = PathPlannerPath.fromPathFile("Example Path"); + //var followPath = AutoBuilder.followPath(path); + super(new MoveToPose(target, drivetrain), buildPath()); // addCommands(new FooCommand(), new BarCommand()); } + + private static Command buildPath() { + var path = PathPlannerPath.fromPathFile("Amp autoalign"); + var followPath = AutoBuilder.followPath(path); + // LightningShuffleboard.setBoolSupplier("AmpAlign", "path is running", () -> !followPath.isFinished()); + return followPath; + } }