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 ab841d9..7996a0f 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 @@ -31,6 +31,7 @@ public class CometBotTeleOpDevelopment { public Gamepad currentGamepad2; public Gamepad previousGamepad1; public Gamepad previousGamepad2; + public boolean grabBlock; private Follower follower; @@ -41,7 +42,7 @@ public class CometBotTeleOpDevelopment { arm = new ArmSubsystem(hardwareMap); wrist = new WristSubsystem(hardwareMap); skyhook = new HangMotorSubsystem(hardwareMap); - + grabBlock = false; this.gamepad1 = gamepad1; this.gamepad2 = gamepad2; currentGamepad1 = new Gamepad(); @@ -153,7 +154,7 @@ public class CometBotTeleOpDevelopment { private void hang(){ if (gamepad1.a) { claw.grabBlueberry(); - arm.setPosition(0.15); + arm.setPosition(0.13); arm.grabBlueberrySkyhook(); //claw Open small amount @@ -163,35 +164,53 @@ public class CometBotTeleOpDevelopment { if(gamepad1.b) { //confirm grab claw.closeClaw(); - + grabBlock = true; } - if (gamepad1.x) { + if (gamepad1.x && grabBlock) { //now slap on bar, first wrist, then arm, then claw then driver must drive away - dualSlides.toFixedPosition(500); + dualSlides.toFixedPosition(300); dualSlides.update(); } if (gamepad1.y) { arm.hangBlueberrySkyhook(); wrist.hangBlueberrySkyhook(); try { - Thread.sleep(500); + Thread.sleep(1500); } catch (InterruptedException e) { throw new RuntimeException(e); } - dualSlides.toFixedPosition(1800); + dualSlides.toFixedPosition(2100); dualSlides.update(); } if (gamepad1.right_bumper) { claw.openClaw(); - } + try { + Thread.sleep(1000); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + dualSlides.toFloorPosition(); + claw.closeClaw(); + arm.toParkPosition(); + wrist.toFloorPosition(); + }/* if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) { skyhook.hangRobot(); - } + }*/ skyhook.setPower(0); - if (gamepad1.dpad_down) { + + /*if (gamepad1.dpad_down) { skyhook.disableMotor(); skyhook.setPower(1); + }*/ + if(gamepad1.right_trigger > 0){ + skyhook.setPower(true, 1); + + } + if(gamepad1.left_trigger > 0){ + skyhook.setPower(false, 1); + } } 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 38b1398..cdd9d8c 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,8 +7,6 @@ public class RobotConstants { 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; 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 94e0558..7fce805 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 @@ -55,5 +55,12 @@ public class HangMotorSubsystem { //write in limits for protection hang.setPower(Position); } + public void setPower(boolean forward, double power){ + if(forward) + hang.setDirection(DcMotorSimple.Direction.FORWARD); + else if(!forward) + hang.setDirection(DcMotorSimple.Direction.REVERSE); + hang.setPower(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 b19abc2..4a07556 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 @@ -88,6 +88,7 @@ public class HookTest extends OpMode { } + } }