Updated Blueberry Specimen Grabing code and new values. Reprogrammed a few servos also and I haven't tweaked the code yet. The Blueberry Specimen Grab is UNTESTED. CAUTION

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

@@ -81,20 +81,36 @@ public class CometBotAutoDevelopment {
         //arm
         armToParkPosition();
         armAndWristToFloor();
+        armToBucketPosition();
         //claw
         openClaw();
         openThumb();
+        //hang
+        grabBlueberrySkyhook();
+
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
+    private void grabBlueberrySkyhook () {
+        if(currentGamepad1.x){
+            claw.grabBlueberry();
+            wrist.grabBlueberrySkyhook();
+            arm.grabBlueberrySkyhook();
+        }
+        if(currentGamepad1.y){
+            wrist.hangBlueberrySkyhook();
+            arm.hangBlueberrySkyhook();
+        }
+    }
+
     private void openClaw(){
         if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
             claw.switchState();
         }
     }
     private void openThumb(){
-        if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
+        if(currentGamepad2.left_bumper && !currentGamepad2.left_bumper){
-            claw.switchState();
+            claw.switchTState();
         }
     }
     private void armAndWristToFloor(){
@@ -103,7 +119,7 @@ public class CometBotAutoDevelopment {
         }
         if(currentGamepad2.a && !previousGamepad2.a){
 
-            double increment = 0.6375 - arm.getPosition();
+            double increment = 0.5 - arm.getPosition();
             for(int i = 0; i < 3; i ++){
                 arm.setPosition(arm.getPosition() + increment);
                 try {
@@ -140,7 +156,7 @@ public class CometBotAutoDevelopment {
     }
     private void dualSlidesToFloorPosition() {
         if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
-            dualSlides.toHighBucketPosition();
+            dualSlides.toFloorPosotion();
         }
     }
     private void dualSlidesToLowBucketPosition() {

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

@@ -7,9 +7,15 @@ public class RobotConstants {
     public final static double clawClose = 0.85;
     public final static double clawOpen = 0.05;
 
-    public final static double armFloor = 0.6375;
+    public final static double armFloor = 0.5;
     public final static double armSubmarine = 0.55;
-    public final static double armPark = 0.1250;
+    public final static double armPark = 0.3;
+    //value for grabbing the hook Specimen
+    public final static double grabBlueberry = 0.3;
+    public final static double armGrabBlueberrySkyhook = 0.15;
+    public final static double wristGrabBlueberrySkyhook = 0.1;
+    public final static double armHangBlueberrySkyhook = 0.6;
+    public final static double wristHangBlueberrySkyhook = 0.3;
     public final static double armBucket = 0.25;
 
     public final static double wristPickup = 0.6;

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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem;
 import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -11,7 +13,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class ArmSubsystem {
 
     public enum ArmState {
-        PARK, FLOOR, BUCKET, SUBMARINE
+        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK
     }
 
     private ServoImplEx arm;
@@ -20,6 +22,14 @@ public class ArmSubsystem {
     public ArmSubsystem(HardwareMap hardwareMap) {
         this.arm = hardwareMap.get(ServoImplEx.class, ARM_SERVO);
     }
+    public void grabBlueberrySkyhook(){
+        setState(ArmState.GRAB_BLUEBERRY_SKYHOOK);
+        arm.setPosition(armGrabBlueberrySkyhook);
+    }
+    public void hangBlueberrySkyhook(){
+        setState(ArmState.HANG_BLUEBERRY_SKYHOOK);
+        arm.setPosition(armHangBlueberrySkyhook);
+    }
 
     public void toParkPosition() {
         arm.setPosition(armPark);

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

@@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
 import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.grabBlueberry;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
@@ -11,7 +12,7 @@ import com.qualcomm.robotcore.hardware.Servo;
 public class ClawSubsystem {
 
     public enum ClawState {
-        CLOSED, OPEN
+        CLOSED, OPEN, GRAB_BLUEBERRY
     }
 
     public enum ThumbState {
@@ -27,7 +28,10 @@ public class ClawSubsystem {
         claw = hardwareMap.get(Servo.class, CLAW_NAME);
         thumb = hardwareMap.get(Servo.class, THUMB_SERVO);
     }
-
+    public void grabBlueberry(){
+        claw.setPosition(grabBlueberry);
+        state = ClawState.GRAB_BLUEBERRY;
+    }
     public void closeClaw() {
         claw.setPosition(clawClose);
         state = ClawState.CLOSED;

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

@@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOT
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
@@ -81,7 +82,8 @@ public class DualMotorSliderSubsystem {
         liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
+        liftSlideLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE);
         liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -121,11 +123,13 @@ public class DualMotorSliderSubsystem {
     }
 
     public void toLowBucketPosition() {
-        setTargetPosition(1500);
-    }
-
-    public void toHighBucketPosition() {
         setTargetPosition(3000);
     }
 
+    public void toHighBucketPosition() {
+        setTargetPosition(4000);
+    }
+
+    public void toFloorPosotion(){setTargetPosition(5);}
+
 }

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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem;
 import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
 
@@ -17,7 +19,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class WristSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK
     }
 
     public ServoImplEx wrist;
@@ -44,7 +46,18 @@ public class WristSubsystem {
             return false;
         }
     }
+    public void grabBlueberrySkyhook(){
+        setState(WristState.GRAB_BLUEBERRY_SKYHOOK);
+        wrist.setPosition(wristGrabBlueberrySkyhook);
+    }
 
+    /**
+     * This function goes after grabBlueberrySkyhook
+     */
+    public void hangBlueberrySkyhook(){
+        setState(WristState.HANG_BLUEBERRY_SKYHOOK);
+        wrist.setPosition(wristHangBlueberrySkyhook);
+    }
     public void toFloorPosition() {
         setState(WristState.FLOOR);
         wrist.setPosition(wristFloor);