diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index a868ec6..d57ea90 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -4,15 +4,16 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunMode; +import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition; -@TeleOp(name = "Dev Teleop Actions", group = "Debug") +@TeleOp(name = "Dev Teleop RR Actions", group = "Debug") public class DevTeleop extends OpMode { - public DevTeleopRunMode runMode; + public DevTeleopRunModeCompetition runMode; @Override public void init() { - this.runMode = new DevTeleopRunMode(hardwareMap, telemetry, gamepad1, gamepad2); + this.runMode = new DevTeleopRunModeCompetition(hardwareMap, telemetry, gamepad1, gamepad2); this.runMode.init(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java deleted file mode 100644 index 039e835..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots; - -public class BluenbAutov1 { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherOrientBlue.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherOrientBlue.java index f933253..4ff899a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherOrientBlue.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -26,6 +27,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AsherOrientBlue", group = "Autonomous Pathing Tuning") public class AsherOrientBlue extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathBlueV1.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathBlueV1.java index 18bddac..d95f40e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathBlueV1.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -26,6 +27,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AsherPathBlueV1", group = "Autonomous Pathing Tuning") public class AsherPathBlueV1 extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathV1.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathV1.java index 060548e..b13d9b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathV1.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -13,7 +14,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - +import com.acmerobotics.roadrunner.SleepAction; /** * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite * a circle, but some Bezier curves that have control points set essentially in a square. However, @@ -26,6 +27,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning") public class AsherPathV1 extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExample.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExample.java index a944187..ec47d35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExample.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -25,6 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning") public class AutoExample extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleFour.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleFour.java index 64e96f1..71501db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleFour.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -26,6 +27,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning") public class AutoExampleFour extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleSeason2025V1.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleSeason2025V1.java index 632804e..51096ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleSeason2025V1.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -25,6 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning") public class AutoExampleSeason2025V1 extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleThree.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleThree.java index 60fb0ec..b762758 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleThree.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -26,6 +27,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning") public class AutoExampleThree extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleTwo.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleTwo.java index 0784e81..edcbe30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleTwo.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -25,6 +26,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning") public class AutoExampleTwo extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BasicOmniOpMode_Linear.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BasicOmniOpMode_Linear.java index c726774..5ee9e5f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BasicOmniOpMode_Linear.java @@ -27,7 +27,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; @@ -44,6 +44,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTI import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -81,6 +82,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; */ @TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode") +@Disabled public class BasicOmniOpMode_Linear extends LinearOpMode { // Declare OpMode members for each of the 4 motors. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BlueBasketAuto.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BlueBasketAuto.java index c596660..3bff880 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BlueBasketAuto.java @@ -1,5 +1,6 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -27,6 +28,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning") public class BlueBasketAuto extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BluebAutoV1.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BluebAutoV1.java index 67007ab..a3ba0ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BluebAutoV1.java @@ -1,9 +1,10 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.poc; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -26,6 +27,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; * @version 1.0, 3/12/2024 */ @Config +@Disabled @Autonomous(name = "BluebAutoV1", group = "Autonomous Pathing Tuning") public class BluebAutoV1 extends OpMode { private Telemetry telemetryA; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java deleted file mode 100644 index 4065a95..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java +++ /dev/null @@ -1,124 +0,0 @@ -/* 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.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.util.ElapsedTime; - -@TeleOp(name = "Lift Raw Test", group = "Debug") -public class LiftRawTest extends LinearOpMode { - - // Declare OpMode members for each of the 4 motors. - private final ElapsedTime runtime = new ElapsedTime(); - - private final int MIN_POINT = 0; - private final int MAX_POINT = 3700; - - @Override - public void runOpMode() { - - /* - * Instantiate Lift - */ - DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor"); - liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - liftDrive.setDirection(DcMotorSimple.Direction.REVERSE); - - /* - * Instantiate gamepad state holders - */ - Gamepad currentGamepad1 = new Gamepad(); - Gamepad previousGamepad1 = new Gamepad(); - - waitForStart(); - runtime.reset(); - - - // run until the end of the match (driver presses STOP) - while (opModeIsActive()) { - - previousGamepad1.copy(currentGamepad1); - currentGamepad1.copy(gamepad1); - - liftDrive.setPower(.5); - - // Max position is 6800, safely setting to 6500 - - if (currentGamepad1.square && !previousGamepad1.square) { - liftDrive.setTargetPosition(MIN_POINT); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (currentGamepad1.triangle && !previousGamepad1.triangle) { - liftDrive.setTargetPosition(1500); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (currentGamepad1.circle && !previousGamepad1.circle) { - liftDrive.setTargetPosition(2750); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (currentGamepad1.cross && !previousGamepad1.cross) { - liftDrive.setTargetPosition(MAX_POINT); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { - int newPosition = liftDrive.getCurrentPosition() - 125; - if (newPosition < MIN_POINT) { - liftDrive.setTargetPosition(MIN_POINT); - } else { - liftDrive.setTargetPosition(newPosition); - } - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { - int newPosition = liftDrive.getCurrentPosition() + 125; - if (newPosition > MAX_POINT) { - liftDrive.setTargetPosition(MAX_POINT); - } else { - liftDrive.setTargetPosition(newPosition); - } - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - // Show the elapsed game time and wheel power. - telemetry.addData("Status", "Run Time: " + runtime.toString()); - telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition()); - telemetry.update(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java index 3867daa..ea4e567 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java @@ -29,32 +29,26 @@ 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.LiftSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; @TeleOp(name = "Lift Test", group = "Debug") public class LiftTest extends LinearOpMode { - // Declare OpMode members for each of the 4 motors. private final ElapsedTime runtime = new ElapsedTime(); - private final int MIN_POINT = 0; - // 2000 ~ 2500 - - // 3750 max - private final int MAX_POINT = 6500; - @Override public void runOpMode() { /* * Instantiate Lift */ - LiftSubsystem lift = new LiftSubsystem(hardwareMap, telemetry); + LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap); /* * Instantiate gamepad state holders @@ -74,21 +68,29 @@ public class LiftTest extends LinearOpMode { currentGamepad1.copy(gamepad1); if (currentGamepad1.square && !previousGamepad1.square) { - lift.toFloor(); + Actions.runBlocking(lift.toFloorPosition()); } if (currentGamepad1.triangle && !previousGamepad1.triangle) { - lift.toHighBucket(); + Actions.runBlocking(lift.toHighBucketPosition()); } if (currentGamepad1.circle && !previousGamepad1.circle) { - lift.toLowBucket(); + Actions.runBlocking(lift.toLowBucketPosition()); } if (currentGamepad1.cross && !previousGamepad1.cross) { lift.switchState(); } + if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { + lift.setPosition(lift.getPosition() + 25); + } + + 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()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java index 028cd81..a72498a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java @@ -29,11 +29,13 @@ 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.WristActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; @TeleOp(name = "Wrist Test", group = "Debug") @@ -48,7 +50,7 @@ public class WristTest extends LinearOpMode { /* * Instantiate Wrist */ - WristSubsystem wrist = new WristSubsystem(hardwareMap, telemetry); + WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap); /* * Instantiate gamepad state holders @@ -67,11 +69,11 @@ public class WristTest extends LinearOpMode { currentGamepad1.copy(gamepad1); if (currentGamepad1.square && !previousGamepad1.square) { - wrist.toBucketPosition(); + Actions.runBlocking(wrist.toBucketPosition()); } if (currentGamepad1.circle && !previousGamepad1.circle) { - wrist.toFloorPosition(); + Actions.runBlocking(wrist.toFloorPosition()); } if (currentGamepad1.cross && !previousGamepad1.cross) { 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 04929b0..dd9f00c 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 @@ -4,18 +4,21 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { - public static double clawClose = 1.00; - public static double clawOpen = 0.0625; + public final static double clawClose = 1.00; + public final static double clawOpen = 0.05; - public static double armFloor = 0.375; - public static double armPark = 0.0; - public static double armBucket = 0.15; + public final static double armFloor = 0.45; + public final static double armSubmarine = 0.375; + public final static double armPark = 0.0; + public final static double armBucket = 0.15; - public static double wristFloor = 0.625; - public static double wristBucket = 0.215; - public static int liftToFloorPos = 20; - public static int liftToFloatPos = 150; - public static int liftToLowBucketPos = 2250; - public static int liftToHighBucketPos = 3850; - public static double liftPower = .375; + public final static double wristFloor = 0.7; + public final static double wristBucket = 0.215; + + public final static int liftToFloorPos = 0; + public final static int liftToFloatPos = 150; + public final static int liftToSubmarinePos = 250; + public final static int liftToLowBucketPos = 2250; + public final static int liftToHighBucketPos = 3850; + public final static double liftPower = .625; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java index 3503b14..8564430 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java @@ -66,11 +66,11 @@ public class DevTeleopRunMode { this.currentGP1.copy(this.GP1); this.previousGP2.copy(currentGP2); this.currentGP2.copy(this.GP2); - this.theDrop(); + this.toTravelfromField(); this.thePickup(); - this.theTravel(); - this.theLowBucketScore(); - this.theHighBucketScore(); + this.toFieldFromBucketScore(); + this.toLowBucketScore(); + this.toHighBucketScore(); this.toHold(); this.motors.calculateTrajectory(this.GP1); this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); @@ -87,15 +87,16 @@ public class DevTeleopRunMode { Button: A Action: On button press, Arm hovers the floor with wrist parallel to arm */ - public void theDrop() { + public void toTravelfromField() { if (this.currentGP1.dpad_down && !this.previousGP1.dpad_down) { - if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD && + if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.TRAVELING && this.lift.getPosition() < 40) { Actions.runBlocking(new SequentialAction( this.wrist.toFloorPosition, new SleepAction(.75), this.arm.toFloorPosition )); + fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); } } } @@ -118,12 +119,10 @@ public class DevTeleopRunMode { */ public void toHold() { if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) { - if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD) { - Actions.runBlocking(new SequentialAction( - arm.toParkPosition, - wrist.toFloorPosition - )); - } + Actions.runBlocking(new SequentialAction( + arm.toParkPosition, + wrist.toFloorPosition + )); } } @@ -133,7 +132,7 @@ public class DevTeleopRunMode { Action: On button press, lift to low bucket height, arm to bucket position, wrist to bucket position */ - public void theLowBucketScore() { + public void toLowBucketScore() { if (this.currentGP1.a && !this.previousGP1.a) { fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); Actions.runBlocking(new SequentialAction( @@ -150,7 +149,7 @@ public class DevTeleopRunMode { Action: On button press, lift to low bucket height, arm to bucket position, wrist to bucket position */ - public void theHighBucketScore() { + public void toHighBucketScore() { if (this.currentGP1.b && !this.previousGP1.b) { fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); Actions.runBlocking(new SequentialAction( @@ -167,15 +166,15 @@ public class DevTeleopRunMode { Action: On directional press, lift to floor height, arm to bucket position, wrist to floor position */ - public void theTravel(){ - if (this.currentGP1.dpad_right && !this.previousGP1.dpad_right){ + public void toFieldFromBucketScore() { + if (this.currentGP1.dpad_right && !this.previousGP1.dpad_right) { if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.BUCKET) { Actions.runBlocking(new SequentialAction( lift.toFloor, arm.toBucketPosition, wrist.toFloorPosition )); - fieldStates.setFieldLocation(FieldStates.FieldLocation.FIELD); +// fieldStates.setFieldLocation(FieldStates.FieldLocation.FIELD); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunModeCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunModeCompetition.java new file mode 100644 index 0000000..a4ee7bd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunModeCompetition.java @@ -0,0 +1,185 @@ +package org.firstinspires.ftc.teamcode.runmodes; + +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.states.FieldStates; +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.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem; + +public class DevTeleopRunModeCompetition { + + /* + Subsystems + */ + private MotorsSubsystem motors; + public ClawActionsSubsystem claw; + public ArmActionsSubsystem arm; + public WristActionsSubsystem wrist; + public LiftActionsSubsystem lift; + + /* + Controllers + */ + public Gamepad GP1; + public Gamepad GP2; + public Gamepad currentGP1; + public Gamepad previousGP1; + public Gamepad currentGP2; + public Gamepad previousGP2; + private Telemetry telemetry; + public FieldStates fieldStates; + + private Follower follower; + + public DevTeleopRunModeCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { + this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55); + this.claw = new ClawActionsSubsystem(hardwareMap); + this.arm = new ArmActionsSubsystem(hardwareMap); + this.wrist = new WristActionsSubsystem(hardwareMap); + this.lift = new LiftActionsSubsystem(hardwareMap); + this.GP1 = gp1; + this.GP2 = gp2; + this.telemetry = telemetry; + this.currentGP1 = new Gamepad(); + this.currentGP2 = new Gamepad(); + this.previousGP1 = new Gamepad(); + this.previousGP2 = new Gamepad(); + this.fieldStates = new FieldStates(); + this.follower = new Follower(hardwareMap); + } + + public void init() { + this.motors.init(); + this.claw.init(); + this.arm.init(); + this.wrist.init(); + this.lift.init(); + this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + follower.setMaxPower(.75); + follower.startTeleopDrive(); + } + + public void update() { + this.previousGP1.copy(currentGP1); + this.currentGP1.copy(this.GP1); + this.previousGP2.copy(currentGP2); + this.currentGP2.copy(this.GP2); + + this.toHighBucketScore(); + this.toLowBucketScore(); + this.toArmParkPosition(); + this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition(); + this.clawControl(); + +// this.motors.calculateTrajectory(this.GP1); + follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x); + follower.update(); + this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); + this.telemetry.addData("Claw State", this.claw.getState()); + this.telemetry.addData("Claw Position", this.claw.getPosition()); + this.telemetry.addData("Wrist State", this.wrist.getState()); + this.telemetry.addData("Arm State", this.arm.getState()); + this.telemetry.addData("Lift State", this.lift.getState()); + this.telemetry.addData("Lift Position", this.lift.getPosition()); + } + + /* + Controller: 1 + Button: A + Action: On button press, Arm hovers the floor with wrist parallel to arm + */ + public void toHighBucketScore() { + if (this.currentGP1.triangle && !this.previousGP1.triangle) { + fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); + Actions.runBlocking(new SequentialAction( + this.wrist.toFloorPosition(), + this.arm.toParkPosition(), + this.lift.toHighBucketPosition(), + new SleepAction(.5), + this.arm.toBucketPosition(), + new SleepAction(.5), + this.wrist.toBucketPosition(), + new SleepAction(.5), + this.claw.openClaw(), + new SleepAction(.5), + this.wrist.toFloorPosition(), + new SleepAction(.5), + this.arm.toParkPosition(), + this.lift.toFloorPosition() + )); + fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + } + } + + public void toLowBucketScore() { + if (this.currentGP1.circle && !this.previousGP1.circle) { + fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); + Actions.runBlocking(new SequentialAction( + this.wrist.toFloorPosition(), + this.arm.toParkPosition(), + this.lift.toLowBucketPosition(), + new SleepAction(.5), + this.arm.toBucketPosition(), + new SleepAction(.5), + this.wrist.toBucketPosition(), + new SleepAction(.5), + this.claw.openClaw(), + new SleepAction(.5), + this.wrist.toFloorPosition(), + new SleepAction(.5), + this.arm.toParkPosition(), + this.lift.toFloorPosition() + )); + fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + } + } + + public void clawControl() { + if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) { + this.claw.switchState(); + } + } + + public void toArmParkPosition() { + if (this.currentGP1.square && !this.previousGP1.square) { + Actions.runBlocking(this.arm.toParkPosition()); + } + } + + public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() { + if (this.currentGP1.cross && !previousGP1.cross) { + if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) { + Actions.runBlocking( + new SequentialAction( + this.arm.toSubmarinePosition(), + this.wrist.toFloorPosition() + ) + ); + } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE) { + Actions.runBlocking( + new SequentialAction( + this.arm.toFloorPosition(), + this.wrist.toFloorPosition() + ) + ); + } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) { + Actions.runBlocking( + new SequentialAction( + this.arm.toSubmarinePosition(), + this.wrist.toFloorPosition() + ) + ); + } + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java index 2a2594f..20d6666 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java @@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.states; public class FieldStates { public enum FieldLocation { - BUCKET, SUBMARINE, FIELD + BUCKET, SUBMARINE, FLOATING, TRAVELING } private FieldLocation fieldLocation; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmActionsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmActionsSubsystem.java new file mode 100644 index 0000000..71c0eaa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmActionsSubsystem.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armSubmarine; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.ServoImplEx; + +public class ArmActionsSubsystem { + + public enum ArmState { + PARK, FLOOR, BUCKET, SUBMARINE + } + + private ServoImplEx arm; + private ArmState state; + + public ArmActionsSubsystem(HardwareMap hardwareMap) { + this.arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); + } + + public class MoveToPosition implements Action { + private double positionValue; + private ArmState positionState; + + public MoveToPosition(double positionValue, ArmState positionState) { + this.positionValue = positionValue; + this.positionState = positionState; + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + arm.setPosition(positionValue); + setState(positionState); + telemetryPacket.put("Arm State", positionState); + return false; + } + } + + public Action toParkPosition() { + return new MoveToPosition(armPark, ArmState.PARK); + } + + public Action toSubmarinePosition() { + return new MoveToPosition(armSubmarine, ArmState.SUBMARINE); + } + + public Action toFloorPosition() { + return new MoveToPosition(armFloor, ArmState.FLOOR); + } + + public Action toBucketPosition() { + return new MoveToPosition(armBucket, ArmState.BUCKET); + } + + public void setState(ArmState armState) { + this.state = armState; + } + + public ArmState getState() { + return this.state; + } + + public void init() { + this.arm.resetDeviceConfigurationForOpMode(); + Actions.runBlocking(this.toParkPosition()); + } + + public void start() { + Actions.runBlocking(this.toParkPosition()); + } + + public double getPosition() { + return this.arm.getPosition(); + } + + public void setPosition(double position) { + this.arm.setPosition(position); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java new file mode 100644 index 0000000..071f746 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java @@ -0,0 +1,81 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +public class ClawActionsSubsystem { + + public enum ClawState { + CLOSED, OPEN + } + + private Servo claw; + private ClawState state; + + public ClawActionsSubsystem(HardwareMap hardwareMap) { + this.claw = hardwareMap.get(Servo.class, "claw-servo"); + } + + public class MoveToPosition implements Action { + private double positionValue; + private ClawState positionState; + + public MoveToPosition(double positionValue, ClawState positionState) { + this.positionValue = positionValue; + this.positionState = positionState; + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + setState(positionState); + claw.setPosition(positionValue); + telemetryPacket.put("Arm State", positionState); + return false; + } + } + + public Action openClaw() { + return new MoveToPosition(clawOpen, ClawState.OPEN); + } + + public Action closeClaw() { + return new MoveToPosition(clawClose, ClawState.CLOSED); + } + + public void setState(ClawState clawState) { + this.state = clawState; + } + + public ClawState getState() { + return this.state; + } + + public void switchState() { + if (state == ClawState.CLOSED) { + Actions.runBlocking(openClaw()); + } else if (state == ClawState.OPEN) { + Actions.runBlocking(closeClaw()); + } + } + + public void init() { + Actions.runBlocking(closeClaw()); + } + + public void start() { + Actions.runBlocking(closeClaw()); + } + + public double getPosition() { + return this.claw.getPosition(); + } + +} \ No newline at end of file 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 new file mode 100644 index 0000000..ec4fe83 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java @@ -0,0 +1,102 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +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; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class LiftActionsSubsystem { + + public DcMotor lift; + + public enum LiftState { + FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, SUBMARINE + } + + private LiftState liftState; + + public LiftActionsSubsystem(HardwareMap hardwareMap) { + lift = hardwareMap.get(DcMotor.class, "lift-motor"); + } + + public class MoveToPosition implements Action { + private int positionValue; + private LiftState positionState; + + public MoveToPosition(int positionValue, LiftState positionState) { + this.positionValue = positionValue; + this.positionState = positionState; + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + setState(positionState); + lift.setTargetPosition(positionValue); + lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + telemetryPacket.put("Lift State", positionState); + telemetryPacket.put("Lift Position", lift.getCurrentPosition()); + boolean result = lift.getTargetPosition() - 15 < lift.getCurrentPosition() && + lift.getCurrentPosition() < lift.getTargetPosition() + 15; + return !result; + } + } + + public Action toFloorPosition() { + return new MoveToPosition(liftToFloorPos, LiftState.FLOOR); + } + + public Action toLowBucketPosition() { + return new MoveToPosition(liftToLowBucketPos, LiftState.LOW_BUCKET); + } + + public Action toHighBucketPosition() { + return new MoveToPosition(liftToHighBucketPos, LiftState.HIGH_BUCKET); + } + + public void switchState() { + if (this.liftState == LiftState.FLOOR) { + Actions.runBlocking(toLowBucketPosition()); + } else if (this.liftState == LiftState.LOW_BUCKET) { + Actions.runBlocking(toHighBucketPosition()); + } else if (this.liftState == LiftState.HIGH_BUCKET) { + Actions.runBlocking(toFloorPosition()); + } + } + + public void init() { + lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lift.setDirection(DcMotorSimple.Direction.REVERSE); + lift.setPower(liftPower); + } + + private void setState(LiftState liftState) { + this.liftState = liftState; + } + + public LiftState getState() { + return this.liftState; + } + + public int getPosition() { + return lift.getCurrentPosition(); + } + + public void setPosition(int position) { + lift.setTargetPosition(position); + lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + public void start() { + Actions.runBlocking(toFloorPosition()); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java index 2b20c7e..3469a7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToSubmarinePos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; @@ -59,7 +59,7 @@ public class LiftSubsystem { public void toFloat() { this.setState(LiftState.FLOAT); - this.setTarget(liftToFloatPos); + this.setTarget(liftToSubmarinePos); } public void toLowBucket() { 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 new file mode 100644 index 0000000..91f1cf2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java @@ -0,0 +1,86 @@ +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 androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.ServoImplEx; + +public class WristActionsSubsystem { + + public enum WristState { + FLOOR, BUCKET + } + + public ServoImplEx wrist; + public WristState state; + + public WristActionsSubsystem(HardwareMap hardwareMap) { + this.wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo"); + } + + public class MoveToPosition implements Action { + private double positionValue; + private WristState positionState; + + public MoveToPosition(double positionValue, WristState positionState) { + this.positionValue = positionValue; + this.positionState = positionState; + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + setState(positionState); + wrist.setPosition(positionValue); + telemetryPacket.put("Wrist State", positionState); + return false; + } + } + + public Action toFloorPosition() { + return new MoveToPosition(wristFloor, WristState.FLOOR); + } + + public Action toBucketPosition() { + return new MoveToPosition(wristBucket, WristState.BUCKET); + } + + public void setState(WristState wristState) { + this.state = wristState; + } + + public void switchState() { + if (state == WristState.FLOOR) { + Actions.runBlocking(this.toBucketPosition()); + } else if (state == WristState.BUCKET) { + Actions.runBlocking(this.toFloorPosition()); + } + } + + public WristState getState() { + return this.state; + } + + public void init() { + wrist.resetDeviceConfigurationForOpMode(); + Actions.runBlocking(this.toFloorPosition()); + } + + public void start() { + Actions.runBlocking(this.toFloorPosition()); + } + + public void setPosition(double position) { + wrist.setPosition(position); + } + + public double getPosition() { + return wrist.getPosition(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java index c1dfa00..6ee3fcd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java @@ -2,31 +2,30 @@ package org.firstinspires.ftc.teamcode.util.action; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import java.util.Timer; -import java.util.TimerTask; - public class SleepAction implements Action { - private final double dt; - private boolean isFinished = false; + private double dt; + private double beginTs = -1.0; public SleepAction(double dt) { this.dt = dt; } + public static double now() { + return System.nanoTime() * 1e-9; + } + @Override public boolean run(TelemetryPacket p) { - if (!isFinished) { - Timer timer = new Timer(); - timer.schedule(new TimerTask() { - @Override - public void run() { - isFinished = true; - } - }, (long) (dt * 1000)); + double t; + if (beginTs < 0) { + beginTs = now(); + t = 0.0; } else { - isFinished = false; // Reset the flag after sleep is complete - return false; // Indicate that the action is finished + t = now() - beginTs; } - return true; // Indicate that the action is still running + boolean output = t < dt; + System.out.println(t + ":" + now() + ":" + beginTs + ":" + output); + + return t < dt; } } \ No newline at end of file diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 4061db7..1b04cd1 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -16,6 +16,9 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' -} + implementation "com.acmerobotics.roadrunner:ftc:0.1.14" + implementation "com.acmerobotics.roadrunner:core:1.0.0" + implementation "com.acmerobotics.roadrunner:actions:1.0.0" + implementation "com.acmerobotics.dashboard:dashboard:0.4.16" +}