Added teleop code

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java

@@ -60,7 +60,7 @@ public class CometBotAutoDevelopment {
         previousGamepad2 = new Gamepad();
         follower = new Follower(hardwareMap);
     }
-    
+
     public void init() {
         dualSlides.init();
         claw.init();
@@ -97,71 +97,75 @@ public class CometBotAutoDevelopment {
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
-    private void grabBlueberrySkyhook () {
+
-        if(currentGamepad1.x){
+    private void grabBlueberrySkyhook() {
-            claw.grabBlueberry();
+        if (currentGamepad1.x) {
-            wrist.grabBlueberrySkyhook();
+            //  claw.grabBlueberry();
-            arm.grabBlueberrySkyhook();
+            //wrist.grabBlueberrySkyhook();
+            //arm.grabBlueberrySkyhook();
         }
 
         /*if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
             claw.closeClaw();*/
     }
+
     private void hangSkyhook() {
         if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
             dualSlides.toHangHeight();
             //check wrist make go throught test files again
-            wrist.hangBlueberrySkyhook();
+            //   wrist.hangBlueberrySkyhook();
-            arm.hangBlueberrySkyhook();
+            // arm.hangBlueberrySkyhook();
-            if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) {
+//            if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) {
-                try {
+//                try {
-                    Thread.sleep(2000);
+//                    Thread.sleep(2000);
-                } catch (InterruptedException e) {
+//                } catch (InterruptedException e) {
-                    throw new RuntimeException(e);
+//                    throw new RuntimeException(e);
-                }
+//                }
-            }
+//            }
         }
-    }private void openClaw(){
+    }
-        if(currentGamepad2.right_bumper && !previousGamepad2.right_bumper){
+
+    private void openClaw() {
+        if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
             claw.switchState();
         }
     }
-    private void openThumb(){
+
-        if(currentGamepad2.left_bumper && !previousGamepad2.left_bumper){
+    private void openThumb() {
+        if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) {
             claw.switchTState();
         }
     }
-    private void armAndWristToFloor(){
+
-        if(currentGamepad2.a && !previousGamepad2.a){
+    private void armAndWristToFloor() {
-            double increment = 0.7 - arm.getPosition();
+        if (currentGamepad2.a && !previousGamepad2.a) {
-            for(int i = 0; i < 3; i ++){
+            arm.toFloorPositionTeleOp();
-                arm.setPosition(arm.getPosition() + increment);
+            if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
-                try {
+                wrist.toFloorPositionTeleop();
-                    Thread.sleep(50);
+            } else {
-                } catch (InterruptedException e) {
+                wrist.toPickupPosition();
-                    throw new RuntimeException(e);
-                }
             }
-            arm.toFloorPosition();
         }
-        if(currentGamepad2.a && !previousGamepad2.a ){
+
+        if (currentGamepad2.a && !previousGamepad2.a) {
             wrist.switchState();
         }
 
 
-
     }
-    private void armToBucketPosition(){
+
-        if(currentGamepad2.dpad_up && !previousGamepad2.dpad_up){
+    private void armToBucketPosition() {
+        if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) {
             arm.toBucketPosition();
             wrist.toBucketPosition();
 
         }
     }
-    private void armToParkPosition(){
+
-        if(currentGamepad2.x && !previousGamepad2.x){
+    private void armToParkPosition() {
+        if (currentGamepad2.x && !previousGamepad2.x) {
             arm.toParkPosition();
-            wrist.toFloorPosition();
+            wrist.toFloorPositionTeleop();
         }
     }
 
@@ -170,29 +174,34 @@ public class CometBotAutoDevelopment {
             dualSlides.toHighBucketPosition();
         }
     }
-    private void robotUp(){
+
-        if(currentGamepad1.dpad_up && !previousGamepad1.dpad_up){
+    private void robotUp() {
+        if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
             dualSlides.toHangBelowFloor();
             dualSlides.update();
             hang.hangRobot();
         }
 
     }
-    private void robotDown(){
+
-        if(currentGamepad1.dpad_down && !previousGamepad1.dpad_down){
+    private void robotDown() {
+        if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
             hang.robotToFloor();
         }
     }
-    private void stopHook(){
+
-        if(gamepad1.left_bumper){
+    private void stopHook() {
+        if (gamepad1.left_bumper) {
             hang.disableMotor();
         }
     }
+
     private void dualSlidesToFloorPosition() {
         if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
             dualSlides.toFloorPosition();
         }
     }
+
     private void dualSlidesToLowBucketPosition() {
         if (currentGamepad2.b && !previousGamepad2.b) {
             dualSlides.toLowBucketPosition();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java

@@ -54,6 +54,11 @@ public class CometBotTeleopCompetition {
      */
     public double currentPower = MAX_POWER;
 
+    /*
+    Misc
+     */
+    private int hbCounter = 0;
+
     public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
         this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
 
@@ -175,12 +180,38 @@ public class CometBotTeleopCompetition {
      */
     public void toHighBucketScore() {
         if (this.currentGP2.y && !this.previousGP2.y) {
-            lift.toHighBucketPosition();
+            switch(hbCounter) {
+                case 0:
+                    lift.toHighBucketPosition();
+                    hbCounter = 1;
+                    break;
+                case 1:
+                    wrist.toBucketPosition();
+                    arm.toBucketPosition();
+                    hbCounter = 2;
+                    break;
+                case 2:
+                    claw.openClaw();
+                    hbCounter = 3;
+                    break;
+                case 3:
+                    claw.closeClaw();
+                    wrist.toTravelPosition();
+                    arm.toParkPosition();
+                    hbCounter = 4;
+                    break;
+                case 4:
+                    lift.toFloorPosition();
+                    break;
+                default:
+                    break;
+            }
+            hbCounter = 1;
             if (lift.getFixedPosition() >= 4450) {
                 arm.toBucketPosition();
-                if(arm.getPosition() == 0.45)
+                if(arm.getPosition() == 0.45);
                     wrist.toBucketPosition();
-                    if(wrist.getPosition() == 0.56)
+                    if(wrist.getPosition() == 0.56);
                         claw.openClaw();
             }
         }