From 359d435def2dd48df020c12edcc2c7f9e28ea7db Mon Sep 17 00:00:00 2001 From: LER-Programming Date: Tue, 19 Mar 2024 21:47:27 -0400 Subject: [PATCH] made some steps parallel in ampcommandgroup --- .../java/frc/robot/commands/AmpCommandGroup.java | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/AmpCommandGroup.java b/src/main/java/frc/robot/commands/AmpCommandGroup.java index aa23fe1..bc133b5 100644 --- a/src/main/java/frc/robot/commands/AmpCommandGroup.java +++ b/src/main/java/frc/robot/commands/AmpCommandGroup.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Wrist; @@ -17,13 +18,15 @@ public class AmpCommandGroup extends SequentialCommandGroup { /** Creates a new IntakeCommandGroup. */ public AmpCommandGroup(Wrist wrist, Arm arm) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); + ParallelCommandGroup extendRotate = new ParallelCommandGroup( new ExtendArmCommand(arm, ArmExtension.RETRACT), + new RotateArmCommand(arm,ArmPosition.MIDDLE)); + + ParallelCommandGroup positionClaw = new ParallelCommandGroup( new WristCommand(wrist, WristPosition.AMP), + new RotateArmCommand(arm, ArmPosition.AMP)); + addCommands(new WristCommand(wrist, WristPosition.UP), - new ExtendArmCommand(arm, ArmExtension.RETRACT), - new RotateArmCommand(arm,ArmPosition.MIDDLE), - new WristCommand(wrist, WristPosition.AMP), - new RotateArmCommand(arm, ArmPosition.AMP) + extendRotate, + positionClaw ); } }