From 44448d642da2577dc0e2241958bbe139893c924c Mon Sep 17 00:00:00 2001 From: robotics1 Date: Tue, 28 Jan 2025 17:10:36 -0800 Subject: [PATCH] Added teleop code --- .../cometbots/CometBotAutoDevelopment.java | 95 ++++++++++--------- .../paths/CometBotTeleopCompetition.java | 37 +++++++- 2 files changed, 86 insertions(+), 46 deletions(-) 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 7043acd..1a8e6dc 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 @@ -60,7 +60,7 @@ public class CometBotAutoDevelopment { previousGamepad2 = new Gamepad(); follower = new Follower(hardwareMap); } - + public void init() { dualSlides.init(); claw.init(); @@ -97,71 +97,75 @@ public class CometBotAutoDevelopment { follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); follower.update(); } - private void grabBlueberrySkyhook () { - if(currentGamepad1.x){ - claw.grabBlueberry(); - wrist.grabBlueberrySkyhook(); - arm.grabBlueberrySkyhook(); + + private void grabBlueberrySkyhook() { + if (currentGamepad1.x) { + // claw.grabBlueberry(); + //wrist.grabBlueberrySkyhook(); + //arm.grabBlueberrySkyhook(); } /*if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK) claw.closeClaw();*/ } + private void hangSkyhook() { if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { dualSlides.toHangHeight(); //check wrist make go throught test files again - wrist.hangBlueberrySkyhook(); - arm.hangBlueberrySkyhook(); - if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) { - try { - Thread.sleep(2000); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } + // wrist.hangBlueberrySkyhook(); + // arm.hangBlueberrySkyhook(); +// if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) { +// try { +// Thread.sleep(2000); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } +// } } - }private void openClaw(){ - if(currentGamepad2.right_bumper && !previousGamepad2.right_bumper){ + } + + private void openClaw() { + if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) { claw.switchState(); } } - private void openThumb(){ - if(currentGamepad2.left_bumper && !previousGamepad2.left_bumper){ + + private void openThumb() { + if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) { claw.switchTState(); } } - private void armAndWristToFloor(){ - if(currentGamepad2.a && !previousGamepad2.a){ - double increment = 0.7 - arm.getPosition(); - for(int i = 0; i < 3; i ++){ - arm.setPosition(arm.getPosition() + increment); - try { - Thread.sleep(50); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } + + private void armAndWristToFloor() { + if (currentGamepad2.a && !previousGamepad2.a) { + arm.toFloorPositionTeleOp(); + if (wrist.getState() != WristSubsystem.WristState.FLOOR) { + wrist.toFloorPositionTeleop(); + } else { + wrist.toPickupPosition(); } - arm.toFloorPosition(); } - if(currentGamepad2.a && !previousGamepad2.a ){ + + if (currentGamepad2.a && !previousGamepad2.a) { wrist.switchState(); } - } - private void armToBucketPosition(){ - if(currentGamepad2.dpad_up && !previousGamepad2.dpad_up){ + + private void armToBucketPosition() { + if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) { arm.toBucketPosition(); wrist.toBucketPosition(); } } - private void armToParkPosition(){ - if(currentGamepad2.x && !previousGamepad2.x){ + + private void armToParkPosition() { + if (currentGamepad2.x && !previousGamepad2.x) { arm.toParkPosition(); - wrist.toFloorPosition(); + wrist.toFloorPositionTeleop(); } } @@ -170,29 +174,34 @@ public class CometBotAutoDevelopment { dualSlides.toHighBucketPosition(); } } - private void robotUp(){ - if(currentGamepad1.dpad_up && !previousGamepad1.dpad_up){ + + private void robotUp() { + if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) { dualSlides.toHangBelowFloor(); dualSlides.update(); hang.hangRobot(); } } - private void robotDown(){ - if(currentGamepad1.dpad_down && !previousGamepad1.dpad_down){ + + private void robotDown() { + if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) { hang.robotToFloor(); } } - private void stopHook(){ - if(gamepad1.left_bumper){ + + private void stopHook() { + if (gamepad1.left_bumper) { hang.disableMotor(); } } + private void dualSlidesToFloorPosition() { if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { dualSlides.toFloorPosition(); } } + private void dualSlidesToLowBucketPosition() { if (currentGamepad2.b && !previousGamepad2.b) { dualSlides.toLowBucketPosition(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java index a9a56b9..b1c3596 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java @@ -54,6 +54,11 @@ public class CometBotTeleopCompetition { */ public double currentPower = MAX_POWER; + /* + Misc + */ + private int hbCounter = 0; + public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55); @@ -175,12 +180,38 @@ public class CometBotTeleopCompetition { */ public void toHighBucketScore() { if (this.currentGP2.y && !this.previousGP2.y) { - lift.toHighBucketPosition(); + switch(hbCounter) { + case 0: + lift.toHighBucketPosition(); + hbCounter = 1; + break; + case 1: + wrist.toBucketPosition(); + arm.toBucketPosition(); + hbCounter = 2; + break; + case 2: + claw.openClaw(); + hbCounter = 3; + break; + case 3: + claw.closeClaw(); + wrist.toTravelPosition(); + arm.toParkPosition(); + hbCounter = 4; + break; + case 4: + lift.toFloorPosition(); + break; + default: + break; + } + hbCounter = 1; if (lift.getFixedPosition() >= 4450) { arm.toBucketPosition(); - if(arm.getPosition() == 0.45) + if(arm.getPosition() == 0.45); wrist.toBucketPosition(); - if(wrist.getPosition() == 0.56) + if(wrist.getPosition() == 0.56); claw.openClaw(); } }