From b72a2756ca523955cf7ed7facfc0d544d12197f3 Mon Sep 17 00:00:00 2001 From: "A. Teo Welton" <76081718+DragonDev07@users.noreply.github.com> Date: Fri, 23 Feb 2024 19:25:25 -0700 Subject: [PATCH] Endgame Button --- .../ftc/teamcode/subsystems/HWC.java | 5 +- .../teamcode/teleop/SingleDriverTeleOp.java | 74 +++++++++---------- 2 files changed, 40 insertions(+), 39 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java index e3fd4df..a52f998 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java @@ -42,6 +42,7 @@ public class HWC { public static double passoverIntakePos = 0.8; public static double wristDeliveryPos = 0.2; public static double wristIntakePos = 0.8; + public static double droneLaunchPos = 0.5; // ------ Declare Motors ------ // public DcMotorEx leftFront, rightFront, leftRear, rightRear, rightPulley, leftPulley, intakeMotor; @@ -50,7 +51,7 @@ public class HWC { public RobotComponents pulleyLComponent, pulleyRComponent; // ------ Declare Servos ------ // - public Servo wrist, clawR, clawL, passoverArmLeft, passoverArmRight; + public Servo wrist, clawR, clawL, passoverArmLeft, passoverArmRight, drone; // ------ Declare Sensors ------ // public ColorRangeSensor colorLeft, colorRight; @@ -110,6 +111,7 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) { wrist = hardwareMap.get(Servo.class, "wrist"); passoverArmLeft = hardwareMap.get(Servo.class, "passoverArmLeft"); passoverArmRight = hardwareMap.get(Servo.class, "passoverArmRight"); + drone = hardwareMap.get(Servo.class, "drone"); // ------ Retrieve Sensors ------ // webcam = hardwareMap.get(WebcamName.class, "webcam"); @@ -123,7 +125,6 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) { rightRear.setDirection(DcMotorEx.Direction.FORWARD); leftPulley.setDirection(DcMotorEx.Direction.REVERSE); - // ------ Set Motor Brake Modes ------ // leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java index 2a90fee..079cb51 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SingleDriverTeleOp.java @@ -15,12 +15,16 @@ @TeleOp(name = "Single Driver", group = "Primary OpModes") public class SingleDriverTeleOp extends OpMode { // ------ Intake State Enum ------ // - private enum IntakeState {INTAKE, OFF, OUTTAKE} + private enum IntakeState { INTAKE, OFF, OUTTAKE } + + // ------ Endgame State Enum ------ // + private enum EndgameState { DRONE, SLIDES_UP, CLIMB } // ------ Declare Others ------ // private HWC robot; private MultiplierSelection selection = MultiplierSelection.TURN_SPEED; private IntakeState intakeState = IntakeState.OFF; + private EndgameState endgameState = EndgameState.DRONE; private boolean testingMode = false; private double turnSpeed = 0.5; // Speed multiplier for turning private double driveSpeed = 1; // Speed multiplier for driving @@ -47,8 +51,10 @@ public void init() { // ------ Reset Servos ------ // robot.clawL.setPosition(1); robot.clawR.setPosition(0); + robot.drone.setPosition(0); robot.wrist.setPosition(HWC.wristIntakePos); robot.passoverArmLeft.setPosition(HWC.passoverIntakePos); + robot.passoverArmRight.setPosition(HWC.passoverIntakePos); // ------ Set Pulley Run Modes ------ // robot.leftPulley.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -191,6 +197,33 @@ public void loop() { } } + // ------ (GAMEPAD 1) Endgame Controls ------ // + if(robot.currentGamepad1.back && !robot.previousGamepad1.back) { + switch(endgameState) { + case DRONE: + // Launch Drone + robot.drone.setPosition(HWC.droneLaunchPos); + + // Set Next State + endgameState = EndgameState.SLIDES_UP; + + break; + case SLIDES_UP: + // Set Slide Height + slideHeight = 3; + + // Set Next State + endgameState = EndgameState.CLIMB; + + break; + case CLIMB: + // Set Slide Height + slideHeight = 0; + + break; + } + } + // ------ (GAMEPAD 1) Slide Position Controls ------ // if (robot.currentGamepad1.dpad_up && !robot.previousGamepad1.dpad_up) { // Increment Value @@ -212,7 +245,7 @@ public void loop() { } } - // ------ (GAMEPAD 1) Position Controls ------ // + // ------ (GAMEPAD 1) Passover & Claw Position Controls ------ // if (robot.currentGamepad1.b && !robot.previousGamepad1.b) { deliveryPosition(); } else if (robot.currentGamepad1.a && !robot.previousGamepad1.a) { @@ -304,41 +337,8 @@ public void loop() { // ------ Telemetry Updates ------ // telemetry.addData("Status", "Running"); telemetry.addData("Intake State", intakeState); - telemetry.addLine(); - telemetry.addData(">", "ALL PRIMARY CONTROLS ARE ON GAMEPAD 1"); - telemetry.addData("Left Stick X", "Strafing Left / Right"); - telemetry.addData("Left Stick Y", "Driving Forward / Backward"); - telemetry.addData("Left Trigger / Right Trigger", "Turning Left / Right"); - telemetry.addData("Left Bumper", "Outtake"); - telemetry.addData("Right Bumper", "Intake"); - telemetry.addData("Both Bumpers", "Intake OFF"); - telemetry.addData("D-Pad Up / Down", "Wrist Up / Down"); - telemetry.addData("Right Stick X", "Intake In / Out"); - telemetry.addData("Right Stick Y", "Slides Up / Down"); - telemetry.addData("Button B", "Delivery Position"); - telemetry.addData("Button A", "Intake Position"); - telemetry.addData("Button X", "Toggle Left Claw"); - telemetry.addData("Button Y", "Toggle Right Claw"); - telemetry.addLine(); - telemetry.addData("Intake Motor Power", robot.intakeMotor.getPower()); - telemetry.addData("Slide Pulley Left Position", robot.leftPulley.getCurrentPosition()); - telemetry.addData("Slide Pulley Right Position", robot.rightPulley.getCurrentPosition()); - telemetry.addData("Claw Left Position", robot.clawL.getPosition()); - telemetry.addData("Claw Right Position", robot.clawR.getPosition()); - telemetry.addData("Left Passover Position", robot.passoverArmLeft.getPosition()); - telemetry.addData("Right Passover Position", robot.passoverArmRight.getPosition()); - telemetry.addData("Wrist Position", robot.wrist.getPosition()); - telemetry.addData("Left Pulley Position", robot.leftPulley.getPower()); - telemetry.addData("Right Pulley Position", robot.rightPulley.getPower()); - telemetry.addData("> Target Wrist Position", wristPosition); - telemetry.addData("> Target Passover Position", passoverPosition); - telemetry.addLine(); - telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFPower, rightFPower); - telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBPower, rightBPower); - telemetry.addData("Front Left Position", robot.leftFront.getCurrentPosition()); - telemetry.addData("Back Left Position", robot.leftRear.getCurrentPosition()); - telemetry.addData("Front Right Position", robot.rightFront.getCurrentPosition()); - telemetry.addData("Back Right Position", robot.rightRear.getCurrentPosition()); + telemetry.addData("Endgame State", endgameState); + telemetry.addData("Slide Height", slideHeight); // ------ Testing Mode Telemetry ------ // if (testingMode) {