Fixed cLaw Bug

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

@@ -15,8 +15,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 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.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
 
 public class CometBotAutoDevelopment {
 
@@ -24,7 +27,9 @@ public class CometBotAutoDevelopment {
     Subsystems
      */
     private DualMotorSliderSubsystem dualSlides;
-
+    private ClawSubsystem claw;
+    private WristSubsystem wrist;
+    private ArmSubsystem arm;
     /*
     Controllers
      */
@@ -35,10 +40,15 @@ public class CometBotAutoDevelopment {
     public Gamepad previousGamepad1;
     public Gamepad previousGamepad2;
 
+
     private Follower follower;
 
     public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
         dualSlides = new DualMotorSliderSubsystem(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
         currentGamepad1 = new Gamepad();
@@ -50,6 +60,9 @@ public class CometBotAutoDevelopment {
     
     public void init() {
         dualSlides.init();
+        claw.init();
+        wrist.init();
+        arm.init();
         follower.setMaxPower(MAX_POWER);
         follower.startTeleopDrive();
     }
@@ -60,25 +73,94 @@ public class CometBotAutoDevelopment {
         previousGamepad2.copy(currentGamepad2);
         currentGamepad2.copy(gamepad2);
 
-        /*
-        Check if dpad_up/down is being pressed for slides
-        */
+        //slides
         dualSlides.update();
         dualSlidesToLowBucketPosition();
         dualSlidesToHighBucketPosition();
+        dualSlidesToFloorPosition();
+        //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 dualSlidesToHighBucketPosition() {
-        if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
-            dualSlides.toHighBucketPosition();
+    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 && !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){
+            wrist.switchState();
+        }
+        if(currentGamepad2.a && !previousGamepad2.a){
+
+            double increment = 0.5 - arm.getPosition();
+            for(int i = 0; i < 3; i ++){
+                arm.setPosition(arm.getPosition() + increment);
+                try {
+                    Thread.sleep(50);
+                } catch (InterruptedException e) {
+                    throw new RuntimeException(e);
+                }
+            }
+            arm.toFloorPosition();
+
+
+        }
+
+
+    }
+    private void armToBucketPosition(){
+        if(currentGamepad2.dpad_up && !previousGamepad2.dpad_up){
+            arm.toBucketPosition();
+            wrist.toBucketPosition();
+
+        }
+    }
+    private void armToParkPosition(){
+        if(currentGamepad2.x && !previousGamepad2.x){
+            arm.toParkPosition();
+
+        }
+    }
+
+    private void dualSlidesToHighBucketPosition() {
+        if (currentGamepad2.y && !previousGamepad2.y) {
+            dualSlides.toHighBucketPosition();
+        }
+    }
+    private void dualSlidesToFloorPosition() {
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
+            dualSlides.toFloorPosotion();
+        }
+    }
     private void dualSlidesToLowBucketPosition() {
-        if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
+        if (currentGamepad2.b && !previousGamepad2.b) {
             dualSlides.toLowBucketPosition();
         }
     }

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

@@ -7,14 +7,21 @@ 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.7;
     public final static double armSubmarine = 0.55;
-    public final static double armPark = 0.1250;
-    public final static double armBucket = 0.25;
-
-    public final static double wristPickup = 0.6;
-    public final static double wristBucket = 0.3;
-    public final static double wristFloor = 0.125;
+    public final static double armPark = 0.33;
+    //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.45;
+    public final static double armInit = 0.125;
+    public final static double wristInit = 0.0;
+    public final static double wristPickup = 0.45;
+    public final static double wristBucket = 0.6;
+    public final static double wristFloor = 0.85;
 
     public final static double wristRung = 0.55;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/BlueBasketAutoStates.java

@@ -0,0 +1,136 @@
+package org.firstinspires.ftc.teamcode.states;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+@Autonomous(name = "Blue Basket Auto States", group = "Competition")
+public class BlueBasketAutoStates extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(11.25, 95.75);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.45);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(11.250, 95.750, Point.CARTESIAN),
+                                new Point(37.000, 108.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(37.000, 108.000, Point.CARTESIAN),
+                                new Point(73.286, 111.536, Point.CARTESIAN),
+                                new Point(67.821, 120.536, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(67.821, 120.536, Point.CARTESIAN),
+                                new Point(28.000, 121.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(28.000, 121.500, Point.CARTESIAN),
+                                new Point(18.000, 130.179, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 5
+                        new BezierCurve(
+                                new Point(18.000, 130.179, Point.CARTESIAN),
+                                new Point(59.000, 102.500, Point.CARTESIAN),
+                                new Point(68.700, 130.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(68.700, 130.500, Point.CARTESIAN),
+                                new Point(18.000, 130.339, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 7
+                        new BezierCurve(
+                                new Point(18.000, 130.339, Point.CARTESIAN),
+                                new Point(49.018, 121.179, Point.CARTESIAN),
+                                new Point(63.804, 135.321, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(63.804, 135.321, Point.CARTESIAN),
+                                new Point(53.036, 135.161, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(53.036, 135.161, Point.CARTESIAN),
+                                new Point(18.643, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 10
+                        new BezierLine(
+                                new Point(18.643, 135.000, Point.CARTESIAN),
+                                new Point(72.300, 97.400, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 11
+                        new BezierLine(
+                                new Point(18.643, 135.000, Point.CARTESIAN),
+                                new Point(83.250, 95.464, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+    }
+}

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);
+        setTargetPosition(2000);
     }
 
     public void toHighBucketPosition() {
-        setTargetPosition(3000);
+        setTargetPosition(4000);
     }
 
+    public void toFloorPosotion(){setTargetPosition(0);}
+
 }

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);