Almost Working Speciem hanging code!

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

@@ -58,6 +58,7 @@ public class ClawTest extends LinearOpMode {
         Gamepad previousGamepad1 = new Gamepad();
 
         waitForStart();
+        claw.init();
         runtime.reset();
 
         // run until the end of the match (driver presses STOP)

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

@@ -96,13 +96,14 @@ public class LiftWristArmTest extends LinearOpMode {
                                 lift.toHighRung(),
                                 arm.toBucketPosition(),
                                 wrist.toRungPosition(),
-                                new SleepAction(2),
+                                new SleepAction(0.5),
                                 lift.toHighRungAttach(),
-                                new SleepAction(2),
+                                new SleepAction(0.5),
                                 claw.openClaw())
                 );
 
-                //Delete open claw }
+                //Delete open claw
+            }
 
 
             if (currentGamepad1.triangle && !previousGamepad1.triangle) {

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

@@ -0,0 +1,149 @@
+/* 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.SequentialAction;
+import com.acmerobotics.roadrunner.SleepAction;
+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.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
+
+
+@TeleOp(name = "SpecimenTest", group = "Debug")
+public class SpecimenTest extends LinearOpMode {
+
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Lift
+         */
+        LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
+        WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
+        ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
+        ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
+        Follower robot = new Follower(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        lift.init();
+        wrist.init();
+        arm.init();
+        claw.init();
+        robot.setMaxPower(.75);
+        robot.startTeleopDrive();
+
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            robot.setTeleOpMovementVectors(-currentGamepad1.left_stick_y, -currentGamepad1.left_stick_x, -currentGamepad1.right_stick_x);
+            //robot.update();
+
+            if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
+                arm.setPosition(arm.getPosition() + .05);
+            }
+
+            if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
+                arm.setPosition(arm.getPosition() - .05);
+            }
+            if (currentGamepad1.circle && !previousGamepad1.circle) {
+                claw.switchState();
+            }
+            if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
+                Actions.runBlocking(
+                        new SequentialAction(
+                                wrist.toFloorPosition(),
+                                lift.toHighRung(),
+                                wrist.toSpeciemenBar(),
+                                lift.dropToHighRung()
+                        )
+                );
+
+            }
+            if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
+                Actions.runBlocking(
+                        new SequentialAction(
+                                claw.openClaw(),
+                                wrist.toFloorPosition(),
+                                lift.toFloorPosition()
+                        )
+                );
+
+            }
+
+
+            if (currentGamepad1.triangle && !previousGamepad1.triangle) {
+                wrist.setPosition(wrist.getPosition() + .05);
+            }
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                wrist.setPosition(wrist.getPosition() - .05);
+            }
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+                lift.setPosition(lift.getPosition() + 175);
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                lift.setPosition(lift.getPosition() - 25);
+            }
+
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Lift Drive Position", lift.getPosition());
+            telemetry.addData("Wrist Position", wrist.getPosition());
+            telemetry.addData("Arm Position", arm.getPosition());
+
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -17,11 +17,15 @@ public class RobotConstants {
     public final static double wristRung = 0.55;
 
     public final static double wristPickup = 0.1;
+    public final static double wristSpeciemen = 0.1;
+
 
     public final static int liftToFloorPos = 350;
     public final static int liftToSubmarinePos = 350;
     public final static int liftToLowBucketPos = 2250;
-    public final static int liftToHighRung = 1245;
+    public final static int liftToHighRung = 2100;
+    public final static int dropToHighRung = 1675;
+
     public final static int liftToHighRungAttach = 1050;
 
 

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

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_NAME;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
@@ -8,6 +9,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBuc
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToSubmarinePos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRung;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighRungAttach;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.dropToHighRung;
+
 
 
 
@@ -25,7 +28,7 @@ public class LiftActionsSubsystem {
     public DcMotor lift;
 
     public enum LiftState {
-        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG
+        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG, HIGH_RUNG_DROP
     }
 
     private LiftState liftState;
@@ -64,6 +67,11 @@ public class LiftActionsSubsystem {
         return new MoveToPosition(liftToHighRung, LiftState.HIGH_RUNG);
     }
 
+    public Action dropToHighRung() {
+        return new MoveToPosition(dropToHighRung, LiftState.HIGH_RUNG_DROP);
+    }
+
+
 
 
 

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

@@ -4,6 +4,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristRung;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
+
 
 
 import androidx.annotation.NonNull;
@@ -17,7 +19,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class WristActionsSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET, PICKUP, RUNG
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN
     }
 
     public ServoImplEx wrist;
@@ -48,6 +50,10 @@ public class WristActionsSubsystem {
     public Action toFloorPosition() {
         return new MoveToPosition(wristFloor, WristState.FLOOR);
     }
+    public Action toSpeciemenBar() {
+        return new MoveToPosition(wristSpeciemen, WristState.SPECIMEN);
+    }
+
     public Action toRungPosition() {
         return new MoveToPosition(wristFloor, WristState.FLOOR);
     }