diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraDemo.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraDemo.java index 8f1fc8498..90b67585b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsCameraDemo.java @@ -11,8 +11,8 @@ import org.firstinspires.ftc.vision.VisionPortal; import java.util.Locale; -@TeleOp(name = "DevApp - Camera Test (HDMI)", group = "SCDS Development") -public class CometBotsCameraTest extends LinearOpMode { +@TeleOp(name = "CometBots Camera Demo (HDMI)", group = "[14493] CometBots") +public class CometBotsCameraDemo extends LinearOpMode { final boolean USING_WEBCAM = true; final BuiltinCameraDirection INTERNAL_CAM_DIR = BuiltinCameraDirection.FRONT; @@ -34,7 +34,7 @@ public class CometBotsCameraTest extends LinearOpMode { if (USING_WEBCAM) { portal = new VisionPortal.Builder() - .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .setCamera(hardwareMap.get(WebcamName.class, "watch out")) .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT)) .build(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUDemo.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUDemo.java index dbb69f2f8..1dcefc78f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsIMUDemo.java @@ -9,8 +9,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; -@TeleOp(name = "DevApp - IMU Test", group = "SCDS Development") -public class CometBotsIMUTest extends LinearOpMode { +@TeleOp(name = "CometBots IMU Demo", group = "[14493] CometBots") +public class CometBotsIMUDemo extends LinearOpMode { IMU imu; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java new file mode 100644 index 000000000..1741a42b0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java @@ -0,0 +1,166 @@ +/* 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.Disabled; +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.Gamepad; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="CometBots Linear OpMode Demo", group = "[14493] CometBots") +public class CometBotsLinearOpMode extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor rhw = null; + private DcMotor rlw = null; + private DcMotor lhw = null; + private DcMotor llw = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + rhw = hardwareMap.get(DcMotor.class, "right hand wheel"); + rlw = hardwareMap.get(DcMotor.class, "right leg wheel"); + lhw = hardwareMap.get(DcMotor.class, "left hand wheel"); + llw = hardwareMap.get(DcMotor.class, "left leg wheel"); + + /* + 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 a + */ + MovementLibrary.setDirectionForward(motors); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + // This is test code: + // + // Uncomment the following code to test your motor directions. + // Each button should make the corresponding motor run FORWARD. + // 1) First get all the motors to take to correct positions on the robot + // by adjusting your Robot Configuration if necessary. + // 2) Then make sure they run in the correct direction by modifying the + // the setDirection() calls above. + // Once the correct motors move in the correct direction re-comment this code. + + /* + leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad + leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad + rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad + rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad + */ + + // Send calculated power to wheels + lhw.setPower(leftFrontPower); + rhw.setPower(rightFrontPower); + llw.setPower(leftBackPower); + rlw.setPower(rightBackPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } + }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVDemo.java similarity index 85% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVDemo.java index 35826be08..c9d814089 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsOpenCVDemo.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -8,8 +9,9 @@ import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; import org.openftc.easyopencv.OpenCvCameraRotation; -@Autonomous(name="DevApp - OpenCV Test", group="SCDS Development") -public class CometBotsOpenCVTest extends LinearOpMode { +@Disabled +@Autonomous(name="DevApp - OpenCV Demo", group="[14493] CometBots") +public class CometBotsOpenCVDemo extends LinearOpMode { private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK private static final int CAMERA_WIDTH = 1280; // width of wanted camera resolution @@ -27,7 +29,7 @@ public class CometBotsOpenCVTest extends LinearOpMode { // Use OpenCvCameraFactory class from FTC SDK to create camera instance controlHubCam = OpenCvCameraFactory.getInstance().createWebcam( - hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + hardwareMap.get(WebcamName.class, "watch out"), cameraMonitorViewId); // controlHubCam.setPipeline(new YellowBlobDetectionPipeline()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java index c3fc27cc1..641b0055f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsProject.java @@ -3,6 +3,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; @@ -12,6 +13,7 @@ import com.qualcomm.robotcore.hardware.TouchSensor; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary; +@Disabled @Autonomous(name = "Development Class", group = "[14493] CometBots") public class CometBotsProject extends LinearOpMode { @@ -38,12 +40,32 @@ public class CometBotsProject extends LinearOpMode { /* Initialize all wheels forward using motors array */ - MovementLibrary.setDirectionForward(motors); +// MovementLibrary.setDirectionForward(motors); + MovementLibrary.setLeftSideBackward(motors); + MovementLibrary.setRightSideForward(motors); /* - Initialize wheel direction with base speed + 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()); + + /* + Capture distance readout from distance sensor */ - MovementLibrary.setSpeed(motors, CB_BASE_SPEED); + telemetry.addData("Distance Sensor [inches]", distanceSensor.getDistance(DistanceUnit.INCH)); + + /* + Capture state when touch sensor is pressed + */ + if (touchSensor.isPressed()) { + telemetry.speak("Touch sensor is pressed"); + telemetry.addData("Touch sensor pressed?", touchSensor.isPressed()); + } + + telemetry.update(); /* Wait for the driver to hit START in the driver hub @@ -52,10 +74,11 @@ public class CometBotsProject extends LinearOpMode { while (opModeIsActive()) { - if (touchSensor.isPressed()) { - telemetry.speak("Touch sensor is pressed"); - telemetry.addData("Touch sensor pressed?", touchSensor.isPressed()); - } + /* + Initialize wheel direction with base speed + */ + MovementLibrary.setSpeed(motors, CB_BASE_SPEED); + /* Distance traveled in inches @@ -63,14 +86,6 @@ public class CometBotsProject extends LinearOpMode { 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 */ @@ -80,9 +95,6 @@ public class CometBotsProject extends LinearOpMode { } else { MovementLibrary.setSpeed(motors, CB_BASE_SPEED); } - - telemetry.update(); - } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsSensorDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsSensorDemo.java new file mode 100644 index 000000000..b6834c62e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsSensorDemo.java @@ -0,0 +1,111 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +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.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; + +@Autonomous(name = "CometBot Sensor Demo", group = "[14493] CometBots") +public class CometBotsSensorDemo extends LinearOpMode { + + private OpenCvCamera controlHubCam; // Use OpenCvCamera class from FTC SDK + private static final int CAMERA_WIDTH = 1280; // width of wanted camera resolution + private static final int CAMERA_HEIGHT = 720; // height of wanted camera resolution + + // Calculate the distance using the formula + public static final double objectWidthInRealWorldUnits = 3.75; // Replace with the actual width of the object in real-world units + public static final double focalLength = 728; // Replace with the focal length of the camera in pixels + + private DcMotor rhw = null; + private DcMotor rlw = null; + private DcMotor lhw = null; + private DcMotor llw = null; + + @Override + public void runOpMode() throws InterruptedException { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + rhw = hardwareMap.get(DcMotor.class, "right hand wheel"); + rlw = hardwareMap.get(DcMotor.class, "right leg wheel"); + lhw = hardwareMap.get(DcMotor.class, "left hand wheel"); + llw = hardwareMap.get(DcMotor.class, "left leg wheel"); + + /* + 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}; + + // Create an instance of the camera + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + + // Use OpenCvCameraFactory class from FTC SDK to create camera instance + controlHubCam = OpenCvCameraFactory.getInstance().createWebcam( + hardwareMap.get(WebcamName.class, "watch out"), cameraMonitorViewId); + + TouchSensor touchSensor = hardwareMap.get(TouchSensor.class, "touchsensor"); + DistanceSensor distanceSensor = hardwareMap.get(DistanceSensor.class, "distancesensor"); + ColorSensor colorSensor = hardwareMap.get(ColorSensor.class, "colorsensor"); + + controlHubCam.openCameraDevice(); + controlHubCam.startStreaming(CAMERA_WIDTH, CAMERA_HEIGHT, OpenCvCameraRotation.UPRIGHT); + + FtcDashboard.getInstance().startCameraStream(controlHubCam, 0); + + /* + Wait for the driver to hit START in the driver hub + */ + waitForStart(); + + while (opModeIsActive()) { + + /* + 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()); + + /* + Capture distance readout from distance sensor + */ + telemetry.addData("Distance Sensor [inches]", distanceSensor.getDistance(DistanceUnit.INCH)); + + /* + Capture state when touch sensor is pressed + */ + telemetry.addData("Touch sensor pressed?", touchSensor.isPressed()); + if (touchSensor.isPressed()) { + telemetry.speak("Touch sensor is pressed"); + } + + if (distanceSensor.getDistance(DistanceUnit.INCH) < 3) { + telemetry.speak("Warning, unknown object near robot"); + MovementLibrary.setDirectionRight(motors); + MovementLibrary.setSpeed(motors,0.25); + sleep(1500); + MovementLibrary.setSpeed(motors,0); + } + + /* + Update stats continually + */ + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetectionEasy.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsTensorFlowDemo.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetectionEasy.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsTensorFlowDemo.java index 73624a232..8c6c209f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetectionEasy.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsTensorFlowDemo.java @@ -29,7 +29,6 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -48,8 +47,8 @@ import java.util.List; * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. */ -@TeleOp(name = "Concept: TensorFlow Object Detection Easy", group = "Concept") -public class ConceptTensorFlowObjectDetectionEasy extends LinearOpMode { +@TeleOp(name = "CometBots TensorFlow Demo (HDMI)", group = "[14493] CometBots") +public class CometBotsTensorFlowDemo extends LinearOpMode { private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera @@ -110,7 +109,7 @@ public class ConceptTensorFlowObjectDetectionEasy extends LinearOpMode { // Create the vision portal the easy way. if (USE_WEBCAM) { visionPortal = VisionPortal.easyCreateWithDefaults( - hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); + hardwareMap.get(WebcamName.class, "watch out"), tfod); } else { visionPortal = VisionPortal.easyCreateWithDefaults( BuiltinCameraDirection.BACK, tfod); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java index 021552ece..6d33663c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java @@ -1,37 +1,55 @@ package org.firstinspires.ftc.teamcode.cometbots; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; public class MovementLibrary { public static void setDirectionForward(DcMotor[] motors) { - motors[0].setDirection(DcMotor.Direction.REVERSE); - motors[1].setDirection(DcMotor.Direction.FORWARD); + setLeftSideForward(motors); + setRightSideForward(motors); + } + + public static void setLeftSideForward(DcMotor[] motors) { motors[2].setDirection(DcMotor.Direction.FORWARD); motors[3].setDirection(DcMotor.Direction.REVERSE); } - public static void setDirectionBackward(DcMotor[] motors) { - motors[0].setDirection(DcMotor.Direction.FORWARD); - motors[1].setDirection(DcMotor.Direction.REVERSE); + public static void setRightSideForward(DcMotor[] motors) { + motors[0].setDirection(DcMotor.Direction.REVERSE); + motors[1].setDirection(DcMotor.Direction.FORWARD); + } + + public static void setLeftSideBackward(DcMotor[] motors) { motors[2].setDirection(DcMotor.Direction.REVERSE); motors[3].setDirection(DcMotor.Direction.FORWARD); } + public static void setRightSideBackward(DcMotor[] motors) { + motors[0].setDirection(DcMotor.Direction.FORWARD); + motors[1].setDirection(DcMotor.Direction.REVERSE); + } + + + public static void setDirectionBackward(DcMotor[] motors) { + setLeftSideBackward(motors); + setRightSideBackward(motors); + } + public static void setSpeed(DcMotor[] motors, double speedValue) { for (DcMotor motor : motors) { motor.setPower(speedValue); } } - public static void setDirectionRight(DcMotor[] motors) { + public static void setDirectionLeft(DcMotor[] motors) { motors[0].setDirection(DcMotor.Direction.REVERSE); motors[1].setDirection(DcMotor.Direction.REVERSE); motors[2].setDirection(DcMotor.Direction.FORWARD); motors[3].setDirection(DcMotor.Direction.FORWARD); } - public static void setDirectionLeft(DcMotor[] motors) { + public static void setDirectionRight(DcMotor[] motors) { motors[0].setDirection(DcMotor.Direction.FORWARD); motors[1].setDirection(DcMotor.Direction.FORWARD); motors[2].setDirection(DcMotor.Direction.REVERSE);