From 90b4d66a3795b7359f413ac064880f1677f42492 Mon Sep 17 00:00:00 2001
From: MysticalApple <timon.cappellapimor@gmail.com>
Date: Wed, 3 Apr 2024 01:00:16 -0700
Subject: [PATCH] added automatic amp sequence

---
 .../robot/commands/auton/AutoAmpSequence.java | 33 +++++++++++++++++++
 1 file changed, 33 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/auton/AutoAmpSequence.java

diff --git a/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java b/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java
new file mode 100644
index 00000000..1d9817e5
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java
@@ -0,0 +1,33 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand;
+import frc.robot.commands.sequences.PrepareAmpSequence;
+import frc.robot.commands.swerve.AlignCommand;
+import frc.robot.subsystems.FieldManagementSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.intake.IntakePivotSubsystem;
+import frc.robot.subsystems.intake.IntakeRollerSubsystem;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+
+/** Automatically runs an amp cycle including preparing the note, going to and aligning with the amp, and depositing the
+ *  note (given that the robot already has a note). */
+public class AutoAmpSequence extends SequentialCommandGroup {
+    
+    /** Constructs a new {@link AutoAmpSequence}. */
+    public AutoAmpSequence(FieldManagementSubsystem fms,
+                           SwerveSubsystem swerveSubsystem,
+                           ElevatorSubsystem elevatorSubsystem,
+                           IntakePivotSubsystem intakePivotSubsystem,
+                           IntakeRollerSubsystem intakeRollerSubsystem) {
+
+        addCommands(
+            Commands.parallel(
+                new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem),
+                AlignCommand.getAmpAlignCommand(swerveSubsystem, fms.isRedAlliance())
+            ),
+            new IntakeRollerOuttakeCommand(intakeRollerSubsystem).withTimeout(1)
+        );
+    }
+}