Add V2 code that actually works, missing breaks in case statements (╯°□°)╯︵ ┻━┻

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java

@@ -22,6 +22,7 @@ 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")
@@ -32,7 +33,7 @@ public class BlueBasketAuto extends OpMode {
     private HighBasketAutoPath1 path1;
     private HighBasketAutoPath2 path2;
     private AutoPark pathPark;
-
+    private SkyHookSubsystem hook;
     private CometBotTeleopCompetition comp;
     private ElapsedTime runtime;
 
@@ -49,6 +50,7 @@ public class BlueBasketAuto extends OpMode {
         comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
         comp.initCloseClaw();
         runtime = new ElapsedTime();
+        hook = new SkyHookSubsystem(hardwareMap);
         state = 0;
 
     }
@@ -84,9 +86,24 @@ public class BlueBasketAuto extends OpMode {
 
                     //pathPark.moveToPark(follower);
 
-                    state = 5;
+                    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;
                 }
-            default:
                 //System.out.println("default");
                 //telemetry.addData("default", "default");
                 //telemetry.update();

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java

@@ -113,7 +113,6 @@ public class CometBotTeleopCompetition {
         this.clawControl();
         this.decreaseMaxPower();
         this.increaseMaxPower();
-        this.moveSkyHook();
 
 
         Actions.runBlocking(this.lift.toFloorPosition());
@@ -287,20 +286,6 @@ public class CometBotTeleopCompetition {
 
 
 
-    public void moveSkyHook() {
-        if (this.currentGP2.dpad_down) {
-            hook.moveSkyHook(-1.00);
-        }
-        else if (this.currentGP2.dpad_up) {
-            hook.moveSkyHook(1.00);
-        }
-        else{
-            hook.moveSkyHook(0.00);
-        }
-
-    }
-
-
 
     /*
     Type: PS4

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SkyhookTest.java

@@ -0,0 +1,88 @@
+/* Copyright (c) 2021 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
+
+@TeleOp(name = "Skyhook Test", group = "Debug")
+public class SkyhookTest extends LinearOpMode {
+
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Lift
+         */
+        SkyHookSubsystem hook = new SkyHookSubsystem(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+
+        Gamepad currentGamepad2 = new Gamepad();
+        Gamepad previousGamepad2 = new Gamepad();
+
+
+        hook.init();
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad2.copy(currentGamepad2);
+            currentGamepad2.copy(gamepad2);
+
+            if (currentGamepad2.dpad_up) {
+                hook.moveSkyHook(-1.00);;
+            } else if (currentGamepad2.dpad_down) {
+                hook.moveSkyHook(1.00);
+            } else {
+                hook.moveSkyHook(0.00);
+            }
+
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Skyhook Position", hook.getHookPosition());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java

@@ -6,45 +6,47 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
 
 import com.acmerobotics.roadrunner.Action;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
+
 public class SkyHookSubsystem {
 
-    public DcMotor hook;
+    public DcMotorEx hook;
-
-    public enum SkyHookState {
-        UP, DOWN, STOP
-    }
-
-    private SkyHookState skyHookState;
 
     public SkyHookSubsystem(HardwareMap hardwareMap) {
-        hook = hardwareMap.get(DcMotor.class, SKYHOOK_NAME);
+        hook = hardwareMap.get(DcMotorEx.class, SKYHOOK_NAME);
+
     }
 
-
+    public void moveSkyHook(int position){
-
+        hook.setTargetPosition(position);
+        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+    }
 
     public void moveSkyHook(double power) {
-        this.hook.setPower(power);
+        hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
+        hook.setPower(power);
-        //this.setState(SkyHookState.DOWN);
     }
 
     public void init() {
         hook.setPower(0);
-        hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        hook.setTargetPosition(0);
     }
 
-    private void setState(SkyHookState liftState) {
+    public void toParkPosition() {
-        this.skyHookState = liftState;
+        moveSkyHook(0);
     }
 
-    public SkyHookState getState() {
+    public void toLevel1Position() {
-        return this.skyHookState;
+        moveSkyHook(1500);
     }
 
-
+    public int getHookPosition() {
+        return hook.getCurrentPosition();
+    }
 
 }