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 edf0b90..b3bcc9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java @@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1; import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2; import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3; import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4; +import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem; @@ -32,6 +33,7 @@ public class ComeBotDriveDevV2 extends OpMode { private HighBasketAutoPath3 path3; private HighBasketAutoPath4 path4; + private HighBasketAutoPath5 path5; private AutoPark pathPark; private SkyHookSubsystem hook; @@ -48,6 +50,9 @@ public class ComeBotDriveDevV2 extends OpMode { path1 = new HighBasketAutoPath1(); path2 = new HighBasketAutoPath2(); path3 = new HighBasketAutoPath3(); + path4 = new HighBasketAutoPath4(); + path5 = new HighBasketAutoPath5(); + pathPark = new AutoPark(); comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); @@ -83,16 +88,16 @@ public class ComeBotDriveDevV2 extends OpMode { theArmThing(); break; case 6: - //moveToPickupAgainPath4(); + moveToPickupAgainPath4(); break; case 7: - //doPickUpThingAgain(); + doPickUpThingAgain(); break; case 8: - //line five + moveToPickupAgainPath5(); break; case 9: - //theArmThingAgain(); + theArmThingAgain(); break; case 10: //line six @@ -156,6 +161,7 @@ public class ComeBotDriveDevV2 extends OpMode { } private void theArmThingAgain() { + follower.breakFollowing(); comp.highBucketDrop(); state = 10; } @@ -181,13 +187,27 @@ public class ComeBotDriveDevV2 extends OpMode { } if (runtime != null) { telemetry.addData("Runtime (seconds)", runtime.seconds()); - if (follower.atParametricEnd() || runtime.seconds() > 12.0) { + 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 moveToBasketPath3() { if (!followingPath) { path3.moveToBasketPath3(follower); @@ -225,6 +245,7 @@ public class ComeBotDriveDevV2 extends OpMode { } private void doPickUpThingAgain() { + follower.breakFollowing(); thePickUp(); state = 8; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java index 1f52aac..45dcd4a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java @@ -24,7 +24,7 @@ public class HighBasketAutoPath2 { // Line 1 new BezierLine( new Point(18.000, 126.000, Point.CARTESIAN), - new Point(29.000, 120.000, Point.CARTESIAN) + new Point(25.500, 120.000, Point.CARTESIAN) ) ) .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)); 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 2342baf..63c90db 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 @@ -18,7 +18,6 @@ public class HighBasketAutoPath3 { public void moveToBasketPath3(Follower robot) { PathChain pathChain; PathBuilder builder = new PathBuilder(); -// robot.setStartingPose( new Pose(29, 120)); builder .addPath( // Line 1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java index 490efd0..63110c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java @@ -22,7 +22,7 @@ public class HighBasketAutoPath4 { // Line 1 new BezierLine( new Point(18.000, 126.000, Point.CARTESIAN), - new Point(29.000, 131.400, Point.CARTESIAN) + new Point(25.000, 129.900, Point.CARTESIAN) ) ) .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath5.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath5.java new file mode 100644 index 0000000..2e134c2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath5.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.subsystem; + + +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 HighBasketAutoPath5 { + + public void moveToPickupAgainPath5(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(24.000, 131.000, Point.CARTESIAN), + new Point(18.000, 126.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} +