Thumb is a work in progress

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();
         }
     }

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;
     }
 
 }