diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java index 4bbca92..9eb9879 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java @@ -71,9 +71,14 @@ public class ClawTest extends LinearOpMode { claw.switchState(); } + if (currentGamepad1.triangle && !previousGamepad1.triangle) { + claw.switchTState(); + } + // Show the elapsed game time and wheel power. telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Claw State", claw.getState()); + telemetry.addData("Claw Thumb State", claw.getTState()); telemetry.update(); } } 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 e66067c..44bf76f 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystem; 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; @@ -13,11 +14,18 @@ public class ClawSubsystem { CLOSED, OPEN } + public enum ThumbState { + DOWN, UP + } + private Servo claw; + private Servo thumb; private ClawState state; + private ThumbState tState; public ClawSubsystem(HardwareMap hardwareMap) { - this.claw = hardwareMap.get(Servo.class, CLAW_NAME); + claw = hardwareMap.get(Servo.class, CLAW_NAME); + thumb = hardwareMap.get(Servo.class, THUMB_SERVO); } public void closeClaw() { @@ -34,6 +42,10 @@ public class ClawSubsystem { return state; } + public ThumbState getTState() { + return tState; + } + public void switchState() { if (state == ClawState.CLOSED) { openClaw(); @@ -42,8 +54,27 @@ public class ClawSubsystem { } } + public void switchTState() { + if (tState == ThumbState.UP) { + thumbDown(); + } else if (tState == ThumbState.DOWN) { + thumbUp(); + } + } + public void init() { closeClaw(); + thumbDown(); + } + + public void thumbUp() { + thumb.setPosition(0.95); + tState = ThumbState.UP; + } + + public void thumbDown() { + thumb.setPosition(0.05); + tState = ThumbState.DOWN; } } \ No newline at end of file