diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java new file mode 100644 index 000000000..c3fc27cc1 --- /dev/null +++ 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(); + + } + } +}