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

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

@@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.states.FieldStates;
 import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
 
@@ -30,6 +31,7 @@ public class CometBotAutoDevelopment {
     private ClawSubsystem claw;
     private WristSubsystem wrist;
     private ArmSubsystem arm;
+    private HangMotorSubsystem hang;
     /*
     Controllers
      */
@@ -48,6 +50,7 @@ public class CometBotAutoDevelopment {
         claw = new ClawSubsystem(hardwareMap);
         arm = new ArmSubsystem(hardwareMap);
         wrist = new WristSubsystem(hardwareMap);
+        hang = new HangMotorSubsystem(hardwareMap);
 
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
@@ -87,7 +90,10 @@ public class CometBotAutoDevelopment {
         openThumb();
         //hang
         grabBlueberrySkyhook();
-
+        hangSkyhook();
+        robotDown();
+        robotUp();
+        stopHook();
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
@@ -97,10 +103,26 @@ public class CometBotAutoDevelopment {
             wrist.grabBlueberrySkyhook();
             arm.grabBlueberrySkyhook();
         }
-        if(currentGamepad1.y){
+        if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
-            wrist.hangBlueberrySkyhook();
+            claw.init();
-            arm.hangBlueberrySkyhook();
+    }
+    private void hangSkyhook (){
+        dualSlides.toHangHeight();
+        wrist.hangBlueberrySkyhook();
+        try {
+            Thread.sleep(1000);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
         }
+        arm.hangBlueberrySkyhook();
+        if(arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK){
+            try {
+                Thread.sleep(1000);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }claw.closeClaw();
+        }
+
     }
 
     private void openClaw(){
@@ -155,12 +177,20 @@ public class CometBotAutoDevelopment {
     }
     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){
-
+            hang.robotToFloor();
+        }
+    }
+    private void stopHook(){
+        if(gamepad1.left_bumper){
+            hang.disableMotor();
         }
     }
     private void dualSlidesToFloorPosition() {

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

@@ -12,11 +12,13 @@ public class RobotConstants {
     public final static double armReverseBucket = 0.08;
     public final static double armPark = 0.33;
     //value for grabbing the hook Specimen
-    public final static double grabBlueberry = 0.3;
+    public final static double grabBlueberry = 0.35;
     public final static double armGrabBlueberrySkyhook = 0.15;
-    public final static double wristGrabBlueberrySkyhook = 0.1;
+    public final static double wristGrabBlueberrySkyhook = 0.15;
-    public final static double armHangBlueberrySkyhook = 0.6;
+    public final static double armHangBlueberrySkyhook = 0.23;
-    public final static double wristHangBlueberrySkyhook = 0.3;
+    public final static double wristHangBlueberrySkyhook = 0.7;
+    public final static int slideHangBlueberrySkyhook = 500;
+    public final static int slideBelowFloor = -150;
     public final static int backwardBucketDrop = 4670;
     public final static double armBucket = 0.45;
     public final static double armInit = 0.135;

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

@@ -5,6 +5,8 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOT
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.backwardBucketDrop;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideBelowFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -138,6 +140,14 @@ public class DualMotorSliderSubsystem {
     public void toFixedPosition(int value) {
         setTargetPosition(value);
     }
+    public void toHangHeight(){
+        setTargetPosition(slideHangBlueberrySkyhook);
+
+    }
+    public void toHangBelowFloor(){
+        setTargetPosition(slideBelowFloor);
+
+    }
 
     public int getFixedPosition() {
         return liftSlideLeft.getCurrentPosition();