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 b3bcc9e..e961acc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java @@ -19,6 +19,7 @@ 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.HighBasketAutoPath6; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem; @@ -34,6 +35,7 @@ public class ComeBotDriveDevV2 extends OpMode { private HighBasketAutoPath3 path3; private HighBasketAutoPath4 path4; private HighBasketAutoPath5 path5; + private HighBasketAutoPath6 path6; private AutoPark pathPark; private SkyHookSubsystem hook; @@ -52,6 +54,7 @@ public class ComeBotDriveDevV2 extends OpMode { path3 = new HighBasketAutoPath3(); path4 = new HighBasketAutoPath4(); path5 = new HighBasketAutoPath5(); + path6 = new HighBasketAutoPath6(); pathPark = new AutoPark(); @@ -207,6 +210,19 @@ public class ComeBotDriveDevV2 extends OpMode { } } } + private void moveToParkPath6() { + if (!followingPath) { + path6.moveToParkPath6(follower); + followingPath = true; + } + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + if (follower.atParametricEnd() || runtime.seconds() > 48.0) { + state = 9; + followingPath = false; + } + } + } private void moveToBasketPath3() { if (!followingPath) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath6.java new file mode 100644 index 0000000..e3cd46d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath6.java @@ -0,0 +1,50 @@ +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 HighBasketAutoPath6 { + + public void moveToParkPath6(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(18.000, 126.000, Point.CARTESIAN), + new Point(18.000, 126.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270)) + .addPath( + // Line 2 + new BezierLine( + new Point(18.000, 126.000, Point.CARTESIAN), + new Point(81.000, 125.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270)) + .addPath( + // Line 3 + new BezierLine( + new Point(81.000, 125.000, Point.CARTESIAN), + new Point(81.000, 100.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} +