From 11df322ec1be7aec87f9460a8d6b71bd32282c47 Mon Sep 17 00:00:00 2001 From: robotics1 Date: Tue, 14 Jan 2025 17:28:22 -0800 Subject: [PATCH] Two Samples in bucket needs fine tuning --- .../ftc/teamcode/ComeBotDriveDevV2.java | 67 +++++++++++-------- .../cometbots/tests/SpecimenTest.java | 1 - .../ftc/teamcode/subsystem/AutoLine2.java | 40 ----------- .../subsystem/HighBasketAutoPath1.java | 2 +- .../subsystem/HighBasketAutoPath3.java | 6 +- 5 files changed, 44 insertions(+), 72 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java index aaa0222..edf0b90 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java @@ -47,18 +47,22 @@ public class ComeBotDriveDevV2 extends OpMode { follower.setMaxPower(.75); path1 = new HighBasketAutoPath1(); path2 = new HighBasketAutoPath2(); + path3 = new HighBasketAutoPath3(); pathPark = new AutoPark(); comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); comp.initCloseClaw(); hook = new SkyHookSubsystem(hardwareMap); state = 0; - } public void loop() { telemetry.addData("state", state); + telemetry.addData("followingPath", followingPath); + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + } switch (state) { case 0: moveToPathOneAndHighBucket(); @@ -73,22 +77,22 @@ public class ComeBotDriveDevV2 extends OpMode { doPickUpThing(); break; case 4: - moveToBasketPath3(); + moveToBasketPath3(); break; case 5: theArmThing(); break; case 6: - moveToPickupAgainPath4(); + //moveToPickupAgainPath4(); break; case 7: - doPickUpThingAgain(); + //doPickUpThingAgain(); break; case 8: //line five break; case 9: - theArmThingAgain(); + //theArmThingAgain(); break; case 10: //line six @@ -109,8 +113,8 @@ public class ComeBotDriveDevV2 extends OpMode { } if (runtime != null) { telemetry.addData("Runtime (seconds)", runtime.seconds()); - if (follower.atParametricEnd() || runtime.seconds() > 6.0) { - state = 3; + if (follower.atParametricEnd() || runtime.seconds() > 4) { + state = 1; followingPath = false; } } @@ -140,12 +144,18 @@ public class ComeBotDriveDevV2 extends OpMode { state = 2; } - private void theArmThing(){ - comp.highBucketDrop(); - state = 6; + private void theArmThing() { + telemetry.addData("busy?", follower.isBusy()); + telemetry.addData("end?", follower.atParametricEnd()); + if (follower.atParametricEnd()){ + follower.breakFollowing(); + comp.highBucketDrop(); + state = 6; + } +// follower.breakFollowing(); } - private void theArmThingAgain(){ + private void theArmThingAgain() { comp.highBucketDrop(); state = 10; } @@ -163,6 +173,7 @@ public class ComeBotDriveDevV2 extends OpMode { } } } + private void moveToPickupAgainPath4() { if (!followingPath) { path4.moveToPickupAgainPath4(follower); @@ -176,6 +187,7 @@ public class ComeBotDriveDevV2 extends OpMode { } } } + private void moveToBasketPath3() { if (!followingPath) { path3.moveToBasketPath3(follower); @@ -190,28 +202,29 @@ public class ComeBotDriveDevV2 extends OpMode { } } - private void thePickUp(){ + private void thePickUp() { Actions.runBlocking(new SequentialAction( - comp.arm.toFloorPosition(), - new SleepAction(.5), - comp.wrist.toPickupPosition(), - new SleepAction(1), - comp.claw.closeClaw(), - new SleepAction(1), - comp.wrist.toFloorPosition(), - new SleepAction(0.5), - comp.arm.toParkPosition() - )); + new SleepAction(.25), + comp.arm.toSubmarinePosition(), + new SleepAction(.5), + comp.wrist.toPickupPosition(), + new SleepAction(.75), + comp.claw.closeClaw(), + new SleepAction(.75), + comp.wrist.toFloorPosition(), + new SleepAction(0.5), + comp.arm.toParkPosition(), + new SleepAction(.25) + )); } - private void doPickUpThing(){ + private void doPickUpThing() { + follower.breakFollowing(); thePickUp(); - if(!follower.isBusy()) { - state = 4; - } + state = 4; } - private void doPickUpThingAgain(){ + private void doPickUpThingAgain() { thePickUp(); state = 8; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java index 9141776..5d3f799 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java @@ -49,7 +49,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.AutoLine1; -import org.firstinspires.ftc.teamcode.subsystem.AutoLine2; import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java deleted file mode 100644 index a2c1c1e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystem; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; - -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -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 AutoLine2 { - - - private PathChain pathChain; - - private PathChain goToStore; - - private Pose startPose = new Pose(37.5, 72); - - public AutoLine2(Follower robot) { - robot.setStartingPose(startPose); - pathChain = robot.pathBuilder().addPath( - new BezierLine( - new Point(37.500, 72.000, Point.CARTESIAN), - new Point(36.000, 72.000, Point.CARTESIAN) - ) - ).setConstantHeadingInterpolation(Math.toRadians(0)).build(); - robot.followPath(pathChain); - } -} - - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java index 76bf66c..ef05bb5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java @@ -29,7 +29,7 @@ public class HighBasketAutoPath1 { new Point(18.000, 126.000, Point.CARTESIAN) ) ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build(); + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)); pathChain = builder.build(); robot.followPath(pathChain); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java index 72b9405..2342baf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java @@ -2,7 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; @@ -14,10 +14,11 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; */ public class HighBasketAutoPath3 { - + public void moveToBasketPath3(Follower robot) { PathChain pathChain; PathBuilder builder = new PathBuilder(); +// robot.setStartingPose( new Pose(29, 120)); builder .addPath( // Line 1 @@ -27,7 +28,6 @@ public class HighBasketAutoPath3 { ) ) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)); - pathChain = builder.build(); robot.followPath(pathChain); }