From 1a878eea1c430d035e159bcae833d710eb73c360 Mon Sep 17 00:00:00 2001 From: robotics3 Date: Thu, 16 Jan 2025 17:07:41 -0800 Subject: [PATCH] Updated Blueberry Specimen Grabing code and new values. Reprogrammed a few servos also and I haven't tweaked the code yet. The Blueberry Specimen Grab is UNTESTED. CAUTION --- .../cometbots/CometBotAutoDevelopment.java | 24 +++++++++++++++---- .../ftc/teamcode/configs/RobotConstants.java | 10 ++++++-- .../ftc/teamcode/subsystem/ArmSubsystem.java | 12 +++++++++- .../ftc/teamcode/subsystem/ClawSubsystem.java | 8 +++++-- .../subsystem/DualMotorSliderSubsystem.java | 14 +++++++---- .../teamcode/subsystem/WristSubsystem.java | 15 +++++++++++- 6 files changed, 68 insertions(+), 15 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 b5d28c2..e07b4a9 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 @@ -81,20 +81,36 @@ public class CometBotAutoDevelopment { //arm armToParkPosition(); armAndWristToFloor(); + armToBucketPosition(); //claw openClaw(); openThumb(); + //hang + grabBlueberrySkyhook(); + 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(currentGamepad1.y){ + wrist.hangBlueberrySkyhook(); + arm.hangBlueberrySkyhook(); + } + } + private void openClaw(){ if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){ claw.switchState(); } } private void openThumb(){ - if(currentGamepad2.right_bumper && !currentGamepad2.right_bumper){ - claw.switchState(); + if(currentGamepad2.left_bumper && !currentGamepad2.left_bumper){ + claw.switchTState(); } } private void armAndWristToFloor(){ @@ -103,7 +119,7 @@ public class CometBotAutoDevelopment { } if(currentGamepad2.a && !previousGamepad2.a){ - double increment = 0.6375 - arm.getPosition(); + double increment = 0.5 - arm.getPosition(); for(int i = 0; i < 3; i ++){ arm.setPosition(arm.getPosition() + increment); try { @@ -140,7 +156,7 @@ public class CometBotAutoDevelopment { } private void dualSlidesToFloorPosition() { if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { - dualSlides.toHighBucketPosition(); + dualSlides.toFloorPosotion(); } } private void dualSlidesToLowBucketPosition() { 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 d374d20..4b499f3 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 @@ -7,9 +7,15 @@ public class RobotConstants { public final static double clawClose = 0.85; public final static double clawOpen = 0.05; - public final static double armFloor = 0.6375; + public final static double armFloor = 0.5; public final static double armSubmarine = 0.55; - public final static double armPark = 0.1250; + public final static double armPark = 0.3; + //value for grabbing the hook Specimen + public final static double grabBlueberry = 0.3; + 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 armBucket = 0.25; public final static double wristPickup = 0.6; 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 5f5cdb9..03a4d5a 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 @@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueberrySkyhook; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -11,7 +13,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; public class ArmSubsystem { public enum ArmState { - PARK, FLOOR, BUCKET, SUBMARINE + PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK } private ServoImplEx arm; @@ -20,6 +22,14 @@ public class ArmSubsystem { public ArmSubsystem(HardwareMap hardwareMap) { this.arm = hardwareMap.get(ServoImplEx.class, ARM_SERVO); } + public void grabBlueberrySkyhook(){ + setState(ArmState.GRAB_BLUEBERRY_SKYHOOK); + arm.setPosition(armGrabBlueberrySkyhook); + } + public void hangBlueberrySkyhook(){ + setState(ArmState.HANG_BLUEBERRY_SKYHOOK); + arm.setPosition(armHangBlueberrySkyhook); + } public void toParkPosition() { arm.setPosition(armPark); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java index 44bf76f..08f2b01 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java @@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME; import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.grabBlueberry; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -11,7 +12,7 @@ import com.qualcomm.robotcore.hardware.Servo; public class ClawSubsystem { public enum ClawState { - CLOSED, OPEN + CLOSED, OPEN, GRAB_BLUEBERRY } public enum ThumbState { @@ -27,7 +28,10 @@ public class ClawSubsystem { claw = hardwareMap.get(Servo.class, CLAW_NAME); thumb = hardwareMap.get(Servo.class, THUMB_SERVO); } - + public void grabBlueberry(){ + claw.setPosition(grabBlueberry); + state = ClawState.GRAB_BLUEBERRY; + } public void closeClaw() { claw.setPosition(clawClose); state = ClawState.CLOSED; 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 f95733a..e4d195f 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,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOT import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; @@ -81,7 +82,8 @@ public class DualMotorSliderSubsystem { liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - + liftSlideLeft.setDirection(DcMotorSimple.Direction.REVERSE); + liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE); liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -121,11 +123,13 @@ public class DualMotorSliderSubsystem { } public void toLowBucketPosition() { - setTargetPosition(1500); - } - - public void toHighBucketPosition() { setTargetPosition(3000); } + public void toHighBucketPosition() { + setTargetPosition(4000); + } + + public void toFloorPosotion(){setTargetPosition(5);} + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java index d2bef2c..a67dfad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java @@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlueberrySkyhook; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen; @@ -17,7 +19,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; public class WristSubsystem { public enum WristState { - FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN + FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK } public ServoImplEx wrist; @@ -44,7 +46,18 @@ public class WristSubsystem { return false; } } + public void grabBlueberrySkyhook(){ + setState(WristState.GRAB_BLUEBERRY_SKYHOOK); + wrist.setPosition(wristGrabBlueberrySkyhook); + } + /** + * This function goes after grabBlueberrySkyhook + */ + public void hangBlueberrySkyhook(){ + setState(WristState.HANG_BLUEBERRY_SKYHOOK); + wrist.setPosition(wristHangBlueberrySkyhook); + } public void toFloorPosition() { setState(WristState.FLOOR); wrist.setPosition(wristFloor);