diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 24038bf..f13a2a0 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="59" + android:versionName="10.3"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 1d14dfb..2644143 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -69,20 +69,20 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { // Declare OpMode members for each of the 4 motors. private ElapsedTime runtime = new ElapsedTime(); - private DcMotor leftFrontDrive = null; - private DcMotor leftBackDrive = null; - private DcMotor rightFrontDrive = null; - private DcMotor rightBackDrive = null; + private DcMotor frontLeftDrive = null; + private DcMotor backLeftDrive = null; + private DcMotor frontRightDrive = null; + private DcMotor backRightDrive = 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. - leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive"); - leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive"); - rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive"); - rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive"); + frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); + backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); + frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); + backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); // ######################################################################################## // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! @@ -94,10 +94,10 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. - leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); - leftBackDrive.setDirection(DcMotor.Direction.REVERSE); - rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); - rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + frontRightDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) telemetry.addData("Status", "Initialized"); @@ -117,22 +117,22 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { // 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; + double frontLeftPower = axial + lateral + yaw; + double frontRightPower = axial - lateral - yaw; + double backLeftPower = axial - lateral + yaw; + double backRightPower = 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)); + max = Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)); + max = Math.max(max, Math.abs(backLeftPower)); + max = Math.max(max, Math.abs(backRightPower)); if (max > 1.0) { - leftFrontPower /= max; - rightFrontPower /= max; - leftBackPower /= max; - rightBackPower /= max; + frontLeftPower /= max; + frontRightPower /= max; + backLeftPower /= max; + backRightPower /= max; } // This is test code: @@ -146,22 +146,22 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { // 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 + frontLeftPower = gamepad1.x ? 1.0 : 0.0; // X gamepad + backLeftPower = gamepad1.a ? 1.0 : 0.0; // A gamepad + frontRightPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad + backRightPower = gamepad1.b ? 1.0 : 0.0; // B gamepad */ // Send calculated power to wheels - leftFrontDrive.setPower(leftFrontPower); - rightFrontDrive.setPower(rightFrontPower); - leftBackDrive.setPower(leftBackPower); - rightBackDrive.setPower(rightBackPower); + frontLeftDrive.setPower(frontLeftPower); + frontRightDrive.setPower(frontRightPower); + backLeftDrive.setPower(backLeftPower); + backRightDrive.setPower(backRightPower); // 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.addData("Front left/Right", "%4.2f, %4.2f", frontLeftPower, frontRightPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", backLeftPower, backRightPower); telemetry.update(); } }} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java new file mode 100644 index 0000000..2c93abb --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java @@ -0,0 +1,87 @@ +/* Copyright (c) 2025 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.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * Demonstrates how to use an OpMode to store data in the blackboard and retrieve it. + * This is useful for storing information in Auto and then retrieving it in your TeleOp. + * + * Be aware that this is NOT saved across power reboots or downloads or robot resets. + * + * You also need to make sure that both the storing and retrieving are done using the same + * type. For example, storing a Double in the blackboard and retrieving it as an Integer + * will fail when you typecast it. + * + * The blackboard is a standard hashmap so you can use methods like: + * put, get, containsKey, remove, etc. + */ +@TeleOp(name = "Concept: Blackboard", group = "Concept") +@Disabled +public class ConceptBlackboard extends OpMode { + // We use constants here so we won't have the problem of typos in different places when we + // meant to refer to the same thing. + public static final String TIMES_STARTED_KEY = "Times started"; + public static final String ALLIANCE_KEY = "Alliance"; + + /** + * This method will be called once, when the INIT button is pressed. + */ + @Override + public void init() { + // This gets us what is in the blackboard or the default if it isn't in there. + Object timesStarted = blackboard.getOrDefault(TIMES_STARTED_KEY, 0); + blackboard.put(TIMES_STARTED_KEY, (int) timesStarted + 1); + + telemetry.addData("OpMode started times", blackboard.get(TIMES_STARTED_KEY)); + } + + /** + * This method will be called repeatedly during the period between when + * the START button is pressed and when the OpMode is stopped. + *

+ * If the left bumper is pressed it will store the value "RED" in the blackboard for alliance. + * If the right bumper is pressed it will store the value "BLUE" in the blackboard for alliance. + *

+ * You'll notice that if you quit the OpMode and come back in, the value will persist. + */ + @Override + public void loop() { + if (gamepad1.left_bumper) { + blackboard.put(ALLIANCE_KEY, "RED"); + } else if (gamepad1.right_bumper) { + blackboard.put(ALLIANCE_KEY, "BLUE"); + } + telemetry.addData("Alliance", blackboard.get(ALLIANCE_KEY)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java new file mode 100644 index 0000000..d5e0032 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java @@ -0,0 +1,92 @@ +/* +Copyright (c) 2024 Miriam Sinton-Remes + +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 FITNESSFOR 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.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * This OpMode illustrates using edge detection on a gamepad. + * + * Simply checking the state of a gamepad button each time could result in triggering an effect + * multiple times. Edge detection ensures that you only detect one button press, regardless of how + * long the button is held. + * + * There are two main types of edge detection. Rising edge detection will trigger when a button is + * first pressed. Falling edge detection will trigger when the button is released. + * + * 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. + */ + +@Disabled +@TeleOp(name="Concept: Gamepad Edge Detection", group ="Concept") +public class ConceptGamepadEdgeDetection extends LinearOpMode { + + @Override + public void runOpMode() { + // Wait for the DS start button to be pressed + waitForStart(); + + while (opModeIsActive()) { + // Update the telemetry + telemetryButtonData(); + + // Wait 2 seconds before doing another check + sleep(2000); + } + } + + public void telemetryButtonData() { + // Add the status of the Gamepad 1 Left Bumper + telemetry.addData("Gamepad 1 Left Bumper Pressed", gamepad1.leftBumperWasPressed()); + telemetry.addData("Gamepad 1 Left Bumper Released", gamepad1.leftBumperWasReleased()); + telemetry.addData("Gamepad 1 Left Bumper Status", gamepad1.left_bumper); + + // Add an empty line to seperate the buttons in telemetry + telemetry.addLine(); + + // Add the status of the Gamepad 1 Right Bumper + telemetry.addData("Gamepad 1 Right Bumper Pressed", gamepad1.rightBumperWasPressed()); + telemetry.addData("Gamepad 1 Right Bumper Released", gamepad1.rightBumperWasReleased()); + telemetry.addData("Gamepad 1 Right Bumper Status", gamepad1.right_bumper); + + // Add a note that the telemetry is only updated every 2 seconds + telemetry.addLine("\nTelemetry is updated every 2 seconds."); + + // Update the telemetry on the DS screen + telemetry.update(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java index 987694d..e143d46 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java @@ -149,39 +149,54 @@ public class ConceptVisionColorLocator extends LinearOpMode * Note: All contours will be still displayed on the Stream Preview, but only those that satisfy the filter * conditions will remain in the current list of "blobs". Multiple filters may be used. * - * Use any of the following filters. + * To perform a filter + * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, blobs); * - * ColorBlobLocatorProcessor.Util.filterByArea(minArea, maxArea, blobs); + * The following criteria are currently supported. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA * A Blob's area is the number of pixels contained within the Contour. Filter out any that are too big or small. * Start with a large range and then refine the range based on the likely size of the desired object in the viewfinder. * - * ColorBlobLocatorProcessor.Util.filterByDensity(minDensity, maxDensity, blobs); + * ColorBlobLocatorProcessor.BlobCriteria.BY_DENSITY * A blob's density is an indication of how "full" the contour is. * If you put a rubber band around the contour you would get the "Convex Hull" of the contour. * The density is the ratio of Contour-area to Convex Hull-area. * - * ColorBlobLocatorProcessor.Util.filterByAspectRatio(minAspect, maxAspect, blobs); + * ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO * A blob's Aspect ratio is the ratio of boxFit long side to short side. * A perfect Square has an aspect ratio of 1. All others are > 1 + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_ARC_LENGTH + * A blob's arc length is the perimeter of the blob. + * This can be used in conjunction with an area filter to detect oddly shaped blobs. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY + * A blob's circularity is how circular it is based on the known area and arc length. + * A perfect circle has a circularity of 1. All others are < 1 */ - ColorBlobLocatorProcessor.Util.filterByArea(50, 20000, blobs); // filter out very small blobs. + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + 50, 20000, blobs); // filter out very small blobs. /* * The list of Blobs can be sorted using the same Blob attributes as listed above. * No more than one sort call should be made. Sorting can use ascending or descending order. - * ColorBlobLocatorProcessor.Util.sortByArea(SortOrder.DESCENDING, blobs); // Default - * ColorBlobLocatorProcessor.Util.sortByDensity(SortOrder.DESCENDING, blobs); - * ColorBlobLocatorProcessor.Util.sortByAspectRatio(SortOrder.DESCENDING, blobs); + * ColorBlobLocatorProcessor.Util.sortByCriteria(ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA SortOrder.DESCENDING, blobs); // Default + * ColorBlobLocatorProcessor.Util.sortByCriteria(ColorBlobLocatorProcessor.BlobCriteria.BY_DENSITY SortOrder.DESCENDING, blobs); + * ColorBlobLocatorProcessor.Util.sortByCriteria(ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO SortOrder.DESCENDING, blobs); + * ColorBlobLocatorProcessor.Util.sortByCriteria(ColorBlobLocatorProcessor.BlobCriteria.BY_ARC_LENGTH SortOrder.DESCENDING, blobs); + * ColorBlobLocatorProcessor.Util.sortByCriteria(ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY SortOrder.DESCENDING, blobs); */ - telemetry.addLine(" Area Density Aspect Center"); + telemetry.addLine("Area Density Aspect Arc Circle Center"); // Display the size (area) and center location for each Blob. for(ColorBlobLocatorProcessor.Blob b : blobs) { RotatedRect boxFit = b.getBoxFit(); - telemetry.addLine(String.format("%5d %4.2f %5.2f (%3d,%3d)", - b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) boxFit.center.x, (int) boxFit.center.y)); + telemetry.addLine(String.format("%5d %4.2f %5.2f %3d %5.3f (%3d,%3d)", + b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) b.getArcLength(), b.getCircularity(), (int) boxFit.center.x, (int) boxFit.center.y)); } telemetry.update(); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java index 6be2bc4..86195cf 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java @@ -21,13 +21,13 @@ package org.firstinspires.ftc.robotcontroller.external.samples; -import android.graphics.Color; import android.util.Size; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.opencv.ImageRegion; @@ -112,22 +112,33 @@ public class ConceptVisionColorSensor extends LinearOpMode .build(); telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode. while (opModeIsActive() || opModeInInit()) { - telemetry.addData("DS preview on/off", "3 dots, Camera Stream\n"); + telemetry.addLine("Preview on/off: 3 dots, Camera Stream\n"); // Request the most recent color analysis. - // This will return the closest matching colorSwatch and the predominant RGB color. - // Note: to take actions based on the detected color, simply use the colorSwatch in a comparison or switch. + // This will return the closest matching colorSwatch and the predominant color in RGB, HSV and YCrCb color spaces. + // The color space values are returned as three-element int[] arrays as follows: + // RGB Red 0-255, Green 0-255, Blue 0-255 + // HSV Hue 0-180, Saturation 0-255, Value 0-255 + // YCrCb Luminance(Y) 0-255, Cr 0-255 (center 128), Cb 0-255 (center 128) + // + // Note: to take actions based on the detected color, simply use the colorSwatch or color space value in a comparison or switch. // eg: // if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {... some code ...} + // or: + // if (result.RGB[0] > 128) {... some code ...} + PredominantColorProcessor.Result result = colorSensor.getAnalysis(); // Display the Color Sensor result. - telemetry.addData("Best Match:", result.closestSwatch); - telemetry.addLine(String.format("R %3d, G %3d, B %3d", Color.red(result.rgb), Color.green(result.rgb), Color.blue(result.rgb))); + telemetry.addData("Best Match", result.closestSwatch); + telemetry.addLine(String.format("RGB (%3d, %3d, %3d)", result.RGB[0], result.RGB[1], result.RGB[2])); + telemetry.addLine(String.format("HSV (%3d, %3d, %3d)", result.HSV[0], result.HSV[1], result.HSV[2])); + telemetry.addLine(String.format("YCrCb (%3d, %3d, %3d)", result.YCrCb[0], result.YCrCb[1], result.YCrCb[2])); telemetry.update(); sleep(20); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java index 9bac006..4b777e2 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java @@ -61,7 +61,7 @@ import java.util.concurrent.TimeUnit; * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. * - * The code assumes a Robot Configuration with motors named: leftfront_drive and rightfront_drive, leftback_drive and rightback_drive. + * The code assumes a Robot Configuration with motors named: front_left_drive and front_right_drive, back_left_drive and back_right_drive. * The motor directions must be set so a positive power goes forward on all wheels. * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default, * so you should choose to approach a valid tag ID. @@ -103,10 +103,10 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot) final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) - private DcMotor leftFrontDrive = null; // Used to control the left front drive wheel - private DcMotor rightFrontDrive = null; // Used to control the right front drive wheel - private DcMotor leftBackDrive = null; // Used to control the left back drive wheel - private DcMotor rightBackDrive = null; // Used to control the right back drive wheel + private DcMotor frontLeftDrive = null; // Used to control the left front drive wheel + private DcMotor frontRightDrive = null; // Used to control the right front drive wheel + private DcMotor backLeftDrive = null; // Used to control the left back drive wheel + private DcMotor backRightDrive = null; // Used to control the right back drive wheel private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. @@ -127,18 +127,18 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode // Initialize the hardware variables. Note that the strings used here as parameters // to 'get' must match the names assigned during the robot configuration. // step (using the FTC Robot Controller app on the phone). - leftFrontDrive = hardwareMap.get(DcMotor.class, "leftfront_drive"); - rightFrontDrive = hardwareMap.get(DcMotor.class, "rightfront_drive"); - leftBackDrive = hardwareMap.get(DcMotor.class, "leftback_drive"); - rightBackDrive = hardwareMap.get(DcMotor.class, "rightback_drive"); + frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); + frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); + backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); + backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips - leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); - leftBackDrive.setDirection(DcMotor.Direction.REVERSE); - rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); - rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + frontRightDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); if (USE_WEBCAM) setManualExposure(6, 250); // Use low exposure time to reduce motion blur @@ -227,28 +227,28 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode */ public void moveRobot(double x, double y, double yaw) { // Calculate wheel powers. - double leftFrontPower = x -y -yaw; - double rightFrontPower = x +y +yaw; - double leftBackPower = x +y -yaw; - double rightBackPower = x -y +yaw; + double frontLeftPower = x - y - yaw; + double frontRightPower = x + y + yaw; + double backLeftPower = x + y - yaw; + double backRightPower = x - y + yaw; // Normalize wheel powers to be less than 1.0 - double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); - max = Math.max(max, Math.abs(leftBackPower)); - max = Math.max(max, Math.abs(rightBackPower)); + double max = Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)); + max = Math.max(max, Math.abs(backLeftPower)); + max = Math.max(max, Math.abs(backRightPower)); if (max > 1.0) { - leftFrontPower /= max; - rightFrontPower /= max; - leftBackPower /= max; - rightBackPower /= max; + frontLeftPower /= max; + frontRightPower /= max; + backLeftPower /= max; + backRightPower /= max; } // Send powers to the wheels. - leftFrontDrive.setPower(leftFrontPower); - rightFrontDrive.setPower(rightFrontPower); - leftBackDrive.setPower(leftBackPower); - rightBackDrive.setPower(rightBackPower); + frontLeftDrive.setPower(frontLeftPower); + frontRightDrive.setPower(frontRightPower); + backLeftDrive.setPower(backLeftPower); + backRightDrive.setPower(backRightPower); } /** diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java new file mode 100644 index 0000000..c5c64d0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java @@ -0,0 +1,163 @@ +/* Copyright (c) 2025 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.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +/* + * This OpMode illustrates how to program your robot to drive field relative. This means + * that the robot drives the direction you push the joystick regardless of the current orientation + * of the robot. + * + * This OpMode assumes that you have four mecanum wheels each on its own motor named: + * front_left_motor, front_right_motor, back_left_motor, back_right_motor + * + * and that the left motors are flipped such that when they turn clockwise the wheel moves backwards + * + * 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 = "Robot: Field Relative Mecanum Drive", group = "Robot") +@Disabled +public class RobotTeleopMecanumFieldRelativeDrive extends OpMode { + // This declares the four motors needed + DcMotor frontLeftDrive; + DcMotor frontRightDrive; + DcMotor backLeftDrive; + DcMotor backRightDrive; + + // This declares the IMU needed to get the current direction the robot is facing + IMU imu; + + @Override + public void init() { + frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); + frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); + backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); + backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); + + // We set the left motors in reverse which is needed for drive trains where the left + // motors are opposite to the right ones. + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); + + // This uses RUN_USING_ENCODER to be more accurate. If you don't have the encoder + // wires, you should remove these + frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + imu = hardwareMap.get(IMU.class, "imu"); + // This needs to be changed to match the orientation on your robot + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = + RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + RevHubOrientationOnRobot orientationOnRobot = new + RevHubOrientationOnRobot(logoDirection, usbDirection); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + } + + @Override + public void loop() { + telemetry.addLine("Press A to reset Yaw"); + telemetry.addLine("Hold left bumper to drive in robot relative"); + telemetry.addLine("The left joystick sets the robot direction"); + telemetry.addLine("Moving the right joystick left and right turns the robot"); + + // If you press the A button, then you reset the Yaw to be zero from the way + // the robot is currently pointing + if (gamepad1.a) { + imu.resetYaw(); + } + // If you press the left bumper, you get a drive from the point of view of the robot + // (much like driving an RC vehicle) + if (gamepad1.left_bumper) { + drive(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + } else { + driveFieldRelative(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + } + } + + // This routine drives the robot field relative + private void driveFieldRelative(double forward, double right, double rotate) { + // First, convert direction being asked to drive to polar coordinates + double theta = Math.atan2(forward, right); + double r = Math.hypot(right, forward); + + // Second, rotate angle by the angle the robot is pointing + theta = AngleUnit.normalizeRadians(theta - + imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + + // Third, convert back to cartesian + double newForward = r * Math.sin(theta); + double newRight = r * Math.cos(theta); + + // Finally, call the drive method with robot relative forward and right amounts + drive(newForward, newRight, rotate); + } + + // Thanks to FTC16072 for sharing this code!! + public void drive(double forward, double right, double rotate) { + // This calculates the power needed for each wheel based on the amount of forward, + // strafe right, and rotate + double frontLeftPower = forward + right + rotate; + double frontRightPower = forward - right - rotate; + double backRightPower = forward + right - rotate; + double backLeftPower = forward - right + rotate; + + double maxPower = 1.0; + double maxSpeed = 1.0; // make this slower for outreaches + + // This is needed to make sure we don't pass > 1.0 to any wheel + // It allows us to keep all of the motors in proportion to what they should + // be and not get clipped + maxPower = Math.max(maxPower, Math.abs(frontLeftPower)); + maxPower = Math.max(maxPower, Math.abs(frontRightPower)); + maxPower = Math.max(maxPower, Math.abs(backRightPower)); + maxPower = Math.max(maxPower, Math.abs(backLeftPower)); + + // We multiply by maxSpeed so that it can be set lower for outreaches + // When a young child is driving the robot, we may not want to allow full + // speed. + frontLeftDrive.setPower(maxSpeed * (frontLeftPower / maxPower)); + frontRightDrive.setPower(maxSpeed * (frontRightPower / maxPower)); + backLeftDrive.setPower(maxSpeed * (backLeftPower / maxPower)); + backRightDrive.setPower(maxSpeed * (backRightPower / maxPower)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java new file mode 100644 index 0000000..5b0f6e9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java @@ -0,0 +1,116 @@ +/* +* Copyright (c) 2025 Alan Smith +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: + +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. + +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; + +/* + * This OpMode illustrates how to use the GoBildaPinpoint + * + * The OpMode assumes that the sensor is configured with a name of "pinpoint". + * + * 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 + * + * See the sensor's product page: https://www.gobilda.com/pinpoint-odometry-computer-imu-sensor-fusion-for-2-wheel-odometry/ + */ +@TeleOp(name = "Sensor: GoBilda Pinpoint", group = "Sensor") +@Disabled +public class SensorGoBildaPinpoint extends OpMode { + // Create an instance of the sensor + GoBildaPinpointDriver pinpoint; + + @Override + public void init() { + // Get a reference to the sensor + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + + // Configure the sensor + configurePinpoint(); + + // Set the location of the robot - this should be the place you are starting the robot from + pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0)); + } + + @Override + public void loop() { + telemetry.addLine("Push your robot around to see it track"); + telemetry.addLine("Press A to reset the position"); + if(gamepad1.a){ + // You could use readings from April Tags here to give a new known position to the pinpoint + pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0)); + } + pinpoint.update(); + Pose2D pose2D = pinpoint.getPosition(); + + telemetry.addData("X coordinate (IN)", pose2D.getX(DistanceUnit.INCH)); + telemetry.addData("Y coordinate (IN)", pose2D.getY(DistanceUnit.INCH)); + telemetry.addData("Heading angle (DEGREES)", pose2D.getHeading(AngleUnit.DEGREES)); + } + + public void configurePinpoint(){ + /* + * Set the odometry pod positions relative to the point that you want the position to be measured from. + * + * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. + * Left of the center is a positive number, right of center is a negative number. + * + * The Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. + * Forward of center is a positive number, backwards is a negative number. + */ + pinpoint.setOffsets(-84.0, -168.0, DistanceUnit.MM); //these are tuned for 3110-0002-0001 Product Insight #1 + + /* + * Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either + * the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD. + * If you're using another kind of odometry pod, uncomment setEncoderResolution and input the + * number of ticks per unit of your odometry pod. For example: + * pinpoint.setEncoderResolution(13.26291192, DistanceUnit.MM); + */ + pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + + /* + * Set the direction that each of the two odometry pods count. The X (forward) pod should + * increase when you move the robot forward. And the Y (strafe) pod should increase when + * you move the robot to the left. + */ + pinpoint.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, + GoBildaPinpointDriver.EncoderDirection.FORWARD); + + /* + * Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary + * The IMU will automatically calibrate when first powered on, but recalibrating before running + * the robot is a good idea to ensure that the calibration is "good". + * resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU. + * This is recommended before you run your autonomous, as a bad initial calibration can cause + * an incorrect starting value for x, y, and heading. + */ + pinpoint.resetPosAndIMU(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java index 6c1f702..2579cd0 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java @@ -100,7 +100,7 @@ public class SensorLimelight3A extends LinearOpMode { status.getPipelineIndex(), status.getPipelineType()); LLResult result = limelight.getLatestResult(); - if (result != null) { + if (result.isValid()) { // Access general information Pose3D botpose = result.getBotpose(); double captureLatency = result.getCaptureLatency(); @@ -109,44 +109,42 @@ public class SensorLimelight3A extends LinearOpMode { telemetry.addData("LL Latency", captureLatency + targetingLatency); telemetry.addData("Parse Latency", parseLatency); telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput())); - - if (result.isValid()) { - telemetry.addData("tx", result.getTx()); - telemetry.addData("txnc", result.getTxNC()); - telemetry.addData("ty", result.getTy()); - telemetry.addData("tync", result.getTyNC()); - telemetry.addData("Botpose", botpose.toString()); + telemetry.addData("tx", result.getTx()); + telemetry.addData("txnc", result.getTxNC()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("tync", result.getTyNC()); - // Access barcode results - List barcodeResults = result.getBarcodeResults(); - for (LLResultTypes.BarcodeResult br : barcodeResults) { - telemetry.addData("Barcode", "Data: %s", br.getData()); - } + telemetry.addData("Botpose", botpose.toString()); - // Access classifier results - List classifierResults = result.getClassifierResults(); - for (LLResultTypes.ClassifierResult cr : classifierResults) { - telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence()); - } + // Access barcode results + List barcodeResults = result.getBarcodeResults(); + for (LLResultTypes.BarcodeResult br : barcodeResults) { + telemetry.addData("Barcode", "Data: %s", br.getData()); + } - // Access detector results - List detectorResults = result.getDetectorResults(); - for (LLResultTypes.DetectorResult dr : detectorResults) { - telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea()); - } + // Access classifier results + List classifierResults = result.getClassifierResults(); + for (LLResultTypes.ClassifierResult cr : classifierResults) { + telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence()); + } - // Access fiducial results - List fiducialResults = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fr : fiducialResults) { - telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(),fr.getTargetXDegrees(), fr.getTargetYDegrees()); - } + // Access detector results + List detectorResults = result.getDetectorResults(); + for (LLResultTypes.DetectorResult dr : detectorResults) { + telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea()); + } - // Access color results - List colorResults = result.getColorResults(); - for (LLResultTypes.ColorResult cr : colorResults) { - telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); - } + // Access fiducial results + List fiducialResults = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fr : fiducialResults) { + telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees()); + } + + // Access color results + List colorResults = result.getColorResults(); + for (LLResultTypes.ColorResult cr : colorResults) { + telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); } } else { telemetry.addData("Limelight", "No data available"); diff --git a/README.md b/README.md index 6630a3d..5b785a6 100644 --- a/README.md +++ b/README.md @@ -59,6 +59,54 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 10.3 (20250625-090416) + +### Breaking Changes +* The behavior of setGlobalErrorMsg() is changed. Note that this is an SDK internal method that is not + meant to be used by team software or third party libraries. Teams or libraries using this method should + find another means to communicate failure. The design intent of setGlobalErrorMsg() is to report an + error and force the user to restart the robot, which in certain circumstances when used inappropriately + could cause a robot to continue running while Driver Station controls are disabled. To prevent this, + processing of a call to setGlobalErrorMsg() is deferred until the robot is in a known safe state. This may + mean that a call to setGlobalErrorMsg() that does not also result in stopping a running OpMode will appear + as though nothing happened until the robot is stopped, at which point, if clearGlobalErrorMsg() has not + been called the message will appear on the Driver Station and a restart will be required. + Addresses issue [1381](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1381) +* Fixes getLatestResult in Limelight3A so if the Limelight hasn't provided data yet, it still returns an LLResult but valid will be false + * If you previously used to check and see if this was `null` to see if the Limelight had been contacted, you now need to use `isValid()` on the result. That is because now it always returns an LLResult even before it talks to the Limelight, but if it doesn't have valid data, the `isValid()` will be `false`. +* Changed all omni samples to use front_left_drive, front_right_drive, back_left_drive, back_right_drive + * This is only breaking for you if you copy one of the changed samples to your own project and expect to use the same robot configuration as before. + +### Known Issues +* The redesigned OnBotJava new file workflow allows the user to use a lowercase letter as the first character of a filename. + This is a regression from 10.2 which required the first character to be uppercase. Software will build, but if the user tries + to rename the file, the rename will fail. + +### Enhancements +* Improved the OBJ new file creation flow workflow. The new flow allows you to easily use samples, craft new custom OpModes and make new Java classes. +* Added support for gamepad edge detection. + * A new sample program `ConceptGamepadEdgeDetection` demonstrates its use. +* Adds a blackboard member to the Opmode that maintains state between opmodes (but not between robot resets). See the ConceptBlackboard sample for how to use it. +* Updated PredominantColorProcessor to also return the predominant color in RGB, HSV and YCrCb color spaces. Updated ConceptVisionColorSensor sample OpMode to display the getAnalysis() result in all three color spaces. +* Adds support for the GoBilda Pinpoint + * Also adds `SensorGoBildaPinpoint` sample to show how to use it +* Added `getArcLength()` and `getCircularity()` to ColorBlobLocatorProcessor.Blob. Added BY_ARC_LENGTH and BY_CIRCULARITY as additional BlobCriteria. +* Added `filterByCriteria()` and `sortByCriteria()` to ColorBlobLocatorProcessor.Util. + * The filter and sort methods for specific criteria have been deprecated. + * The updated sample program `ConceptVisionColorLocator` provides more details on the new syntax. +* Add Help menu item and Help page that is available when connected to the robot controller via Program and Manage. The Help page has links to team resources such as [FTC Documentation](https://ftc-docs.firstinspires.org/), [FTC Discussion Forums](https://ftc-community.firstinspires.org), [Java FTC SDK API Documentation](https://javadoc.io/doc/org.firstinspires.ftc), and [FTC Game Information](https://ftc.game/). +* Self inspection changes: + * List both the Driver Station Name and Robot Controller Name when inspecting the Driver Station. + * Report if the team number portion of the device names do not match. + * -rc is no longer valid as part of a Robot Controller name, must be -RC. + * Use Robot Controller Name or Driver Station Name labels on the inspection screens instead of WIFI Access Point or WIFI Direct Name. + +### Bug Fixes +* Fixes issue [1478](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1478) in AnnotatedHooksClassFilter that ignored exceptions if they occur in one of the SDK app hooks. +* Fix initialize in distance sensor (Rev 2m) to prevent bad data in first call to getDistance. +* Fixes issue [1470](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1470) Scaling a servo range is now irrespective of reverse() being called. For example, if you set the scale range to [0.0, 0.5] and the servo is reversed, it will be from 0.5 to 0.0, NOT 1.0 to 0.5. +* Fixes issue [1232](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1232), a rare race condition where using the log rapidly along with other telemetry could cause a crash. + ## Version 10.2 (20250121-174034) ### Enhancements diff --git a/build.dependencies.gradle b/build.dependencies.gradle index f99ba5c..a8bb721 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -4,14 +4,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:10.2.0' - implementation 'org.firstinspires.ftc:Blocks:10.2.0' - implementation 'org.firstinspires.ftc:RobotCore:10.2.0' - implementation 'org.firstinspires.ftc:RobotServer:10.2.0' - implementation 'org.firstinspires.ftc:OnBotJava:10.2.0' - implementation 'org.firstinspires.ftc:Hardware:10.2.0' - implementation 'org.firstinspires.ftc:FtcCommon:10.2.0' - implementation 'org.firstinspires.ftc:Vision:10.2.0' + implementation 'org.firstinspires.ftc:Inspection:10.3.0' + implementation 'org.firstinspires.ftc:Blocks:10.3.0' + implementation 'org.firstinspires.ftc:RobotCore:10.3.0' + implementation 'org.firstinspires.ftc:RobotServer:10.3.0' + implementation 'org.firstinspires.ftc:OnBotJava:10.3.0' + implementation 'org.firstinspires.ftc:Hardware:10.3.0' + implementation 'org.firstinspires.ftc:FtcCommon:10.3.0' + implementation 'org.firstinspires.ftc:Vision:10.3.0' implementation 'androidx.appcompat:appcompat:1.2.0' }