Merge branch 'auto-testing' of https://github.com/FRC-Team-620/Warbot…

…s2024 into auto-testing

src/main/java/org/jmhsrobotics/frc2024/ComboCommands/AmpHelper.java

@@ -0,0 +1,27 @@
+package org.jmhsrobotics.frc2024.ComboCommands;
+
+import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.arm.commands.ArmSetAmpCommand;
+import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand;
+import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShootOpenLoopCommand;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AmpHelper extends SequentialCommandGroup {
+	private ArmPIDSubsystem armPIDSubsystem;
+	private ShooterSubsystem shooterSubsystem;
+	private IntakeSubsystem intakeSubsystem;
+
+	public AmpHelper(ArmPIDSubsystem armPIDSubsystemm, ShooterSubsystem shooterSubsystem,
+			IntakeSubsystem intakeSubsystem) {
+		this.intakeSubsystem = intakeSubsystem;
+		this.shooterSubsystem = shooterSubsystem;
+		this.armPIDSubsystem = armPIDSubsystem;
+
+		addCommands(new ArmSetAmpCommand(this.armPIDSubsystem), new ShootOpenLoopCommand(1, this.shooterSubsystem).withTimeout(0.5),
+						new IntakeCommand(0.8, this.intakeSubsystem, this.shooterSubsystem));
+	}
+}

src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java

@@ -4,6 +4,7 @@
 
 package org.jmhsrobotics.frc2024;
 
+import org.jmhsrobotics.frc2024.ComboCommands.AmpHelper;
 import org.jmhsrobotics.frc2024.autoCommands.FireCommand;
 import org.jmhsrobotics.frc2024.autoCommands.TurnAndShootCommand;
 import org.jmhsrobotics.frc2024.controlBoard.CompControl;
@@ -80,6 +81,8 @@ public RobotContainer() {
 		// TODO: test this combo command after two commands above work as intended(lift
 		// the arm first)
 		SmartDashboard.putData("FireCommand", new FireCommand(this.intakeSubsystem, this.shooterSubsystem));
+
+		SmartDashboard.putData("AmpHelper", new AmpHelper(this.armSubsystem, this.shooterSubsystem, this.intakeSubsystem));
 
 		// SmartDashboard.putData("LockAprilTagCommand", new LockAprilTag(7,
 		// this.driveSubsystem, this.visionSubsystem));