diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java new file mode 100644 index 0000000..0162aa3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.teamcode; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +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 V2", group = "Dev") +public class ComeBotDriveDevV2 extends OpMode { + private Follower follower; + private int state; + + private HighBasketAutoPath1 path1; + private HighBasketAutoPath2 path2; + private AutoPark pathPark; + private SkyHookSubsystem hook; + private CometBotTeleopCompetition comp; + private static ElapsedTime runtime; + private static boolean initalized = false; + private static boolean followingPath = false; + 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(); + hook = new SkyHookSubsystem(hardwareMap); + state = 0; + + } + + + public void loop() { + telemetry.addData("state", state); + switch (state) { + case 0: + moveToPathOneAndHighBucket(); + break; + case 1: + doArmThing(); + break; + case 2: + moveToPathTwoAndPickSampleUp(); + break; + } + telemetry.update(); + follower.update(); + } + + private void moveToPathOneAndHighBucket() { + if (!followingPath) { + runtime = new ElapsedTime(); + path1.moveToPath1(follower); + followingPath = true; + } + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + if (follower.atParametricEnd() || runtime.seconds() > 6.0) { + state = 1; + followingPath = false; + } + } + } + + public class SetStateAction implements Action { + + private int value; + + public SetStateAction(int value) { + this.value = value; + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + state = value; + return false; + } + } + + private Action setStateValue(int value) { + return new SetStateAction(value); + } + + private void doArmThing() { + comp.highBucketDrop(); + state = 2; + } + + private void moveToPathTwoAndPickSampleUp() { + if (!followingPath) { + path2.moveToPath2(follower); + followingPath = true; + } + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + if (follower.atParametricEnd() || runtime.seconds() > 12.0) { + state = 3; + followingPath = false; + } + } + } + + +} +