Merge remote-tracking branch 'origin/branch-swiss-cheese' into branch-swiss-cheese

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

@@ -97,60 +97,63 @@ 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) {
-            claw.grabBlueberry();
+            //  claw.grabBlueberry();
-            wrist.grabBlueberrySkyhook();
+            //wrist.grabBlueberrySkyhook();
-            arm.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(){
+    private void openClaw() {
         if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
             claw.switchState();
         }
     }
+
     private void openThumb() {
         if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) {
             claw.switchTState();
         }
     }
+
     private void armAndWristToFloor() {
         if (currentGamepad2.a && !previousGamepad2.a) {
-            double increment = 0.7 - arm.getPosition();
+            arm.toFloorPositionTeleOp();
-            for(int i = 0; i < 3; i ++){
+            if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
-                arm.setPosition(arm.getPosition() + increment);
+                wrist.toFloorPositionTeleop();
-                try {
+            } else {
-                    Thread.sleep(50);
+                wrist.toPickupPosition();
-                } catch (InterruptedException e) {
-                    throw new RuntimeException(e);
             }
         }
-            arm.toFloorPosition();
+
-        }
         if (currentGamepad2.a && !previousGamepad2.a) {
             wrist.switchState();
         }
 
 
-
     }
+
     private void armToBucketPosition() {
         if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) {
             arm.toBucketPosition();
@@ -158,10 +161,11 @@ public class CometBotAutoDevelopment {
 
         }
     }
+
     private void armToParkPosition() {
         if (currentGamepad2.x && !previousGamepad2.x) {
             arm.toParkPosition();
-            wrist.toFloorPosition();
+            wrist.toFloorPositionTeleop();
         }
     }
 
@@ -170,6 +174,7 @@ public class CometBotAutoDevelopment {
             dualSlides.toHighBucketPosition();
         }
     }
+
     private void robotUp() {
         if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
             dualSlides.toHangBelowFloor();
@@ -178,21 +183,25 @@ public class CometBotAutoDevelopment {
         }
 
     }
+
     private void robotDown() {
         if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
             hang.robotToFloor();
         }
     }
+
     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) {
+            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();
             }
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java

@@ -45,6 +45,10 @@ public class ArmSubsystem {
         arm.setPosition(armFloor);
         setState(ArmState.FLOOR);
     }
+    public void toFloorPositionTeleOp() {
+        arm.setPosition(.68);
+        setState(ArmState.FLOOR);
+    }
 
     public void toBucketPosition() {
         arm.setPosition(armBucket);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java

@@ -64,6 +64,11 @@ public class WristSubsystem {
         wrist.setPosition(wristFloor);
     }
 
+    public void toFloorPositionTeleop() {
+        setState(WristState.FLOOR);
+        wrist.setPosition(.425);
+    }
+
     public void toBucketPosition() {
         setState(WristState.BUCKET);
         wrist.setPosition(wristBucket);