From dbc9cf929b8d7742feaf44732424938437cbbb57 Mon Sep 17 00:00:00 2001 From: robotics1 Date: Tue, 19 Nov 2024 17:31:33 -0800 Subject: [PATCH] Almost Working Speciem hanging code! --- .../cometbots/tests/LiftWristArmTest.java | 26 +++++++++++++++++++ .../ftc/teamcode/configs/RobotConstants.java | 6 ++++- .../subsystem/LiftActionsSubsystem.java | 16 +++++++++++- .../subsystem/WristActionsSubsystem.java | 7 ++++- 4 files changed, 52 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java index dcbaebd..7b5f965 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java +++ b/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()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 36e2f16..d3f78a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java index 03f91e5..0986b0f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java +++ b/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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java index 231f707..cd75f01 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java +++ b/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);