Almost Working Speciem hanging code!

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

@@ -29,6 +29,9 @@
 
 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;
@@ -37,6 +40,8 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
+
 
 @TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
 public class LiftWristArmTest extends LinearOpMode {
@@ -52,6 +57,8 @@ public class LiftWristArmTest extends LinearOpMode {
         LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
         WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
         ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
+        ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
+
 
         /*
          * Instantiate gamepad state holders
@@ -62,6 +69,7 @@ public class LiftWristArmTest extends LinearOpMode {
         lift.init();
         wrist.init();
         arm.init();
+        claw.init();
         waitForStart();
         runtime.reset();
 
@@ -79,6 +87,23 @@ public class LiftWristArmTest extends LinearOpMode {
             if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
                 arm.setPosition(arm.getPosition() - .05);
             }
+            if (currentGamepad1.circle && !previousGamepad1.circle) {
+                claw.switchState();
+            }
+            if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
+                Actions.runBlocking(
+                        new SequentialAction(
+                                lift.toHighRung(),
+                                arm.toBucketPosition(),
+                                wrist.toRungPosition(),
+                                new SleepAction(2),
+                                lift.toHighRungAttach(),
+                                new SleepAction(2),
+                                claw.openClaw())
+                );
+
+                //Delete open claw }
+
 
             if (currentGamepad1.triangle && !previousGamepad1.triangle) {
                 wrist.setPosition(wrist.getPosition() + .05);
@@ -96,6 +121,7 @@ public class LiftWristArmTest extends LinearOpMode {
                 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());

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

@@ -14,12 +14,16 @@ public class RobotConstants {
 
     public final static double wristFloor = 0.55;
     public final static double wristBucket = 0.25;
+    public final static double wristRung = 0.55;
+
     public final static double wristPickup = 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 = 2250;
+    public final static int liftToHighRung = 1245;
+    public final static int liftToHighRungAttach = 1050;
+
 
     public final static int liftToHighBucketPos = 3850;
     public final static double liftPower = .625;

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

@@ -6,6 +6,10 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorP
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
 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 androidx.annotation.NonNull;
 
@@ -21,7 +25,7 @@ public class LiftActionsSubsystem {
     public DcMotor lift;
 
     public enum LiftState {
-        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE,
+        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE, HIGH_RUNG
     }
 
     private LiftState liftState;
@@ -56,6 +60,16 @@ public class LiftActionsSubsystem {
         return new MoveToPosition(liftToSubmarinePos, LiftState.FLOOR);
     }
 
+    public Action toHighRung() {
+        return new MoveToPosition(liftToHighRung, LiftState.HIGH_RUNG);
+    }
+
+
+
+
+    public Action toHighRungAttach() {
+        return new MoveToPosition(liftToHighRungAttach, LiftState.HIGH_RUNG);
+    }
     public Action toLowBucketPosition() {
         return new MoveToPosition(liftToLowBucketPos, LiftState.LOW_BUCKET);
 

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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem;
 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 androidx.annotation.NonNull;
 
@@ -15,7 +17,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class WristActionsSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET, PICKUP
+        FLOOR, BUCKET, PICKUP, RUNG
     }
 
     public ServoImplEx wrist;
@@ -46,6 +48,9 @@ public class WristActionsSubsystem {
     public Action toFloorPosition() {
         return new MoveToPosition(wristFloor, WristState.FLOOR);
     }
+    public Action toRungPosition() {
+        return new MoveToPosition(wristFloor, WristState.FLOOR);
+    }
 
     public Action toBucketPosition() {
         return new MoveToPosition(wristBucket, WristState.BUCKET);