Pickup macro, teleop cleaned up a lil bit

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
+import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2;
 import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE;
 import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK;
 import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
@@ -34,10 +35,11 @@ import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT;
 import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
@@ -67,6 +69,8 @@ public class Robot {
     @Getter
     private Slides slides;
 
+    GamepadEx controller2 = new GamepadEx(gamepad2);
+
     public Robot init(HardwareMap hardwareMap) {
         this.drive = new MecanumDrive(hardwareMap);
         this.hang = new Hang().init(hardwareMap);
@@ -237,29 +241,33 @@ public class Robot {
         public void wristScore() {
             this.wrist.setPosition(WRISTSCORE);
         }
+
     }
 
-    public static class Claw {
+    public static boolean clawIsOpen;
-        private static final double CLAW_KP = 0.15;
 
+    public static class Claw {
         private Servo claw;
 
         public Claw init(HardwareMap hardwareMap) {
             this.claw = hardwareMap.get(Servo.class, CLAW);
-            this.claw.setPosition(CLOSE);
+            close();
             return this;
         }
 
         public void close() {
             this.claw.setPosition(CLOSE);
+            clawIsOpen = false;
         }
 
         public void open() {
             this.claw.setPosition(OPEN);
+            clawIsOpen = true;
         }
 
         public void openStack() {
             this.claw.setPosition(BIGOPEN);
+            clawIsOpen = true;
         }
 
         public void setPos(double pos) {
@@ -276,6 +284,14 @@ public class Robot {
             return this;
         }
 
+        public void LED() {
+            if (clawIsOpen) {
+                white();
+            } else {
+                gold();
+            }
+        }
+
         public void gold() {
             this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_VIOLET);
         }
@@ -304,25 +320,45 @@ public class Robot {
 
     }
 
-    public void pickupMacro(Gamepad gamepad, Runtime runtime) {
+    public void pickupMacro(GamepadEx gamepadEx, double runtime) {
         switch (pickupMacroState) {
             case IDLE:
-                if (gamepad.dpad_down)
+                if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)){
+                    pickupMacroState = pickupMacroStates.OPEN;
+                }
                 break;
             case OPEN:
+                    delay = runtime + .2;
+                    this.getClaw().open();
+                    pickupMacroState = pickupMacroStates.DROP;
                 break;
             case DROP:
+                    if (runtime > delay) {
+                        this.getArm().pickup();
+                        delay= runtime + .2;
+                        pickupMacroState = pickupMacroStates.CLOSE;
+                    }
                 break;
             case CLOSE:
+                if (runtime > delay) {
+                    this.getClaw().close();
+                    delay= runtime + .1;
+                    pickupMacroState = pickupMacroStates.NEUTRAL;
+                }
                 break;
             case NEUTRAL:
+                if (runtime > delay) {
+                    this.getArm().armRest();
+                    pickupMacroState = pickupMacroStates.IDLE;
+                }
                 break;
 
         }
     }
 
-    pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
+    public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
-    enum pickupMacroStates{
+
+    public enum pickupMacroStates{
                 IDLE,
                 OPEN,
                 DROP,
@@ -331,9 +367,7 @@ public class Robot {
 
         }
 
-    double runtime;
+    double delay;
-
-
 
     public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
         this.drive.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
@@ -15,16 +17,11 @@ public class MainTeleOp extends OpMode {
         this.robot = new Robot().init(hardwareMap);
     }
 
-    @Override
+    GamepadEx controller2 = new GamepadEx(gamepad2);
+
     public void loop() {
         boolean slideUp = gamepad2.dpad_up;
-        boolean restArm = gamepad2.x;
-        boolean pickupArm = gamepad2.dpad_down;
-        boolean scoreArm = gamepad2.a;
         boolean plane = gamepad2.right_bumper;
-        boolean claw = gamepad2.b;
-        boolean pickupWrist = gamepad2.x;
-        boolean scoreWrist = gamepad2.a;
         boolean slideDown = gamepad2.dpad_left;
         boolean hang = gamepad2.left_bumper;
         boolean hangRelease = gamepad2.y;
@@ -39,27 +36,23 @@ public class MainTeleOp extends OpMode {
         } else {
             this.robot.getSlides().slideStop();
         }
-//Arm
+
-        if (pickupArm) {
+//Macros
-            this.robot.getArm().pickup();
+        this.robot.getLed().LED();
-        } else if (restArm) {
+        this.robot.pickupMacro(controller2,getRuntime());
-            this.robot.getArm().armRest();
+
-        } else if (scoreArm) {
+//Arm and Wrist
+        if (controller2.wasJustPressed(GamepadKeys.Button.X)){
             this.robot.getArm().armSecondaryScore();
+            this.robot.getWrist().wristScore();
+        } else if (controller2.wasJustPressed(GamepadKeys.Button.A)){
+            this.robot.getArm().armRest();
+            this.robot.getWrist().wristPickup();
+            this.robot.getClaw().close();
         }
 //Claw
-        if (claw) {
+        if (controller2.wasJustPressed(GamepadKeys.Button.B)){
             this.robot.getClaw().open();
-            this.robot.getLed().white();
-        } else {
-            this.robot.getClaw().close();
-            this.robot.getLed().gold();
-        }
-//Wrist
-        if (pickupWrist) {
-            this.robot.getWrist().wristPickup();
-        } else if (scoreWrist) {
-            this.robot.getWrist().wristScore();
         }
 //Hang
         if (hang) {
@@ -68,17 +61,18 @@ public class MainTeleOp extends OpMode {
             this.robot.getHang().hangRelease();
         } else if (hangPlane) {
             this.robot.getHang().hangPlane();
-        } else {
-            this.robot.getHang().hangIdle();
         }
+
         int Position = this.robot.getHang().hangRight.getCurrentPosition();
         telemetry.addData("position",(Position));
         telemetry.update();
+
 //Plane
         if (plane) {
             this.robot.getPlane().planeLaunch();
         }else {
             this.robot.getPlane().planeLock();
         }
+
     }
 }