From 8c6ce96ae45856d3e75ab40bcf4b777d9d30480a Mon Sep 17 00:00:00 2001 From: robotics3 Date: Thu, 16 Jan 2025 08:01:31 -0800 Subject: [PATCH] New TeleOp --- .../ftc/teamcode/GeneratedPath.java | 0 .../ftc/teamcode/MultipleTelemetry.java | 0 .../cometbots/CometBotAutoDevelopment.java | 84 +++++++++-- .../teamcode/states/BlueBasketAutoStates.java | 136 ++++++++++++++++++ 4 files changed, 211 insertions(+), 9 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GeneratedPath.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MultipleTelemetry.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/BlueBasketAutoStates.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GeneratedPath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GeneratedPath.java new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MultipleTelemetry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MultipleTelemetry.java new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java index a0e30d6..b5d28c2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java +++ b/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); - /* - Check if dpad_up/down is being pressed for slides - */ + //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(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/BlueBasketAutoStates.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/BlueBasketAutoStates.java new file mode 100644 index 0000000..21b9044 --- /dev/null +++ b/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); + } +}