From 0ef239ca91a19e324477386f140dad7486adf174 Mon Sep 17 00:00:00 2001 From: sihan Date: Wed, 28 Feb 2024 12:56:22 -0600 Subject: [PATCH] Pickup macro, teleop cleaned up a lil bit --- .../ftc/teamcode/hardware/Robot.java | 56 +++++++++++++++---- .../ftc/teamcode/opmodes/MainTeleOp.java | 46 +++++++-------- 2 files changed, 65 insertions(+), 37 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 30f9115..42403f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST; @@ -34,10 +35,11 @@ import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT; import static org.firstinspires.ftc.teamcode.util.Constants.WRIST; import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -67,6 +69,8 @@ public class Robot { @Getter private Slides slides; + GamepadEx controller2 = new GamepadEx(gamepad2); + public Robot init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); this.hang = new Hang().init(hardwareMap); @@ -237,29 +241,33 @@ public class Robot { public void wristScore() { this.wrist.setPosition(WRISTSCORE); } + } - public static class Claw { - private static final double CLAW_KP = 0.15; + public static boolean clawIsOpen; + public static class Claw { private Servo claw; public Claw init(HardwareMap hardwareMap) { this.claw = hardwareMap.get(Servo.class, CLAW); - this.claw.setPosition(CLOSE); + close(); return this; } public void close() { this.claw.setPosition(CLOSE); + clawIsOpen = false; } public void open() { this.claw.setPosition(OPEN); + clawIsOpen = true; } public void openStack() { this.claw.setPosition(BIGOPEN); + clawIsOpen = true; } public void setPos(double pos) { @@ -276,6 +284,14 @@ public class Robot { return this; } + public void LED() { + if (clawIsOpen) { + white(); + } else { + gold(); + } + } + public void gold() { this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_VIOLET); } @@ -304,25 +320,45 @@ public class Robot { } - public void pickupMacro(Gamepad gamepad, Runtime runtime) { + public void pickupMacro(GamepadEx gamepadEx, double runtime) { switch (pickupMacroState) { case IDLE: - if (gamepad.dpad_down) + if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)){ + pickupMacroState = pickupMacroStates.OPEN; + } break; case OPEN: + delay = runtime + .2; + this.getClaw().open(); + pickupMacroState = pickupMacroStates.DROP; break; case DROP: + if (runtime > delay) { + this.getArm().pickup(); + delay= runtime + .2; + pickupMacroState = pickupMacroStates.CLOSE; + } break; case CLOSE: + if (runtime > delay) { + this.getClaw().close(); + delay= runtime + .1; + pickupMacroState = pickupMacroStates.NEUTRAL; + } break; case NEUTRAL: + if (runtime > delay) { + this.getArm().armRest(); + pickupMacroState = pickupMacroStates.IDLE; + } break; } } - pickupMacroStates pickupMacroState = pickupMacroStates.IDLE; - enum pickupMacroStates{ + public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE; + + public enum pickupMacroStates{ IDLE, OPEN, DROP, @@ -331,9 +367,7 @@ public class Robot { } - double runtime; - - + double delay; public TrajectorySequenceBuilder getTrajectorySequenceBuilder() { this.drive.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java index 1d32585..f3382ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.opmodes; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; @@ -15,16 +17,11 @@ public class MainTeleOp extends OpMode { this.robot = new Robot().init(hardwareMap); } - @Override + GamepadEx controller2 = new GamepadEx(gamepad2); + public void loop() { boolean slideUp = gamepad2.dpad_up; - boolean restArm = gamepad2.x; - boolean pickupArm = gamepad2.dpad_down; - boolean scoreArm = gamepad2.a; boolean plane = gamepad2.right_bumper; - boolean claw = gamepad2.b; - boolean pickupWrist = gamepad2.x; - boolean scoreWrist = gamepad2.a; boolean slideDown = gamepad2.dpad_left; boolean hang = gamepad2.left_bumper; boolean hangRelease = gamepad2.y; @@ -39,27 +36,23 @@ public class MainTeleOp extends OpMode { } else { this.robot.getSlides().slideStop(); } -//Arm - if (pickupArm) { - this.robot.getArm().pickup(); - } else if (restArm) { - this.robot.getArm().armRest(); - } else if (scoreArm) { + +//Macros + this.robot.getLed().LED(); + this.robot.pickupMacro(controller2,getRuntime()); + +//Arm and Wrist + if (controller2.wasJustPressed(GamepadKeys.Button.X)){ this.robot.getArm().armSecondaryScore(); + this.robot.getWrist().wristScore(); + } else if (controller2.wasJustPressed(GamepadKeys.Button.A)){ + this.robot.getArm().armRest(); + this.robot.getWrist().wristPickup(); + this.robot.getClaw().close(); } //Claw - if (claw) { + if (controller2.wasJustPressed(GamepadKeys.Button.B)){ this.robot.getClaw().open(); - this.robot.getLed().white(); - } else { - this.robot.getClaw().close(); - this.robot.getLed().gold(); - } -//Wrist - if (pickupWrist) { - this.robot.getWrist().wristPickup(); - } else if (scoreWrist) { - this.robot.getWrist().wristScore(); } //Hang if (hang) { @@ -68,17 +61,18 @@ public class MainTeleOp extends OpMode { this.robot.getHang().hangRelease(); } else if (hangPlane) { this.robot.getHang().hangPlane(); - } else { - this.robot.getHang().hangIdle(); } + int Position = this.robot.getHang().hangRight.getCurrentPosition(); telemetry.addData("position",(Position)); telemetry.update(); + //Plane if (plane) { this.robot.getPlane().planeLaunch(); }else { this.robot.getPlane().planeLock(); } + } } \ No newline at end of file