From dd01c4ce1a5d23fc6b3c51b9807506a01eedb72a Mon Sep 17 00:00:00 2001 From: Xander Haemel Date: Wed, 29 Jan 2025 19:09:28 -0800 Subject: [PATCH 1/2] Commit Hanging Function in the new TeleOp --- .../ftc/teamcode/CometBotDevAuto.java | 24 ---- .../teamcode/CometBotTeleOpCompetition.java | 27 +++++ ...nt.java => CometBotTeleOpDevelopment.java} | 114 ++++++++---------- .../paths/CometBotTeleopDriveV2.java | 2 + .../ftc/teamcode/configs/RobotConstants.java | 17 +-- .../ftc/teamcode/subsystem/ArmSubsystem.java | 2 +- .../subsystem/DualMotorSliderSubsystem.java | 5 +- .../subsystem/HangMotorSubsystem.java | 6 +- .../ftc/teamcode/subsystem/HookTest.java | 57 +++++++-- 9 files changed, 139 insertions(+), 115 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{CometBotAutoDevelopment.java => CometBotTeleOpDevelopment.java} (66%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java deleted file mode 100644 index 51270ee..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment; - -@TeleOp(name = "CometBot Auto v2", group = "Development") -public class CometBotDevAuto extends OpMode { - - public CometBotAutoDevelopment runMode; - - @Override - public void init() { - runMode = new CometBotAutoDevelopment(hardwareMap, gamepad1, gamepad2); - runMode.init(); - } - - @Override - public void loop() { - runMode.update(); - telemetry.update(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java new file mode 100644 index 0000000..4713f2f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleOpDevelopment; + +@TeleOp(name = "CometBot Drive V2.137", group = "Competition") +public class CometBotTeleOpCompetition extends OpMode { + + public CometBotTeleOpDevelopment runMode; + + @Override + public void init() { + runMode = new CometBotTeleOpDevelopment(hardwareMap, gamepad1, gamepad2); + runMode.init(); + } + + @Override + public void loop() { + runMode.update(); + telemetry.update(); + } + public void stop(){ + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java similarity index 66% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java index 1a8e6dc..7963dd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java @@ -2,27 +2,17 @@ package org.firstinspires.ftc.teamcode.cometbots; import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; -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.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.ArmSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; -public class CometBotAutoDevelopment { +public class CometBotTeleOpDevelopment { /* Subsystems @@ -31,7 +21,7 @@ public class CometBotAutoDevelopment { private ClawSubsystem claw; private WristSubsystem wrist; private ArmSubsystem arm; - private HangMotorSubsystem hang; + private HangMotorSubsystem skyhook; /* Controllers */ @@ -45,12 +35,12 @@ public class CometBotAutoDevelopment { private Follower follower; - public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) { + public CometBotTeleOpDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) { dualSlides = new DualMotorSliderSubsystem(hardwareMap); claw = new ClawSubsystem(hardwareMap); arm = new ArmSubsystem(hardwareMap); wrist = new WristSubsystem(hardwareMap); - hang = new HangMotorSubsystem(hardwareMap); + skyhook = new HangMotorSubsystem(hardwareMap); this.gamepad1 = gamepad1; this.gamepad2 = gamepad2; @@ -89,41 +79,14 @@ public class CometBotAutoDevelopment { openClaw(); openThumb(); //hang - grabBlueberrySkyhook(); - hangSkyhook(); - robotDown(); - robotUp(); - stopHook(); + + hang(); 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(); - } - /*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); -// } -// } - } - } private void openClaw() { if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) { @@ -175,27 +138,6 @@ public class CometBotAutoDevelopment { } } - 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) { - hang.robotToFloor(); - } - } - - private void stopHook() { - if (gamepad1.left_bumper) { - hang.disableMotor(); - } - } - private void dualSlidesToFloorPosition() { if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { dualSlides.toFloorPosition(); @@ -207,5 +149,49 @@ public class CometBotAutoDevelopment { dualSlides.toLowBucketPosition(); } } + private void hang(){ + if (gamepad1.a) { + claw.grabBlueberry(); + arm.setPosition(0.15); + arm.grabBlueberrySkyhook(); + + //claw Open small amount + wrist.grabBlueberrySkyhook(); + //wrist grab in strange way + } + if(gamepad1.b) { + //confirm grab + claw.closeClaw(); + + } + + if (gamepad1.x) { + //now slap on bar, first wrist, then arm, then claw then driver must drive away + dualSlides.toFixedPosition(500); + dualSlides.update(); + } + if (gamepad1.y) { + arm.hangBlueberrySkyhook(); + wrist.hangBlueberrySkyhook(); + try { + Thread.sleep(500); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + dualSlides.toFixedPosition(1800); + dualSlides.update(); + } + if (gamepad1.right_bumper) { + claw.openClaw(); + } + if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) { + skyhook.hangRobot(); + } + skyhook.setPower(0); + if (gamepad1.dpad_down) { + skyhook.disableMotor(); + skyhook.setPower(1); + } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java index a398d3f..3f7ceab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java @@ -1,10 +1,12 @@ package org.firstinspires.ftc.teamcode.cometbots.paths; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition") +@Disabled public class CometBotTeleopDriveV2 extends OpMode { public CometBotTeleopCompetition runMode; 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 4dbb6a7..38b1398 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,20 +4,23 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { - public final static double clawClose = 0.95; + public final static double clawClose = 1; public final static double clawOpen = 0.05; + + public final static double armFloor = 0.7; public final static double armSubmarine = 0.55; public final static double armReverseBucket = 0.08; public final static double armPark = 0.33; //value for grabbing the hook Specimen - public final static double grabBlueberry = 0.35; - public final static double armGrabBlueberrySkyhook = 0.1; - public final static double wristGrabBlueberrySkyhook = 0.15; - public final static double armHangBlueberrySkyhook = 0.23; - public final static double wristHangBlueberrySkyhook = 0.7; + public final static double grabBlueberry = 0.69; + public final static double armGrabBlueberrySkyhook = 0.05; + public final static double wristGrabBlueberrySkyhook = 0.1; + public final static double armHangBlueberrySkyhook = 0.18; + public final static double wristHangBlueberrySkyhook = 0; public final static int slideHangBlueberrySkyhook = 500; + public final static int slideBelowFloor = -150; public final static int slideSpecimanHang = 900; public final static int slideSpecimanRelease = 200; @@ -38,7 +41,7 @@ public class RobotConstants { public final static double wristSpeciemen = 0.1; - public final static int toBar = 500; + public final static int toBar = -7000; public final static double wristtravel = 0.525; public final static int toFloor = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java index 0290f06..55ae281 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -27,7 +27,7 @@ public class ArmSubsystem { } public void grabBlueberrySkyhook(){ setState(ArmState.GRAB_BLUEBERRY_SKYHOOK); - arm.setPosition(armGrabBlueberrySkyhook+ 0.3); + arm.setPosition(armGrabBlueberrySkyhook); } public void hangBlueberrySkyhook(){ setState(ArmState.HANG_BLUEBERRY_SKYHOOK); 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 343c39b..b4dde68 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 @@ -142,10 +142,7 @@ public class DualMotorSliderSubsystem { public void toFixedPosition(int value) { setTargetPosition(value); } - public void toHangHeight(){ - setTargetPosition(slideHangBlueberrySkyhook); - - } + public void toHangHeight(){setTargetPosition(slideHangBlueberrySkyhook);} public void toHangBelowFloor(){ setTargetPosition(slideBelowFloor); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java index 414fdb3..94e0558 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java @@ -30,15 +30,17 @@ public class HangMotorSubsystem { } public void hangRobot(){ + hang.setMode(DcMotor.RunMode.RUN_TO_POSITION); hang.setTargetPosition(toBar); - + while(hang.isBusy()){ + hang.setPower(1); + } } public void robotToFloor(){ hang.setTargetPosition(toFloor); } public void disableMotor(){ hang.setPower(0); - hang.setMotorDisable(); } public void setPower(double power){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java index 20114d6..b19abc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; @@ -8,8 +9,9 @@ import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; - +@TeleOp(name = "Hook Test") public class HookTest extends OpMode { + private HangMotorSubsystem skyhook; private ArmSubsystem arm; private WristSubsystem wrist; @@ -26,37 +28,66 @@ public class HookTest extends OpMode { arm.initTeleOp(); wrist.initTeleOp(); slides.init(); + claw.thumbUp(); } @Override public void loop() { + telemetry.addData("hook", skyhook.getCurrentPosition()); telemetry.addData("arm", arm.getState()); telemetry.addData("wrist", wrist.getState()); + telemetry.addData("slides", slides.getCurrentPosition()); telemetry.update(); - if(gamepad1.a){ + slides.update(); + + + if (gamepad1.a) { + claw.grabBlueberry(); + arm.setPosition(0.15); arm.grabBlueberrySkyhook(); telemetry.update(); + //claw Open small amount + wrist.grabBlueberrySkyhook(); + //wrist grab in strange way + telemetry.update(); + + + } + if(gamepad1.b) { + //confirm grab + claw.closeClaw(); + + } + + if (gamepad1.x) { + //now slap on bar, first wrist, then arm, then claw then driver must drive away + slides.toFixedPosition(500); + slides.update(); + } + if (gamepad1.y) { + arm.hangBlueberrySkyhook(); + wrist.hangBlueberrySkyhook(); try { Thread.sleep(500); } catch (InterruptedException e) { throw new RuntimeException(e); } - //claw Open small amount - wrist.grabBlueberrySkyhook(); - //wrist grab in strange way - telemetry.update(); + slides.toFixedPosition(1800); + slides.update(); } - if(gamepad1.b){ - //after confirmed grab, close claw - claw.closeClaw(); + if (gamepad1.right_bumper) { + claw.openClaw(); } - if(gamepad1.x){ - //now slap on bar, first wrist, then arm, then claw then driver must drive away + if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) { + skyhook.hangRobot(); } - if(gamepad1.y){ - //go up + skyhook.setPower(0); + if (gamepad1.dpad_down) { + skyhook.disableMotor(); + skyhook.setPower(1); } + } } From 884e232b2bd9e360f92d6f5a50b57222f2ae95c9 Mon Sep 17 00:00:00 2001 From: Xander Haemel Date: Wed, 29 Jan 2025 20:44:57 -0800 Subject: [PATCH 2/2] fixed bug --- .../ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java index 7963dd7..ab841d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java @@ -56,6 +56,7 @@ public class CometBotTeleOpDevelopment { claw.init(); wrist.initTeleOp(); arm.initTeleOp(); + skyhook.init(); follower.setMaxPower(MAX_POWER); follower.startTeleopDrive(); }