From e1be70c23f689ba93d3770681861d4e85dbe1fec Mon Sep 17 00:00:00 2001 From: robotics1 Date: Mon, 20 Jan 2025 12:41:48 -0800 Subject: [PATCH] Need path tuning high basket auto path 1 (I need oranges) --- .../ftc/teamcode/PedroConstants.java | 10 +- .../cometbots/CometBotAutoDevelopment.java | 2 +- .../cometbots/paths/CometBotDriveV2.java | 310 ++++++++++++++++++ .../cometbots/paths/HighBasketPath1.java | 38 +++ .../ftc/teamcode/cometbots/tests/ArmTest.java | 2 +- .../teamcode/cometbots/tests/WristTest.java | 2 +- .../ftc/teamcode/configs/RobotConstants.java | 12 +- .../tuning/FollowerConstants.java | 6 +- .../ftc/teamcode/subsystem/ClawSubsystem.java | 13 +- .../subsystem/DualMotorSliderSubsystem.java | 16 +- .../ftc/teamcode/subsystem/LiftArmWrist.java | 131 ++++++++ 11 files changed, 519 insertions(+), 23 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index 2b15078..ce2912d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -46,7 +46,7 @@ public class PedroConstants { // Robot encoder direction public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD; public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE; - public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD; + public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE; // Arm config public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left"; @@ -64,16 +64,16 @@ public class PedroConstants { public static final double ROBOT_WEIGHT_IN_KG = 9; // Maximum velocity of the robot going forward - public static final double ROBOT_SPEED_FORWARD = 53.223; + public static final double ROBOT_SPEED_FORWARD = 50.02; // Maximum velocity of the robot going right - public static final double ROBOT_SPEED_LATERAL = 41.4081; + public static final double ROBOT_SPEED_LATERAL = 36.07; // Rate of deceleration when power is cut-off when the robot is moving forward - public static final double FORWARD_ZERO_POWER_ACCEL = -76.8421; + public static final double FORWARD_ZERO_POWER_ACCEL = -91.4557; // Rate of deceleration when power is cut-off when the robot is moving to the right - public static final double LATERAL_ZERO_POWER_ACCEL = -93.4183; + public static final double LATERAL_ZERO_POWER_ACCEL = -98.514; // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop public static final double ZERO_POWER_ACCEL_MULT = 4.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java index c9ecdb2..e82cfe3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java @@ -156,7 +156,7 @@ public class CometBotAutoDevelopment { } private void dualSlidesToFloorPosition() { if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { - dualSlides.toFloorPosotion(); + dualSlides.toFloorPosition(); } } private void dualSlidesToLowBucketPosition() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java new file mode 100644 index 0000000..7967cc4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java @@ -0,0 +1,310 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + +import static com.qualcomm.hardware.lynx.commands.core.LynxSetMotorPIDFControlLoopCoefficientsCommand.InternalMotorControlAlgorithm.Max; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +//import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition; +//import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +//import org.firstinspires.ftc.teamcode.subsystem.AutoPark; +import org.firstinspires.ftc.teamcode.cometbots.paths.HighBasketPath1; +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2; +//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3; +//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4; +//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5; +//import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6; +//import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; +//import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem; + + +@Autonomous(name = "Auto Test Competition V2", group = "Dev") +public class CometBotDriveV2 extends OpMode { + private Follower follower; + private int state; + + private HighBasketPath1 path1; + + + + //private CometBotTeleopCompetition comp; + private static ElapsedTime runtime; + private static boolean initalized = false; + private static boolean followingPath = false; + private DualMotorSliderSubsystem lift; + private ClawSubsystem claw; + private WristSubsystem wrist; + private ArmSubsystem arm; + + @Override + public void init() { + follower = new Follower(hardwareMap); + follower.setMaxPower(.75); + + path1 = new HighBasketPath1(); + + lift = new DualMotorSliderSubsystem(hardwareMap); + arm = new ArmSubsystem(hardwareMap); + wrist = new WristSubsystem(hardwareMap); + claw = new ClawSubsystem(hardwareMap); + + + + lift.init(); + arm.initAuto(); + wrist.initTeleOp(); + claw.init(); + + //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); + // comp.initCloseClaw(); + + } + + + public void loop() { + telemetry.addData("state", state); + telemetry.addData("followingPath", followingPath); + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + } + switch (state) { + case 0: + moveToPathOneAndHighBucket(); +// doArmThing1(); + break; + case 1: +// doArmThing2(); + break; +// case 2: +// moveToPathTwoAndPickSampleUp(); +// break; +// case 3: +// doPickUpThing(); +// break; +// case 4: +// moveToBasketPath3(); +// break; +// case 5: +// theArmThing(); +// break; +// case 6: +// moveToPickupAgainPath4(); +// break; +// case 7: +// doPickUpThingAgain(); +// break; +// case 8: +// moveToPickupAgainPath5(); +// break; +// case 9: +// //theArmThingAgain(); +// break; +// case 10: +// //moveToParkPath6(); +// break; +// case 11: + + + } + telemetry.update(); + + follower.update(); + + + } + + private void moveToPathOneAndHighBucket() { + if (!followingPath) { + runtime = new ElapsedTime(); + path1.moveToPath1(follower); + followingPath = true; + } + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + if (runtime.seconds() > 4) { + state = 1; + followingPath = false; + } + } + } + + public class SetStateAction implements Action { + + private int value; + + public SetStateAction(int value) { + this.value = value; + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + state = value; + return false; + } + } + + private Action setStateValue(int value) { + return new SetStateAction(value); + } + + private void doArmThing1() { + lift.toHighBucketPosition(); + arm.toBucketPosition(); + + state = 2; + } + private void doArmThing2() { + wrist.toBucketPosition(); + claw.openClaw(); + wrist.toFloorPosition(); + + state = 2; + } + +// private void theArmThing() { +// telemetry.addData("busy?", follower.isBusy()); +// telemetry.addData("end?", follower.atParametricEnd()); +// if (follower.atParametricEnd()){ +// follower.breakFollowing(); +// comp.highBucketDropAuto(); +// state = 6; +// } +// follower.breakFollowing(); +// } + +// private void theArmThingAgain() { +// follower.breakFollowing(); +// comp.highBucketDropAuto(); +// state = 10; +// } + +// private void moveToPathTwoAndPickSampleUp() { +// if (!followingPath) { +// path2.moveToPath2(follower); +// followingPath = true; +// } +// if (runtime != null) { +// telemetry.addData("Runtime (seconds)", runtime.seconds()); +// if (follower.atParametricEnd() || runtime.seconds() > 22.0) { +// state = 3; +// followingPath = false; +// } +// } +// } +// +// private void moveToPickupAgainPath4() { +// if (!followingPath) { +// path4.moveToPickupAgainPath4(follower); +// followingPath = true; +// } +// if (runtime != null) { +// telemetry.addData("Runtime (seconds)", runtime.seconds()); +// if (follower.atParametricEnd() || runtime.seconds() > 27.0) { +// state = 7; +// followingPath = false; +// } +// } +// } +// +// private void moveToPickupAgainPath5() { +// if (!followingPath) { +// path5.moveToPickupAgainPath5(follower); +// followingPath = true; +// } +// if (runtime != null) { +// telemetry.addData("Runtime (seconds)", runtime.seconds()); +// if (follower.atParametricEnd() || runtime.seconds() > 36.0) { +// state = 9; +// followingPath = false; +// } +// } +// } +// private void moveToParkPath6() { +// if (!followingPath) { +// path6.moveToParkPath6(follower); +// followingPath = true; +// } +// if (runtime != null) { +// telemetry.addData("Runtime (seconds)", runtime.seconds()); +// if (follower.atParametricEnd() || runtime.seconds() > 48.0) { +// state = 11; +// followingPath = false; +// } +// } +// } +// +// private void moveToBasketPath3() { +// if (!followingPath) { +// path3.moveToBasketPath3(follower); +// followingPath = true; +// } +// if (runtime != null) { +// telemetry.addData("Runtime (seconds)", runtime.seconds()); +// if (follower.atParametricEnd() || runtime.seconds() > 10.0) { +// state = 5; +// followingPath = false; +// } +// } +// } +// +// private void thePickUpAuto() { +// Actions.runBlocking(new SequentialAction( +// new SleepAction(.1), +// comp.claw.openClaw(), +// new SleepAction(.2), +// comp.wrist.toPickupPosition(), +// new SleepAction(.2), +// comp.arm.toSubmarinePosition(), +// new SleepAction(.5), +// comp.claw.closeClaw(), +// new SleepAction(.3), +// comp.wrist.toFloorPosition(), +// new SleepAction(.2), +// comp.arm.toParkPosition(), +// new SleepAction(.2) +// )); +// } +// private void thePickUp() { +// Actions.runBlocking(new SequentialAction( +// new SleepAction(.5), +// comp.wrist.toPickupPosition(), +// new SleepAction(.5), +// comp.arm.toSubmarinePosition(), +// new SleepAction(.7), +// comp.claw.closeClaw(), +// new SleepAction(.7), +// comp.wrist.toFloorPosition(), +// new SleepAction(.5), +// comp.arm.toParkPosition(), +// new SleepAction(.5) +// )); +// } +// +// private void doPickUpThing() { +// follower.breakFollowing(); +// thePickUpAuto(); +// state = 4; +// } +// +// private void doPickUpThingAgain() { +// follower.breakFollowing(); +// thePickUpAuto(); +// state = 8; +// } +// +// +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java new file mode 100644 index 0000000..235802e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + + +/* + AutoLine# - This file does something of a path...... + + */ +public class HighBasketPath1 { + + private final Pose startPose = new Pose(8, 89); + + public void moveToPath1(Follower robot) { + PathChain pathChain; + robot.setStartingPose(startPose); + PathBuilder builder = new PathBuilder(); + builder + .addPath( + new BezierCurve( + new Point(8.000, 89.000, Point.CARTESIAN), + new Point(24.000, 96.000, Point.CARTESIAN), + new Point(18.000, 126.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java index 8f9c131..19872b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java @@ -56,7 +56,7 @@ public class ArmTest extends LinearOpMode { Gamepad currentGamepad1 = new Gamepad(); Gamepad previousGamepad1 = new Gamepad(); - arm.init(); + arm.initTeleOp(); waitForStart(); runtime.reset(); 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 e8fd1ff..980c22c 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 @@ -57,7 +57,7 @@ public class WristTest extends LinearOpMode { Gamepad currentGamepad1 = new Gamepad(); Gamepad previousGamepad1 = new Gamepad(); - wrist.init(); + wrist.initTeleOp(); waitForStart(); runtime.reset(); 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 4f39023..bbfae83 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,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { - public final static double clawClose = 0.85; + public final static double clawClose = 0.95; public final static double clawOpen = 0.05; public final static double armFloor = 0.7; @@ -19,9 +19,9 @@ public class RobotConstants { public final static double armBucket = 0.45; public final static double armInit = 0.125; public final static double wristInit = 0.0; - public final static double wristPickup = 0.45; - public final static double wristBucket = 0.6; - public final static double wristFloor = 0.85; + public final static double wristPickup = 0.475; + public final static double wristBucket = 0.56; + public final static double wristFloor = 0.75; public final static double wristRung = 0.55; @@ -30,13 +30,13 @@ public class RobotConstants { public final static int liftToFloorPos = 350; public final static int liftToSubmarinePos = 350; - public final static int liftToLowBucketPos = 2650; + public final static int liftToLowBucketPos = 1750; public final static int liftToHighRung = 2100; public final static int dropToHighRung = 1675; public final static int liftToHighRungAttach = 1050; - public final static int liftToHighBucketPos = 4900; + public final static int liftToHighBucketPos = 4000; public final static double liftPower = 1; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index 0c325d9..f1bfdc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -31,7 +31,7 @@ public class FollowerConstants { public static String rightFrontMotorName = FRONT_RIGHT_MOTOR; public static String rightRearMotorName = BACK_RIGHT_MOTOR; - public static DcMotorSimple.Direction leftFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION; + public static DcMotorSimple.Direction leftFrontMotorDirection = FRONT_LEFT_MOTOR_DIRECTION; public static DcMotorSimple.Direction rightFrontMotorDirection = FRONT_RIGHT_MOTOR_DIRECTION; public static DcMotorSimple.Direction leftRearMotorDirection = BACK_LEFT_MOTOR_DIRECTION; public static DcMotorSimple.Direction rightRearMotorDirection = BACK_RIGHT_MOTOR_DIRECTION; @@ -48,7 +48,7 @@ public class FollowerConstants { public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients( .25, 0, - 0.0375, + 0.02, 0); // Translational Integral @@ -75,7 +75,7 @@ public class FollowerConstants { // Drive PIDF coefficients public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients( - 0.00375, + 0.00235, 0, 0.00003, 0.8, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java index 08f2b01..16f186e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java @@ -16,7 +16,7 @@ public class ClawSubsystem { } public enum ThumbState { - DOWN, UP + DOWN, UP, AWAY } private Servo claw; @@ -34,11 +34,13 @@ public class ClawSubsystem { } public void closeClaw() { claw.setPosition(clawClose); + thumbDown(); state = ClawState.CLOSED; } public void openClaw() { claw.setPosition(clawOpen); + thumbUp(); state = ClawState.OPEN; } @@ -72,13 +74,18 @@ public class ClawSubsystem { } public void thumbUp() { - thumb.setPosition(0.95); + thumb.setPosition(0.5); tState = ThumbState.UP; } public void thumbDown() { - thumb.setPosition(0.05); + thumb.setPosition(0.00); tState = ThumbState.DOWN; } + public void thumbOutOfTheWay() { + thumb.setPosition(0.95); + tState = ThumbState.AWAY; + } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java index 950dbb5..7172afa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java @@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -123,13 +125,21 @@ public class DualMotorSliderSubsystem { } public void toLowBucketPosition() { - setTargetPosition(2000); + setTargetPosition(liftToLowBucketPos); } public void toHighBucketPosition() { - setTargetPosition(4000); + setTargetPosition(liftToHighBucketPos); } - public void toFloorPosotion(){setTargetPosition(0);} + public void toFixedPosition(int value) { + setTargetPosition(value); + } + + public int getFixedPosition() { + return liftSlideLeft.getCurrentPosition(); + } + + public void toFloorPosition(){setTargetPosition(0);} } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java new file mode 100644 index 0000000..dfde281 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java @@ -0,0 +1,131 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +/* 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. + */ + +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.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; + + +@TeleOp(name = "Lift Wrist Arm Test", group = "Debug") +public class LiftArmWrist extends LinearOpMode { + + private final ElapsedTime runtime = new ElapsedTime(); + + @Override + public void runOpMode() { + + /* + * Instantiate Lift + */ + DualMotorSliderSubsystem lift = new DualMotorSliderSubsystem(hardwareMap); + WristSubsystem wrist = new WristSubsystem(hardwareMap); + ArmSubsystem arm = new ArmSubsystem(hardwareMap); + ClawSubsystem claw = new ClawSubsystem(hardwareMap); + + + /* + * Instantiate gamepad state holders + */ + Gamepad currentGamepad1 = new Gamepad(); + Gamepad previousGamepad1 = new Gamepad(); + + lift.init(); + wrist.initTeleOp(); + arm.initTeleOp(); + claw.init(); + waitForStart(); + runtime.reset(); + + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + + 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.b && !previousGamepad1.b) { + claw.switchState(); + } + + if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) { + lift.toHighBucketPosition(); + } + + if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) { + lift.toLowBucketPosition(); + } + + if (currentGamepad1.y && !previousGamepad1.y) { + wrist.setPosition(wrist.getPosition() + .05); + } + + if (currentGamepad1.a && !previousGamepad1.a) { + wrist.setPosition(wrist.getPosition() - .05); + } + + if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { + lift.toHighBucketPosition(); + } + + if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { + lift.toLowBucketPosition(); + + } + + lift.update(); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Lift Drive Position", lift.getFixedPosition()); + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Arm Position", arm.getPosition()); + + telemetry.update(); + } + } +}