From f03515a169bd4c107a263c98768171891e31cc0b Mon Sep 17 00:00:00 2001 From: robotics1 Date: Thu, 23 Jan 2025 17:47:00 -0800 Subject: [PATCH] Added new paths for autonomous --- .../cometbots/paths/CometBotDriveV2.java | 102 +++++++++--------- .../cometbots/paths/HighBasketPath3.java | 2 +- .../cometbots/paths/HighBasketPath4.java | 35 ++++++ 3 files changed, 86 insertions(+), 53 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java index f810a65..f6cea25 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java @@ -39,7 +39,7 @@ public class CometBotDriveV2 extends OpMode { private HighBasketPath1 path1; private HighBasketPath2 path2; private HighBasketPath3 path3; - + private HighBasketPath4 path4; //private CometBotTeleopCompetition comp; @@ -59,61 +59,71 @@ public class CometBotDriveV2 extends OpMode { path1 = new HighBasketPath1(); path2 = new HighBasketPath2(); path3 = new HighBasketPath3(); + path4 = new HighBasketPath4(); lift = new DualMotorSliderSubsystem(hardwareMap); arm = new ArmSubsystem(hardwareMap); wrist = new WristSubsystem(hardwareMap); claw = new ClawSubsystem(hardwareMap); - lift.init(); arm.initAuto(); wrist.InitAuto(); claw.init(); //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); - // comp.initCloseClaw(); - + // comp.initCloseClaw(); + runtime = new ElapsedTime(); } public void loop() { telemetry.addData("state", state); +// telemetry.addData("arm", arm); +// telemetry.addData("lift", lift); +// telemetry.addData("wrist", wrist); +// telemetry.addData("claw", claw); telemetry.addData("followingPath", followingPath); telemetry.addData("busy", follower.isBusy()); + if (runtime != null) { telemetry.addData("Runtime (seconds)", runtime.seconds()); } switch (state) { case 0: +// claw.thumbOutOfTheWay(); moveToPathOneAndHighBucket(); break; case 1: - doArmThing(); + //doArmThing(); + //lift.update(); state = 2; break; case 2: moveToPathTwoAndPickSampleUp(); + //lift.update(); break; case 3: // doPickUpThing(); +// runtime.reset(); state = 4; break; case 4: - // moveToBasketPath3(); + moveToBasketPath3(); break; -// case 5: -// theArmThing(); -// break; -// case 6: -// moveToPickupAgainPath4(); -// break; -// case 7: + case 5: + // theArmThing(); + state = 6; + break; + case 6: + moveToPickupAgainPath4(); + break; + case 7: // doPickUpThingAgain(); -// break; -// case 8: -// moveToPickupAgainPath5(); -// break; + break; + case 8: + moveToPickupAgainPath5(); + break; // case 9: // //theArmThingAgain(); // break; @@ -121,7 +131,8 @@ public class CometBotDriveV2 extends OpMode { // //moveToParkPath6(); // break; // case 11: - + default: + telemetry.addLine("default"); } telemetry.update(); @@ -132,8 +143,8 @@ public class CometBotDriveV2 extends OpMode { } private void moveToPathOneAndHighBucket() { - path1.moveToPath1(follower); - state = 1; + path1.moveToPath1(follower); + state = 1; } public class SetStateAction implements Action { @@ -143,6 +154,7 @@ public class CometBotDriveV2 extends OpMode { public SetStateAction(int value) { this.value = value; } + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { state = value; @@ -157,8 +169,8 @@ public class CometBotDriveV2 extends OpMode { private void doArmThing() { lift.toHighBucketReverseDrop(); - arm.toBucketPosition(); - wrist.toBucketPosition(); + arm.toReverseBucket(); + wrist.toReverseBucket(); claw.openClaw(); wrist.toFloorPosition(); @@ -184,46 +196,32 @@ public class CometBotDriveV2 extends OpMode { // } private void moveToPathTwoAndPickSampleUp() { - if (!follower.isBusy()) { + if (runtime.seconds() > 6) { path2.moveToPath2(follower); state = 3; } } -// + + // private void moveToBasketPath3() { - if (!follower.isBusy()) { + if (runtime.seconds() > 10) + { path3.moveToBasketPath3(follower); state = 5; } } -// private void moveToPickupAgainPath4() { -// if (!followingPath) { -// path4.moveToPickupAgainPath4(follower); -// followingPath = true; -// } -// if (runtime != null) { -// telemetry.addData("Runtime (seconds)", runtime.seconds()); -// if (follower.atParametricEnd() || runtime.seconds() > 27.0) { -// state = 7; -// followingPath = false; -// } -// } -// } -// -// private void moveToPickupAgainPath5() { -// if (!followingPath) { -// path5.moveToPickupAgainPath5(follower); -// followingPath = true; -// } -// if (runtime != null) { -// telemetry.addData("Runtime (seconds)", runtime.seconds()); -// if (follower.atParametricEnd() || runtime.seconds() > 36.0) { -// state = 9; -// followingPath = false; -// } -// } -// } + private void moveToPickupAgainPath4() { + if (runtime.seconds() > 14) { + path4.moveToPickupPath4(follower); + state = 7; + } + } + + private void moveToPickupAgainPath5() { + + } + // private void moveToParkPath6() { // if (!followingPath) { // path6.moveToParkPath6(follower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java index c2ebd55..4dde5cb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java @@ -23,7 +23,7 @@ public class HighBasketPath3 { // Line 1 new BezierLine( new Point(26.000, 117.000, Point.CARTESIAN), - new Point(10.000, 89.00, Point.CARTESIAN) + new Point(22.000, 132.000, Point.CARTESIAN) ) ) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java new file mode 100644 index 0000000..b699774 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath4.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + + +/* + AutoLine# - This file does something of a path...... + + */ +public class HighBasketPath4 { + + + public void moveToPickupPath4(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(22.000, 132.000, Point.CARTESIAN), + new Point(26.000, 127.500, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} +