Viggo's fully function 20403 robot

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/ArmSubCmds.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.twenty403.commands;
 
 import com.technototes.library.command.Command;
-
 import org.firstinspires.ftc.twenty403.subsystems.ArmSubsystem;
 
 public class ArmSubCmds {
@@ -11,20 +10,25 @@ public static class cmds {
         public static Command slideZero(ArmSubsystem AS) {
             return Command.create(AS::setSlideToZero);
         }
+
         public static Command armIntake(ArmSubsystem AS) {
             return Command.create(AS::setArmToIntake);
         }
+
         public static Command highbasketSlide(ArmSubsystem AS) {
             return Command.create(AS::highBasketSlides);
         }
+
         public static Command highbasketArm(ArmSubsystem AS) {
             return Command.create(AS::highBasket);
         }
+
         public static Command lowbasketArm(ArmSubsystem AS) {
             return Command.create(AS::lowBasket);
         }
 
-
-
+        public static Command intakePos(ArmSubsystem AS) {
+            return Command.create(AS::setArmToIntake);
+        }
     }
 }

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/HighBasketCommand.java

@@ -22,7 +22,7 @@ public static SequentialCommandGroup HighBasket(Robot r) {
             KidShampooCmds.cmds.ScoopWrist(r.kidShampooSubsystem),
             ArmSubCmds.cmds.slideZero(r.armSubsystem),
             ArmSubCmds.cmds.highbasketArm(r.armSubsystem),
-            new WaitCommand(0.5),
+            new WaitCommand(1),
             ArmSubCmds.cmds.highbasketSlide(r.armSubsystem),
             new WaitCommand(1.2),
             HighBasketPreArm(r)

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/IntakePositionCommand.java

@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.twenty403.commands;
+
+import com.technototes.library.command.SequentialCommandGroup;
+import org.firstinspires.ftc.twenty403.Robot;
+
+public class IntakePositionCommand {
+
+    public static SequentialCommandGroup Intakepos(Robot r) {
+        return new SequentialCommandGroup(
+            KidShampooCmds.cmds.OpenRetainer(r.kidShampooSubsystem),
+            KidShampooCmds.cmds.StopIntake(r.kidShampooSubsystem),
+            KidShampooCmds.cmds.DumpWrist(r.kidShampooSubsystem),
+            ArmSubCmds.cmds.slideZero(r.armSubsystem),
+            ArmSubCmds.cmds.intakePos(r.armSubsystem),
+            KidShampooCmds.cmds.CloseRetainer(r.kidShampooSubsystem)
+        );
+    }
+}

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java

@@ -9,6 +9,7 @@
 import org.firstinspires.ftc.twenty403.Setup;
 import org.firstinspires.ftc.twenty403.commands.ArmSubCmds;
 import org.firstinspires.ftc.twenty403.commands.HighBasketCommand;
+import org.firstinspires.ftc.twenty403.commands.IntakePositionCommand;
 import org.firstinspires.ftc.twenty403.commands.JoystickIncDecCommand;
 import org.firstinspires.ftc.twenty403.commands.JoystickSlideIncDecCommand;
 import org.firstinspires.ftc.twenty403.commands.KidShampooCmds;
@@ -49,6 +50,7 @@ public class OperatorController {
     public CommandButton IntakeSpecimen;
     public CommandButton WristInc;
     public CommandButton WristDec;
+    public CommandButton IntakePos;
 
     public OperatorController(CommandGamepad g, Robot r) {
         robot = r;
@@ -70,7 +72,7 @@ private void AssignNamedControllerButton() {
         slideMax = gamepad.ps_share;
         slideMin = gamepad.ps_options;
         // suspend = gamepad.ps_circle;
-
+        IntakePos = gamepad.dpadDown;
         //        armLowNet = gamepad.dpadLeft;
         //        armLowSpecimen = gamepad.leftStickButton;
         //        armHighSpecimen = gamepad.rightStickButton;
@@ -142,7 +144,7 @@ public void bindArmControls() {
         HighBasket.whenPressed(HighBasketCommand.HighBasket(robot));
         //IntakeSample.whenPressed(IntakeSampleCommand.IntakeSample(robot));
         // IntakeSpecimen.whenPressed(IntakeSpecimenCommand.IntakeSpecimen(robot));*/
-
+        IntakePos.whenPressed(IntakePositionCommand.Intakepos(robot));
         armHorizontal.whenPressed(
             Command.create(robot.armSubsystem::horizontal, robot.armSubsystem)
         );

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java

@@ -37,6 +37,7 @@ public class ArmSubsystem implements Subsystem, Loggable {
     public static int ARM_VERTICAL = 3100;
     public static int ARM_HORIZONTAL = 1000;
     public static int INITIAL_POSITION = 150;
+    public static int INTAKE_POS;
 
     public static int INCREMENT_DECREMENT = 230;
     public static int SLIDE_INC_DEC = 250;
@@ -132,8 +133,9 @@ The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2)
              */
 
             (ticks, velocity) -> {
-                armFeedFwdValue =
-                    FEEDFORWARD_COEFFICIENT * Math.cos(getArmAngle(ticks)) * getSlideLength();
+                armFeedFwdValue = FEEDFORWARD_COEFFICIENT *
+                Math.cos(getArmAngle(ticks)) *
+                getSlideLength();
 
                 //                if (velocity > MIN_ANGULAR_VELOCITY) {
                 //                    //increase armFeedFwdValue to avoid slamming or increase D in PID
@@ -301,7 +303,7 @@ public void highSpecimenSlides() {
 
     //intake position
     public void setArmToIntake() {
-        setArmPos(INITIAL_POSITION);
+        setArmPos(INTAKE_POS);
     }
 
     @Override