From 95f20096b9768263b80c6bbd841623df4f8c4986 Mon Sep 17 00:00:00 2001
From: Carlos Rivas <carlos@riv.as>
Date: Fri, 27 Dec 2024 01:37:31 -0800
Subject: [PATCH] Version 1 of claws without actions nor thumb

---
 .../teamcode/cometbots/tests/ClawTest.java    | 80 +++++++++++++++++++
 .../ftc/teamcode/subsystem/ClawSubsystem.java | 49 ++++++++++++
 2 files changed, 129 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java

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
new file mode 100644
index 0000000..4bbca92
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java
@@ -0,0 +1,80 @@
+/* Copyright (c) 2021 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.cometbots.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
+
+
+@TeleOp(name = "Claw Test", group = "Debug")
+public class ClawTest extends LinearOpMode {
+
+    // Declare OpMode members for each of the 4 motors.
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Claw
+         */
+        ClawSubsystem claw = new ClawSubsystem(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        waitForStart();
+        claw.init();
+        runtime.reset();
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                claw.switchState();
+            }
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Claw State", claw.getState());
+            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
new file mode 100644
index 0000000..e66067c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class ClawSubsystem {
+
+    public enum ClawState {
+        CLOSED, OPEN
+    }
+
+    private Servo claw;
+    private ClawState state;
+
+    public ClawSubsystem(HardwareMap hardwareMap) {
+        this.claw = hardwareMap.get(Servo.class, CLAW_NAME);
+    }
+
+    public void closeClaw() {
+        claw.setPosition(clawClose);
+        state = ClawState.CLOSED;
+    }
+
+    public void openClaw() {
+        claw.setPosition(clawOpen);
+        state = ClawState.OPEN;
+    }
+
+    public ClawState getState() {
+        return state;
+    }
+
+    public void switchState() {
+        if (state == ClawState.CLOSED) {
+            openClaw();
+        } else if (state == ClawState.OPEN) {
+            closeClaw();
+        }
+    }
+
+    public void init() {
+        closeClaw();
+    }
+
+}
\ No newline at end of file