diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java new file mode 100644 index 0000000..edcb7d6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java @@ -0,0 +1,115 @@ +package org.firstinspires.ftc.teamcode; +/* +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.SleepAction; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition; +import org.firstinspires.ftc.teamcode.configs.RobotConstants; +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.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.subsystem.AutoPark; +import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1; +import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2; +import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem; + +@Autonomous(name = "Auto Test Competition", group = "Dev") +public class BlueBasketAuto extends OpMode { + private Follower follower; + private int state; + + private HighBasketAutoPath1 path1; + private HighBasketAutoPath2 path2; + private AutoPark pathPark; + private SkyHookSubsystem hook; + private CometBotTeleopCompetition comp; + private ElapsedTime runtime; + + private LiftActionsSubsystem liftActionsSubsystem; + + @Override + public void init() { + follower = new Follower(hardwareMap); + follower.setMaxPower(.75); + path1 = new HighBasketAutoPath1(); + path2 = new HighBasketAutoPath2(); + pathPark = new AutoPark(); + + comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); + comp.initCloseClaw(); + runtime = new ElapsedTime(); + hook = new SkyHookSubsystem(hardwareMap); + state = 0; + + } + + + @Override + public void loop() { + switch(state) { + case 0: + telemetry.addData("case0", "case0"); + + path1.moveToPath1(follower); + state = 1; + runtime.reset(); + case 1: + if (runtime.seconds() > 5) { + telemetry.addData("case1", "case1"); + + new SleepAction(.5); + comp.highBucketDropAuto(); + + state = 2; + } + case 2: + if (runtime.seconds() > 15) { + telemetry.addData("case2", "case2"); + + // new SleepAsction(.5); + //path2.moveToPath1(follower); + + //For next time, add encoder control to skyhook and extend here + //comp.moveSkyHook(); + + //pathPark.moveToPark(follower); + + state = 3; + } + case 3: + if (runtime.seconds() > 15) { + telemetry.addData("case3", "case3"); + hook.toLevel1Position(); + + + state = 4; + } + case 4: + if (runtime.seconds() > 15) { + telemetry.addData("case3", "case3"); + hook.toLevel1Position(); + + + state = 4; + } + //System.out.println("default"); + //telemetry.addData("default", "default"); + //telemetry.update(); + } + telemetry.update(); + follower.update(); + //follower.telemetryDebug(telemetry); + } +} +*/ \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java index 9a15b57..8934107 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java @@ -13,7 +13,7 @@ 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 = "Pre Loaded Blue Basket Auto", group = "Competition") +@Autonomous(name = "Specimen Test", group = "Competition") public class PreLoadedBlueBasketAuto extends OpMode { private Telemetry telemetryA; @@ -21,7 +21,7 @@ public class PreLoadedBlueBasketAuto extends OpMode { private PathChain path; - private final Pose startPose = new Pose(7.875, 89.357); + private final Pose startPose = new Pose(8, 55); @Override public void init() { @@ -35,79 +35,11 @@ public class PreLoadedBlueBasketAuto extends OpMode { .addPath( // Line 1 new BezierLine( - new Point(8.036, 89.196, Point.CARTESIAN), - new Point(10.125, 126.804, Point.CARTESIAN) + new Point(8, 55, Point.CARTESIAN), + new Point(24, 21, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 2 - new BezierCurve( - new Point(10.125, 126.804, Point.CARTESIAN), - new Point(37.607, 90.000, Point.CARTESIAN), - new Point(62.357, 119.893, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 3 - new BezierCurve( - new Point(62.357, 119.893, Point.CARTESIAN), - new Point(33.750, 112.500, Point.CARTESIAN), - new Point(15.107, 130.661, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 4 - new BezierCurve( - new Point(15.107, 130.661, Point.CARTESIAN), - new Point(58.821, 103.018, Point.CARTESIAN), - new Point(59.625, 126.964, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 5 - new BezierLine( - new Point(59.625, 126.964, Point.CARTESIAN), - new Point(15.107, 130.339, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 6 - new BezierLine( - new Point(15.107, 130.339, Point.CARTESIAN), - new Point(59.625, 126.964, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 7 - new BezierLine( - new Point(59.625, 126.964, Point.CARTESIAN), - new Point(57.857, 133.071, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 8 - new BezierLine( - new Point(57.857, 133.071, Point.CARTESIAN), - new Point(18.964, 134.679, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 9 - new BezierCurve( - new Point(18.964, 134.679, Point.CARTESIAN), - new Point(84.536, 131.786, Point.CARTESIAN), - new Point(80.036, 96.429, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build(); + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-180)).build(); follower.followPath(path); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());