From c20c5ba6248ea049b96b0bf22576f6882201ec2b Mon Sep 17 00:00:00 2001 From: robotics1 Date: Thu, 21 Nov 2024 17:29:15 -0800 Subject: [PATCH] Almost Working Speciem hanging code! --- .../teamcode/cometbots/tests/ClawTest.java | 1 + .../cometbots/tests/LiftWristArmTest.java | 7 +- .../cometbots/tests/SpecimenTest.java | 149 ++++++++++++++++++ .../ftc/teamcode/configs/RobotConstants.java | 6 +- .../subsystem/LiftActionsSubsystem.java | 10 +- .../subsystem/WristActionsSubsystem.java | 8 +- 6 files changed, 175 insertions(+), 6 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java index 1b051cd..43fb6dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java +++ b/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) 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 7b5f965..da35a48 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 @@ -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) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java new file mode 100644 index 0000000..302e724 --- /dev/null +++ b/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(); + } + } +} 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 d3f78a8..ff0934b 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 @@ -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; 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 0986b0f..dc7bd6d 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 @@ -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); + } + + 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 cd75f01..3989181 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 @@ -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); }