Merge remote-tracking branch 'origin/branch-swiss-cheese' into branch-swiss-cheese

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);
+    }
+}
+*/