From 793f75371e7aa1f174fb25677f8066385553b71d Mon Sep 17 00:00:00 2001 From: robotics1 Date: Tue, 7 Jan 2025 15:28:48 -0800 Subject: [PATCH] Actions Code --- .../ftc/teamcode/BlueBasketAuto.java | 22 ++++- .../cometbots/CometBotTeleopCompetition.java | 15 ---- .../teamcode/cometbots/tests/SkyhookTest.java | 88 +++++++++++++++++++ .../teamcode/subsystem/SkyHookSubsystem.java | 42 ++++----- 4 files changed, 129 insertions(+), 38 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SkyhookTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java index d87e554..ca54b98 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java +++ b/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,23 @@ public class BlueBasketAuto extends OpMode { //pathPark.moveToPark(follower); - state = 5; + state = 3; } - default: + 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; //System.out.println("default"); //telemetry.addData("default", "default"); //telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java index 6d4dca3..4b706ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java +++ b/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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SkyhookTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SkyhookTest.java new file mode 100644 index 0000000..80d1ae6 --- /dev/null +++ b/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(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java index 82a243d..55c9407 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java +++ b/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 enum SkyHookState { - UP, DOWN, STOP - } - - private SkyHookState skyHookState; + public DcMotorEx hook; 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); - this.hook.setDirection(DcMotorSimple.Direction.REVERSE); - //this.setState(SkyHookState.DOWN); + public void moveSkyHook(double power) { + hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + hook.setPower(power); } 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) { - this.skyHookState = liftState; + public void toParkPosition() { + moveSkyHook(0); } - public SkyHookState getState() { - return this.skyHookState; + public void toLevel1Position() { + moveSkyHook(1500); } - + public int getHookPosition() { + return hook.getCurrentPosition(); + } }