Add new test class

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftWristArmTest.java

@@ -0,0 +1,108 @@
+/* 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.ArmActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;
+
+@TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
+public class LiftWristArmTest extends LinearOpMode {
+
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Lift
+         */
+        LiftActionsSubsystem lift = new LiftActionsSubsystem(hardwareMap);
+        WristActionsSubsystem wrist = new WristActionsSubsystem(hardwareMap);
+        ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        lift.init();
+        wrist.init();
+        arm.init();
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
+                arm.setPosition(arm.getPosition() + .05);
+            }
+
+            if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
+                arm.setPosition(arm.getPosition() - .05);
+            }
+
+            if (currentGamepad1.triangle && !previousGamepad1.triangle) {
+                wrist.setPosition(wrist.getPosition() + .05);
+            }
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                wrist.setPosition(wrist.getPosition() - .05);
+            }
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+                lift.setPosition(lift.getPosition() + 175);
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                lift.setPosition(lift.getPosition() - 25);
+            }
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Lift Drive Position", lift.getPosition());
+            telemetry.addData("Wrist Position", wrist.getPosition());
+            telemetry.addData("Arm Position", arm.getPosition());
+
+            telemetry.update();
+        }
+    }
+}