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