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

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

@@ -31,6 +31,7 @@ public class CometBotTeleOpDevelopment {
     public Gamepad currentGamepad2;
     public Gamepad previousGamepad1;
     public Gamepad previousGamepad2;
+    public boolean grabBlock;
 
 
     private Follower follower;
@@ -41,7 +42,7 @@ public class CometBotTeleOpDevelopment {
         arm = new ArmSubsystem(hardwareMap);
         wrist = new WristSubsystem(hardwareMap);
         skyhook = new HangMotorSubsystem(hardwareMap);
-
+        grabBlock = false;
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
         currentGamepad1 = new Gamepad();
@@ -153,7 +154,7 @@ public class CometBotTeleOpDevelopment {
     private void hang(){
         if (gamepad1.a) {
             claw.grabBlueberry();
-            arm.setPosition(0.15);
+            arm.setPosition(0.13);
             arm.grabBlueberrySkyhook();
 
             //claw Open small amount
@@ -163,35 +164,50 @@ public class CometBotTeleOpDevelopment {
         if(gamepad1.b) {
             //confirm grab
             claw.closeClaw();
-
+            grabBlock = true;
         }
 
-        if (gamepad1.x) {
+        if (gamepad1.x && grabBlock) {
             //now slap on bar, first wrist, then arm, then claw then driver must drive away
-            dualSlides.toFixedPosition(500);
+            dualSlides.toFixedPosition(300);
             dualSlides.update();
         }
         if (gamepad1.y) {
             arm.hangBlueberrySkyhook();
             wrist.hangBlueberrySkyhook();
             try {
-                Thread.sleep(500);
+                Thread.sleep(1500);
             } catch (InterruptedException e) {
                 throw new RuntimeException(e);
             }
-            dualSlides.toFixedPosition(1800);
+            dualSlides.toFixedPosition(2100);
             dualSlides.update();
         }
         if (gamepad1.right_bumper) {
             claw.openClaw();
-        }
-        if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
-            skyhook.hangRobot();
+            try {
+                Thread.sleep(1000);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            dualSlides.toFloorPosition();
+            claw.closeClaw();
+            arm.toParkPosition();
+            wrist.toFloorPosition();
         }
         skyhook.setPower(0);
-        if (gamepad1.dpad_down) {
+
+        /*if (gamepad1.dpad_down) {
             skyhook.disableMotor();
             skyhook.setPower(1);
+        }*/
+        if(gamepad1.right_trigger  > 0){
+            skyhook.setPower(true, 1);
+
+        }
+        if(gamepad1.left_trigger  > 0){
+            skyhook.setPower(false, 1);
+
         }
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -7,8 +7,6 @@ public class RobotConstants {
     public final static double clawClose = 1;
     public final static double clawOpen = 0.05;
 
-
-
     public final static double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
     public final static double armReverseBucket = 0.08;

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

@@ -55,5 +55,12 @@ public class HangMotorSubsystem {
         //write in limits for protection
         hang.setPower(Position);
     }
+    public void setPower(boolean forward, double power){
+        if(forward)
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        else if(!forward)
+            hang.setDirection(DcMotorSimple.Direction.REVERSE);
+        hang.setPower(power);
+    }
 
 }

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

@@ -88,6 +88,7 @@ public class HookTest extends OpMode {
         }
 
 
+
     }
 
 }