Added sample Autonomous class file

2024-04-01 22:21:28 -07:00
parent 6e6e626816
commit 63158215d0
1 changed files with 88 additions and 0 deletions
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java
@ -0,0 +1,88 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.firstinspires.ftc.teamcode.cometbots.Constants.*;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.TouchSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary;
+
+@Autonomous(name = "Development Class", group = "[14493] CometBots")
+public class CometBotsProject extends LinearOpMode {
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        DcMotor rhw = hardwareMap.get(DcMotor.class, "right hand wheel");
+        DcMotor rlw = hardwareMap.get(DcMotor.class, "right leg wheel");
+        DcMotor lhw = hardwareMap.get(DcMotor.class, "left hand wheel");
+        DcMotor llw = hardwareMap.get(DcMotor.class, "left leg wheel");
+        TouchSensor touchSensor = hardwareMap.get(TouchSensor.class, "touchsensor");
+        DistanceSensor distanceSensor = hardwareMap.get(DistanceSensor.class, "distancesensor");
+        ColorSensor colorSensor = hardwareMap.get(ColorSensor.class, "colorsensor");
+
+        /*
+            Put all motors in an array for easier handling
+
+            NOTE: Placement of motor in array is important
+            especially when dealing with functions that
+            set direction
+         */
+        DcMotor[] motors = {rhw, rlw, lhw, llw};
+
+        /*
+            Initialize all wheels forward using motors array
+         */
+        MovementLibrary.setDirectionForward(motors);
+
+        /*
+            Initialize wheel direction with base speed
+         */
+        MovementLibrary.setSpeed(motors, CB_BASE_SPEED);
+
+        /*
+            Wait for the driver to hit START in the driver hub
+         */
+        waitForStart();
+
+        while (opModeIsActive()) {
+
+            if (touchSensor.isPressed()) {
+                telemetry.speak("Touch sensor is pressed");
+                telemetry.addData("Touch sensor pressed?", touchSensor.isPressed());
+            }
+
+            /*
+                Distance traveled in inches
+             */
+            double distanceInInches = distanceSensor.getDistance(DistanceUnit.INCH);
+            telemetry.addData("Distance traveled (inches)", "%.2f", distanceInInches);
+
+            /*
+                Capture color readout from color sensor
+             */
+            telemetry.addData("Color Sensor [Red]", colorSensor.red());
+            telemetry.addData("Color Sensor [Green]", colorSensor.green());
+            telemetry.addData("Color Sensor [Blue]", colorSensor.blue());
+            telemetry.addData("Color Sensor [Alpha]", colorSensor.alpha());
+
+            /*
+                Speed up when going over an object that is blue-ish / green
+             */
+            if (colorSensor.blue() > 800 && colorSensor.green() > 1100) {
+                telemetry.speak("Green speed boost");
+                MovementLibrary.setSpeed(motors, CB_BASE_SPEED * 2.0);
+            } else {
+                MovementLibrary.setSpeed(motors, CB_BASE_SPEED);
+            }
+
+            telemetry.update();
+
+        }
+    }
+}