New TeleOp

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,78 @@ public class CometBotAutoDevelopment {
         previousGamepad2.copy(currentGamepad2);
         currentGamepad2.copy(gamepad2);
 
-        /*
+        //slides
-        Check if dpad_up/down is being pressed for slides
-        */
         dualSlides.update();
         dualSlidesToLowBucketPosition();
         dualSlidesToHighBucketPosition();
-
+        dualSlidesToFloorPosition();
+        //arm
+        armToParkPosition();
+        armAndWristToFloor();
+        //claw
+        openClaw();
+        openThumb();
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
+    private void openClaw(){
+        if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
+            claw.switchState();
+        }
+    }
+    private void openThumb(){
+        if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){
+            claw.switchState();
+        }
+    }
+    private void armAndWristToFloor(){
+        if(currentGamepad2.a && !previousGamepad2.a){
+            wrist.switchState();
+        }
+        if(currentGamepad2.a && !previousGamepad2.a){
+
+            double increment = 0.6375 - 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 (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
-            dualSlides.toHighBucketPosition();
         }
     }
 
+    private void dualSlidesToHighBucketPosition() {
+        if (currentGamepad2.y && !previousGamepad2.y) {
+            dualSlides.toHighBucketPosition();
+        }
+    }
+    private void dualSlidesToFloorPosition() {
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
+            dualSlides.toHighBucketPosition();
+        }
+    }
     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/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);
+    }
+}