From b14e91b0940b13964ccdb2fa71aca401c5e311f3 Mon Sep 17 00:00:00 2001 From: robotics3 Date: Thu, 23 Jan 2025 16:12:59 -0800 Subject: [PATCH] Commit Hang Subsystem new --- .../cometbots/CometBotAutoDevelopment.java | 42 ++++++++++++++++--- .../ftc/teamcode/configs/RobotConstants.java | 10 +++-- .../subsystem/DualMotorSliderSubsystem.java | 10 +++++ 3 files changed, 52 insertions(+), 10 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 f2a27c9..ece2fcb 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 @@ -18,6 +18,7 @@ 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; @@ -30,6 +31,7 @@ public class CometBotAutoDevelopment { private ClawSubsystem claw; private WristSubsystem wrist; private ArmSubsystem arm; + private HangMotorSubsystem hang; /* Controllers */ @@ -48,6 +50,7 @@ public class CometBotAutoDevelopment { claw = new ClawSubsystem(hardwareMap); arm = new ArmSubsystem(hardwareMap); wrist = new WristSubsystem(hardwareMap); + hang = new HangMotorSubsystem(hardwareMap); this.gamepad1 = gamepad1; this.gamepad2 = gamepad2; @@ -87,7 +90,10 @@ public class CometBotAutoDevelopment { openThumb(); //hang grabBlueberrySkyhook(); - + hangSkyhook(); + robotDown(); + robotUp(); + stopHook(); follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); follower.update(); } @@ -97,10 +103,26 @@ public class CometBotAutoDevelopment { wrist.grabBlueberrySkyhook(); arm.grabBlueberrySkyhook(); } - if(currentGamepad1.y){ - wrist.hangBlueberrySkyhook(); - arm.hangBlueberrySkyhook(); + if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK) + claw.init(); + } + private void hangSkyhook (){ + dualSlides.toHangHeight(); + wrist.hangBlueberrySkyhook(); + try { + Thread.sleep(1000); + } catch (InterruptedException e) { + throw new RuntimeException(e); } + arm.hangBlueberrySkyhook(); + if(arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK){ + try { + Thread.sleep(1000); + } catch (InterruptedException e) { + throw new RuntimeException(e); + }claw.closeClaw(); + } + } private void openClaw(){ @@ -155,12 +177,20 @@ 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() { 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 488dde1..9345f6d 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 @@ -12,11 +12,13 @@ public class RobotConstants { 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.3; + public final static double grabBlueberry = 0.35; public final static double armGrabBlueberrySkyhook = 0.15; - public final static double wristGrabBlueberrySkyhook = 0.1; - public final static double armHangBlueberrySkyhook = 0.6; - public final static double wristHangBlueberrySkyhook = 0.3; + public final static double wristGrabBlueberrySkyhook = 0.15; + public final static double armHangBlueberrySkyhook = 0.23; + public final static double wristHangBlueberrySkyhook = 0.7; + public final static int slideHangBlueberrySkyhook = 500; + public final static int slideBelowFloor = -150; public final static int backwardBucketDrop = 4670; public final static double armBucket = 0.45; public final static double armInit = 0.135; 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 2a43bd5..0c735e1 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 @@ -5,6 +5,8 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOT import static org.firstinspires.ftc.teamcode.configs.RobotConstants.backwardBucketDrop; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideBelowFloor; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -138,6 +140,14 @@ public class DualMotorSliderSubsystem { public void toFixedPosition(int value) { setTargetPosition(value); } + public void toHangHeight(){ + setTargetPosition(slideHangBlueberrySkyhook); + + } + public void toHangBelowFloor(){ + setTargetPosition(slideBelowFloor); + + } public int getFixedPosition() { return liftSlideLeft.getCurrentPosition();