Add back Lift Raw subsystem

2024-11-03 11:38:01 -08:00
parent 39094d531e
commit b35cefe917
1 changed files with 124 additions and 0 deletions
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftRawTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftRawTest.java
@ -0,0 +1,124 @@
+/* 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;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+@TeleOp(name = "Lift Raw Test", group = "Debug")
+public class LiftRawTest extends LinearOpMode {
+
+    // Declare OpMode members for each of the 4 motors.
+    private final ElapsedTime runtime = new ElapsedTime();
+
+    private final int MIN_POINT = 0;
+    private final int MAX_POINT = 3700;
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Instantiate Lift
+         */
+        DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor");
+        liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        liftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        /*
+         * Instantiate gamepad state holders
+         */
+        Gamepad currentGamepad1 = new Gamepad();
+        Gamepad previousGamepad1 = new Gamepad();
+
+        waitForStart();
+        runtime.reset();
+
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            previousGamepad1.copy(currentGamepad1);
+            currentGamepad1.copy(gamepad1);
+
+            liftDrive.setPower(.5);
+
+            // Max position is 6800, safely setting to 6500
+
+            if (currentGamepad1.square && !previousGamepad1.square) {
+                liftDrive.setTargetPosition(MIN_POINT);
+                liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+
+            if (currentGamepad1.triangle && !previousGamepad1.triangle) {
+                liftDrive.setTargetPosition(1500);
+                liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+
+            if (currentGamepad1.circle && !previousGamepad1.circle) {
+                liftDrive.setTargetPosition(2750);
+                liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+
+            if (currentGamepad1.cross && !previousGamepad1.cross) {
+                liftDrive.setTargetPosition(MAX_POINT);
+                liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+
+            if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
+                int newPosition = liftDrive.getCurrentPosition() - 125;
+                if (newPosition < MIN_POINT) {
+                    liftDrive.setTargetPosition(MIN_POINT);
+                } else {
+                    liftDrive.setTargetPosition(newPosition);
+                }
+                liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+
+            if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
+                int newPosition = liftDrive.getCurrentPosition() + 125;
+                if (newPosition > MAX_POINT) {
+                    liftDrive.setTargetPosition(MAX_POINT);
+                } else {
+                    liftDrive.setTargetPosition(newPosition);
+                }
+                liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            }
+
+            // Show the elapsed game time and wheel power.
+            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition());
+            telemetry.update();
+        }
+    }
+}