[#29] sequential command

frc-862 · Mar 8, 2024 · d6be198 · d6be198
1 parent bdae52c
commit d6be198
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 8 deletions.
diff --git 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
@@ -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;
+  }
 }