From 8602dd878b4c3310909cf97e6bde020a11425d1a Mon Sep 17 00:00:00 2001
From: robotics1 <troy@scds.org>
Date: Thu, 9 Jan 2025 18:05:29 -0800
Subject: [PATCH] =?UTF-8?q?Add=20V2=20code=20that=20actually=20works,=20mi?=
 =?UTF-8?q?ssing=20breaks=20in=20case=20statements=20(=E2=95=AF=C2=B0?=
 =?UTF-8?q?=E2=96=A1=C2=B0)=E2=95=AF=EF=B8=B5=20=E2=94=BB=E2=94=81?=
 =?UTF-8?q?=E2=94=BB?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../ftc/teamcode/ComeBotDriveDevV2.java       | 123 ++++++++++++++++++
 1 file changed, 123 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java

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;
+            }
+        }
+    }
+
+
+}
+