Merge pull request #17 from LakeEffectRobotics/faster_amp_glenn

Faster amp glenn > master
LakeEffectRobotics · Mar 21, 2024 · 22bd055 · 22bd055
2 parents 5fab191 + 359d435
commit 22bd055
Showing 1 changed file with 9 additions and 6 deletions.
diff --git 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
     );
   }
 }