From 52e21fd468855f7af662c91f811e69400d9739bb Mon Sep 17 00:00:00 2001 From: robotics3 Date: Thu, 23 Jan 2025 10:52:56 -0800 Subject: [PATCH] Commit Hang Subsystem --- .../cometbots/CometBotAutoDevelopment.java | 21 +++++-- .../ftc/teamcode/configs/RobotConstants.java | 3 +- .../subsystem/HangMotorSubsystem.java | 59 +++++++++++++++++++ .../ftc/teamcode/subsystem/LiftArmWrist.java | 9 +++ 4 files changed, 85 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java 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 e82cfe3..f2a27c9 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 @@ -114,12 +114,9 @@ public class CometBotAutoDevelopment { } } private void armAndWristToFloor(){ - if(currentGamepad2.a && !previousGamepad2.a){ - wrist.switchState(); - } if(currentGamepad2.a && !previousGamepad2.a){ - double increment = 0.5 - arm.getPosition(); + double increment = 0.7 - arm.getPosition(); for(int i = 0; i < 3; i ++){ arm.setPosition(arm.getPosition() + increment); try { @@ -129,9 +126,11 @@ public class CometBotAutoDevelopment { } } arm.toFloorPosition(); - - } + if(currentGamepad2.a && !previousGamepad2.a ){ + wrist.switchState(); + } + } @@ -154,6 +153,16 @@ public class CometBotAutoDevelopment { dualSlides.toHighBucketPosition(); } } + private void robotUp(){ + if(currentGamepad1.dpad_up && !previousGamepad1.dpad_up){ + + } + } + private void robotDown(){ + if(currentGamepad1.dpad_down && !previousGamepad1.dpad_down){ + + } + } private void dualSlidesToFloorPosition() { if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { dualSlides.toFloorPosition(); 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 9e2bf89..488dde1 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 @@ -30,7 +30,8 @@ public class RobotConstants { public final static double wristSpeciemen = 0.1; - + public final static int toBar = 500; + public final static int toFloor = 0; public final static int liftToFloorPos = 350; public final static int liftToSubmarinePos = 350; public final static int liftToLowBucketPos = 1750; 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 new file mode 100644 index 0000000..237ed61 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR; + +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 static org.firstinspires.ftc.teamcode.configs.RobotConstants.toBar; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.toFloor; + +public class HangMotorSubsystem { + private DcMotorEx hang; + + public HangMotorSubsystem(HardwareMap hardwareMap) { + + hang = hardwareMap.get(DcMotorEx.class, "skyhook"); + + + } + + public int getCurrentPosition(){ + int Return = hang.getCurrentPosition(); + return Return; + } + public void init(){ + hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + hang.setDirection(DcMotorSimple.Direction.FORWARD); + hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + hang.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + } + public void hangRobot(){ + hang.setTargetPosition(toBar); + + } + public void robotToFloor(){ + hang.setTargetPosition(toFloor); + } + public void disableMotor(){ + hang.setPower(0); + hang.setMotorDisable(); + + } + public void setPower(double power){ + hang.setPower(power); + } + public void stopMotor(){ + hang.setPower(0); + + + } + public void toHangPosition(int Position){ + //write in limits for protection + hang.setPower(Position); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java index b07e06c..09c574d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java @@ -58,6 +58,7 @@ public class LiftArmWrist extends LinearOpMode { WristSubsystem wrist = new WristSubsystem(hardwareMap); ArmSubsystem arm = new ArmSubsystem(hardwareMap); ClawSubsystem claw = new ClawSubsystem(hardwareMap); + HangMotorSubsystem hang = new HangMotorSubsystem(hardwareMap); /* @@ -117,15 +118,23 @@ public class LiftArmWrist extends LinearOpMode { lift.toLowBucketPosition(); } + if(currentGamepad1.left_trigger > 0){ + hang.setPower((double) currentGamepad1.left_trigger); + } + if(currentGamepad1.right_trigger > 0){ + hang.setPower((double) currentGamepad1.right_trigger); + } lift.update(); // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Lift Drive Position", lift.getFixedPosition()); telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Arm Position", arm.getPosition()); telemetry.addData("Slide Ticks", lift.getCurrentPosition()); + telemetry.addData("Hook Ticks", hang.getCurrentPosition()); telemetry.update(); } }