diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index f13a2a0102c1..c873221b6dd0 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="60" + android:versionName="11.0"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java index d90261ef00ab..3cb4d9fb0f58 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -224,14 +224,17 @@ private void telemetryAprilTag() { for (AprilTagDetection detection : currentDetections) { if (detection.metadata != null) { telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); - telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", - detection.robotPose.getPosition().x, - detection.robotPose.getPosition().y, - detection.robotPose.getPosition().z)); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", - detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), - detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), - detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + // Only use tags that don't have Obelisk in them + if (!detection.metadata.name.contains("Obelisk")) { + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, + detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } } else { telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); 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 index d5e0032f8b80..90243ac50a2c 100644 --- 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 @@ -75,7 +75,7 @@ public void telemetryButtonData() { 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 + // Add an empty line to separate the buttons in telemetry telemetry.addLine(); // Add the status of the Gamepad 1 Right Bumper diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java new file mode 100644 index 000000000000..7e7eb67292b6 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java @@ -0,0 +1,244 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * 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 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.Circle; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.firstinspires.ftc.vision.opencv.ImageRegion; + +import java.util.List; + +/* + * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions. + * This sample is targeted towards circular blobs. To see rectangles, look at ConceptVisionColorLocator_Rectangle + * + * Unlike a "color sensor" which determines the color of nearby object, this "color locator" + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that + * match the requested color range. These blobs can be further filtered and sorted to find the one + * most likely to be the item the user is looking for. + * + * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. + * The ColorBlobLocatorProcessor (CBLP) process is created first, and then the VisionPortal is built. + * The (CBLP) analyses the ROI and locates pixels that match the ColorRange to form a "mask". + * The matching pixels are then collected into contiguous "blobs" of pixels. + * The outer boundaries of these blobs are called its "contour". For each blob, the process then + * creates the smallest possible circle that will fully encase the contour. The user can then call + * getBlobs() to retrieve the list of Blobs, where each contains the contour and the circle. + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest + * contours are listed first. + * + * A colored enclosing circle is drawn on the camera preview to show the location of each Blob + * The original Blob contour can also be added to the preview. + * This is helpful when configuring the ColorBlobLocatorProcessor parameters. + * + * Tip: Connect an HDMI monitor to the Control Hub to view the Color Location process in real-time. + * Or use a screen copy utility like ScrCpy.exe to view the video remotely. + * + * 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: Vision Color-Locator (Circle)") +public class ConceptVisionColorLocator_Circle extends LinearOpMode { + @Override + public void runOpMode() { + /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. + * - Specify the color range you are looking for. Use a predefined color, or create your own + * + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + * Available colors are: RED, BLUE, YELLOW, GREEN, ARTIFACT_GREEN, ARTIFACT_PURPLE + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match + * new Scalar( 32, 176, 0), + * new Scalar(255, 255, 132))) + * + * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixels at upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height in center + * + * - Define which contours are included. + * You can get ALL the contours, ignore contours that are completely inside another contour. + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + * EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up solid colors. + * + * - Turn the displays of contours ON or OFF. + * Turning these on helps debugging but takes up valuable CPU time. + * .setDrawContours(true) Draws an outline of each contour. + * .setEnclosingCircleColor(int color) Draws a circle around each contour. 0 to disable. + * .setBoxFitColor(int color) Draws a rectangle around each contour. 0 to disable. ON by default. + * + * + * - include any pre-processing of the image or mask before looking for Blobs. + * There are some extra processing you can include to improve the formation of blobs. + * Using these features requires an understanding of how they may effect the final + * blobs. The "pixels" argument sets the NxN kernel size. + * .setBlurSize(int pixels) + * Blurring an image helps to provide a smooth color transition between objects, + * and smoother contours. The higher the number, the more blurred the image becomes. + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. + * Blurring too much may hide smaller features. A size of 5 is good for a 320x240 image. + * + * .setErodeSize(int pixels) + * Erosion removes floating pixels and thin lines so that only substantive objects remain. + * Erosion can grow holes inside regions, and also shrink objects. + * "pixels" in the range of 2-4 are suitable for low res images. + * + * .setDilateSize(int pixels) + * Dilation makes objects and lines more visible by filling in small holes, and making + * filled shapes appear larger. Dilation is useful for joining broken parts of an + * object, such as when removing noise from an image. + * "pixels" in the range of 2-4 are suitable for low res images. + * + * .setMorphOperationType(MorphOperationType morphOperationType) + * This defines the order in which the Erode/Dilate actions are performed. + * OPENING: Will Erode and then Dilate which will make small noise blobs go away + * CLOSING: Will Dilate and then Erode which will tend to fill in any small holes in blob edges. + */ + ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) // Use a predefined color match + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.75, 0.75, 0.75, -0.75)) + .setDrawContours(true) // Show contours on the Stream Preview + .setBoxFitColor(0) // Disable the drawing of rectangles + .setCircleFitColor(Color.rgb(255, 255, 0)) // Draw a circle + .setBlurSize(5) // Smooth the transitions between different colors in image + + // the following options have been added to fill in perimeter holes. + .setDilateSize(15) // Expand blobs to fill any divots on the edges + .setErodeSize(15) // Shrink blobs back to original size + .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) + + .build(); + /* + * Build a vision portal to run the Color Locator process. + * + * - Add the colorLocator process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution + * that is supported by your camera. This will improve overall performance and reduce + * latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorLocator) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) { + telemetry.addData("preview on/off", "... Camera Stream\n"); + + // Read the current list + List blobs = colorLocator.getBlobs(); + + /* + * The list of Blobs can be filtered to remove unwanted Blobs. + * 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. + * + * To perform a filter + * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, 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.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.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.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + 50, 20000, blobs); // filter out very small blobs. + + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY, + 0.6, 1, blobs); /* filter out non-circular blobs. + * NOTE: You may want to adjust the minimum value depending on your use case. + * Circularity values will be affected by shadows, and will therefore vary based + * on the location of the camera on your robot and venue lighting. It is strongly + * encouraged to test your vision on the competition field if your event allows + * sensor calibration time. + */ + + /* + * 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. + * Here is an example.: + * ColorBlobLocatorProcessor.Util.sortByCriteria( + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, SortOrder.DESCENDING, blobs); + */ + + telemetry.addLine("Circularity Radius Center"); + + // Display the Blob's circularity, and the size (radius) and center location of its circleFit. + for (ColorBlobLocatorProcessor.Blob b : blobs) { + + Circle circleFit = b.getCircle(); + telemetry.addLine(String.format("%5.3f %3d (%3d,%3d)", + b.getCircularity(), (int) circleFit.getRadius(), (int) circleFit.getX(), (int) circleFit.getY())); + } + + telemetry.update(); + sleep(100); // Match the telemetry update interval. + } + } +} 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_Rectangle.java similarity index 50% rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java index e143d4628ee3..5681f71231dd 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_Rectangle.java @@ -41,78 +41,89 @@ /* * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions * - * Unlike a "color sensor" which determines the color of an object in the field of view, this "color locator" - * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that match the requested color range. - * These blobs can be further filtered and sorted to find the one most likely to be the item the user is looking for. + * Unlike a "color sensor" which determines the color of nearby object, this "color locator" + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that + * match the requested color range. These blobs can be further filtered and sorted to find the one + * most likely to be the item the user is looking for. * * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. - * The ColorBlobLocatorProcessor process is created first, and then the VisionPortal is built to use this process. - * The ColorBlobLocatorProcessor analyses the ROI and locates pixels that match the ColorRange to form a "mask". - * The matching pixels are then collected into contiguous "blobs" of pixels. The outer boundaries of these blobs are called its "contour". - * For each blob, the process then creates the smallest possible rectangle "boxFit" that will fully encase the contour. - * The user can then call getBlobs() to retrieve the list of Blobs, where each Blob contains the contour and the boxFit data. - * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest contours are listed first. + * The ColorBlobLocatorProcessor (CBLP) process is created first, and then the VisionPortal is built. + * The (CBLP) analyses the ROI and locates pixels that match the ColorRange to form a "mask". + * The matching pixels are then collected into contiguous "blobs" of pixels. + * The outer boundaries of these blobs are called its "contour". For each blob, the process then + * creates the smallest possible rectangle "boxFit" that will fully encase the contour. The user can + * then call getBlobs() to retrieve the list of Blobs, where each contains the contour and the boxFit. + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest + * contours are listed first. * - * To aid the user, a colored boxFit rectangle is drawn on the camera preview to show the location of each Blob - * The original Blob contour can also be added to the preview. This is helpful when configuring the ColorBlobLocatorProcessor parameters. + * A colored boxFit rectangle is drawn on the camera preview to show the location of each Blob + * The original Blob contour can also be added to the preview. + * This is helpful when configuring the ColorBlobLocatorProcessor parameters. * * 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: Vision Color-Locator", group = "Concept") -public class ConceptVisionColorLocator extends LinearOpMode +@TeleOp(name = "Concept: Color-Locator (Rectangle)", group = "Concept") +public class ConceptVisionColorLocator_Rectangle extends LinearOpMode { @Override public void runOpMode() { /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. - * - Specify the color range you are looking for. You can use a predefined color, or create you own color range - * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match - * Available predefined colors are: RED, BLUE YELLOW GREEN - * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match - * new Scalar( 32, 176, 0), - * new Scalar(255, 255, 132))) + * - Specify the color range you are looking for. Use a predefined color, or create your own + * + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + * Available colors are: RED, BLUE YELLOW GREEN ARTIFACT_GREEN and ARTIFACT_PURPLE + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match + * new Scalar( 32, 176, 0), + * new Scalar(255, 255, 132))) * * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. * This can be the entire frame, or a sub-region defined using: * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. * Use one form of the ImageRegion class to define the ROI. - * ImageRegion.entireFrame() - * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner - * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height square centered on screen + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixels at upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height in center * * - Define which contours are included. - * You can get ALL the contours, or you can skip any contours that are completely inside another contour. - * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) // return all contours - * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude contours inside other contours - * note: EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up areas of solid color. + * You can get ALL the contours, ignore contours that are completely inside another contour. + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + * EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up solid colors. * - * - turn the display of contours ON or OFF. Turning this on helps debugging but takes up valuable CPU time. + * - turn the display of contours ON or OFF. Helps debugging but takes up valuable CPU time. * .setDrawContours(true) * * - include any pre-processing of the image or mask before looking for Blobs. - * There are some extra processing you can include to improve the formation of blobs. Using these features requires - * an understanding of how they may effect the final blobs. The "pixels" argument sets the NxN kernel size. - * .setBlurSize(int pixels) Blurring an image helps to provide a smooth color transition between objects, and smoother contours. - * The higher the number of pixels, the more blurred the image becomes. - * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. - * Blurring too much may hide smaller features. A "pixels" size of 5 is good for a 320x240 image. - * .setErodeSize(int pixels) Erosion removes floating pixels and thin lines so that only substantive objects remain. - * Erosion can grow holes inside regions, and also shrink objects. - * "pixels" in the range of 2-4 are suitable for low res images. - * .setDilateSize(int pixels) Dilation makes objects more visible by filling in small holes, making lines appear thicker, - * and making filled shapes appear larger. Dilation is useful for joining broken parts of an - * object, such as when removing noise from an image. - * "pixels" in the range of 2-4 are suitable for low res images. + * There are some extra processing you can include to improve the formation of blobs. + * Using these features requires an understanding of how they may effect the final blobs. + * The "pixels" argument sets the NxN kernel size. + * .setBlurSize(int pixels) + * Blurring an image helps to provide a smooth color transition between objects, + * and smoother contours. The higher the number, the more blurred the image becomes. + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. + * Blurring too much may hide smaller features. A size of 5 is good for a 320x240 image. + * + * .setErodeSize(int pixels) + * Erosion removes floating pixels and thin lines so that only substantive objects remain. + * Erosion can grow holes inside regions, and also shrink objects. + * "pixels" in the range of 2-4 are suitable for low res images. + * + * .setDilateSize(int pixels) + * Dilation makes objects and lines more visible by filling in small holes, and making + * filled shapes appear larger. Dilation is useful for joining broken parts of an + * object, such as when removing noise from an image. + * "pixels" in the range of 2-4 are suitable for low res images. */ ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() - .setTargetColorRange(ColorRange.BLUE) // use a predefined color match - .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs - .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5)) // search central 1/4 of camera view - .setDrawContours(true) // Show contours on the Stream Preview - .setBlurSize(5) // Smooth the transitions between different colors in image + .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) // use a predefined color match + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.75, 0.75, 0.75, -0.75)) + .setDrawContours(true) // Show contours on the Stream Preview + .setBlurSize(5) // Smooth the transitions between different colors in image .build(); /* @@ -120,7 +131,7 @@ public void runOpMode() * * - Add the colorLocator process created above. * - Set the desired video resolution. - * Since a high resolution will not improve this process, choose a lower resolution that is + * A high resolution will not improve this process, so choose a lower resolution that is * supported by your camera. This will improve overall performance and reduce latency. * - Choose your video source. This may be * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam @@ -133,10 +144,10 @@ public void runOpMode() .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) .build(); - telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates 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. + // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode. while (opModeIsActive() || opModeInInit()) { telemetry.addData("preview on/off", "... Camera Stream\n"); @@ -146,8 +157,9 @@ public void runOpMode() /* * The list of Blobs can be filtered to remove unwanted Blobs. - * 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. + * 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. * * To perform a filter * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, blobs); @@ -155,12 +167,13 @@ public void runOpMode() * 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. + * 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.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. + * If you put a rubber band around the contour you get the "Convex Hull" of the contour. * The density is the ratio of Contour-area to Convex Hull-area. * * ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO @@ -181,26 +194,25 @@ public void runOpMode() /* * 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.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); + * No more than one sort call should be made. Sorting can be ASCENDING or DESCENDING. + * Here is an example.: + * ColorBlobLocatorProcessor.Util.sortByCriteria( + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, SortOrder.DESCENDING, blobs); */ - telemetry.addLine("Area Density Aspect Arc Circle Center"); + telemetry.addLine("Ctr:(X,Y) Area Dens Aspect Arc Circ"); // 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 %5.3f (%3d,%3d)", - b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) b.getArcLength(), b.getCircularity(), (int) boxFit.center.x, (int) boxFit.center.y)); + telemetry.addLine(String.format("(%3d,%3d) %5d %4.2f %5.2f %3d %5.3f ", + (int) boxFit.center.x, (int) boxFit.center.y, b.getContourArea(), b.getDensity(), + b.getAspectRatio(), (int) b.getArcLength(), b.getCircularity())); } telemetry.update(); - sleep(50); + sleep(100); // Match the telemetry update interval. } } -} +} \ No newline at end of file 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 86195cfcb76c..5cce468d7b7d 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 @@ -40,16 +40,20 @@ * * This sample performs the same function, except it uses a video camera to inspect an object or scene. * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view. - * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching color will be selected. + * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching + * color will be selected. * * To perform this function, a VisionPortal runs a PredominantColorProcessor process. - * The PredominantColorProcessor process is created first, and then the VisionPortal is built to use this process. - * The PredominantColorProcessor analyses the ROI and splits the colored pixels into several color-clusters. + * The PredominantColorProcessor (PCP) process is created first, and then the VisionPortal is built. + * The (PCP) analyses the ROI and splits the colored pixels into several color-clusters. * The largest of these clusters is then considered to be the "Predominant Color" * The process then matches the Predominant Color with the closest Swatch and returns that match. * * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest, - * The Predominant Color is used to paint the rectangle border, so the user can verify that the color is reasonable. + * The Predominant Color is used to paint the rectangle border, so the user can visualize the color. + * + * Tip: Connect an HDMI monitor to the Control Hub to view the Color Sensor process in real-time. + * Or use a screen copy utility like ScrCpy.exe to view the video remotely. * * 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 @@ -65,19 +69,20 @@ public void runOpMode() /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class. * * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect. - * This can be the entire frame, or a sub-region defined using: - * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. - * Use one form of the ImageRegion class to define the ROI. - * ImageRegion.entireFrame() - * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner - * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% width/height square centered on screen + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 square at the top left corner + * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% W * H centered square * * - Set the list of "acceptable" color swatches (matches). * Only colors that you assign here will be returned. * If you know the sensor will be pointing to one of a few specific colors, enter them here. - * Or, if the sensor may be pointed randomly, provide some additional colors that may match the surrounding. + * Or, if the sensor may be pointed randomly, provide some additional colors that may match. * Possible choices are: - * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE; + * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE + * Note: For the 2026 season ARTIFACT_PURPLE and ARTIFACT_GREEN have been added. * * Note that in the example shown below, only some of the available colors are included. * This will force any other colored region into one of these colors. @@ -86,6 +91,8 @@ public void runOpMode() PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder() .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1)) .setSwatches( + PredominantColorProcessor.Swatch.ARTIFACT_GREEN, + PredominantColorProcessor.Swatch.ARTIFACT_PURPLE, PredominantColorProcessor.Swatch.RED, PredominantColorProcessor.Swatch.BLUE, PredominantColorProcessor.Swatch.YELLOW, @@ -98,7 +105,7 @@ public void runOpMode() * * - Add the colorSensor process created above. * - Set the desired video resolution. - * Since a high resolution will not improve this process, choose a lower resolution that is + * Since a high resolution will not improve this process, choose a lower resolution * supported by your camera. This will improve overall performance and reduce latency. * - Choose your video source. This may be * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam @@ -111,34 +118,38 @@ public void runOpMode() .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) .build(); - telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates, 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. + // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode. while (opModeIsActive() || opModeInInit()) { 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 color in RGB, HSV and YCrCb color spaces. + // Request the most recent color analysis. This will return the closest matching + // colorSwatch and the predominant color in the 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 ...} + // 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 ...} + // 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("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.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/SensorAndyMarkIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java new file mode 100644 index 000000000000..cd678efecd99 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java @@ -0,0 +1,193 @@ +/* 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.andymark.AndyMarkIMUOrientationOnRobot; +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.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +import static com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.xyzOrientation; + +/* + * This OpMode shows how to use the AndyMark IMU sensor. It assumes that the AndyMark IMU is + * configured with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot. + * + * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) + * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) + * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) + * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller). + * + * This specific sample DOES NOT assume that the AndyMark IMU is mounted on one of the three + * orthogonal planes (X/Y, X/Z or Y/Z) OR that the AndyMark IMU has only been rotated in a range of + * 90 degree increments. + * + * Note: if your AndyMark IMU is mounted Orthogonally (on a orthogonal surface, angled at some + * multiple of 90 Degrees), then you should use the simpler SensorAndyMarkIMUOrthogonal sample in + * this folder. + * + * But... If your AndyMark IMU is mounted Non-Orthogonally, you must specify one or more rotational + * angles that transform a "Default" IMU orientation into your desired orientation. That is what is + * illustrated here. + * + * 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. + * + * Finally, edit this OpMode to use at least one angle around an axis to orient your AndyMark IMU. + */ +@TeleOp(name = "Sensor: AndyMark IMU Non-Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorAndyMarkIMUNonOrthogonal extends LinearOpMode +{ + // The AndyMark IMU sensor object + private IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the AndyMark IMU. + // This sample expects the AndyMark IMU to be named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the AndyMark IMU is mounted to the robot to get the correct Yaw, Pitch, and + * Roll values. + * + * You can apply up to three axis rotations to orient your AndyMark IMU according to how + * it's mounted on the robot. + * + * The starting point for these rotations is the "Default" IMU orientation, which is: + * 1) IMU laying flat on a horizontal surface, with the AndyMark logo facing up. + * 2) Rotated such that the I2C port is facing forward on the robot. + * + * The order that the rotations are performed matters, so this sample shows doing them in + * the order X, Y, then Z. + * For specifying non-orthogonal IMU mounting orientations, we must temporarily use axes + * defined relative to the AndyMark IMU itself, instead of the usual Robot Coordinate System + * axes used for the results the AndyMark IMU gives us. In the starting orientation, the + * AndyMark IMU axes are aligned with the Robot Coordinate System: + * + * X Axis: Starting at center of IMU, pointing out towards the side. + * Y Axis: Starting at center of IMU, pointing out towards I2C port. + * Z Axis: Starting at center of IMU, pointing up through the AndyMark logo. + * + * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on + * axis. + * + * Some examples. + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example A) Assume that the AndyMark IMU is mounted on a sloped plate at the back of the + * robot, with the I2C port coming out the top of the AndyMark IMU. The plate is tilted UP + * 60 degrees from horizontal. + * + * To get the "Default" IMU into this configuration you would just need a single rotation. + * 1) Rotate the AndyMark IMU +60 degrees around the X axis to tilt up the front edge. + * 2) No rotation around the Y or Z axes. + * + * So the X,Y,Z rotations would be 60,0,0 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example B) Assume that the AndyMark IMU is laying flat on the chassis, but it has been + * twisted 30 degrees towards the right front wheel. + * + * To get the "Default" IMU into this configuration you would just need a single rotation, + * but around a different axis. + * 1) No rotation around the X or Y axes. + * 2) Rotate the AndyMark IMU -30 degrees (Clockwise) around the Z axis, since a positive + * angle would be Counter Clockwise. + * + * So the X,Y,Z rotations would be 0,0,-30 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example C) Assume that the AndyMark IMU is mounted on a vertical plate on the right side + * of the robot, with the AndyMark logo facing out, and the AndyMark IMU rotated so that the + * I2C port is facing down 30 degrees towards the back wheels of the robot. + * + * To get the "Default" IMU into this configuration will require several rotations. + * 1) Rotate the AndyMark IMU +90 degrees around the X axis to get it standing upright with + * the logo pointing backwards on the robot + * 2) Next, rotate the AndyMark IMU +90 around the Y axis to get the logo facing to the + * right. + * 3) Finally rotate the AndyMark IMU +120 degrees around the Z axis to take the I2C port + * from vertical to sloping down 30 degrees and facing towards the back of the robot. + * + * So the X,Y,Z rotations would be 90,90,120 + */ + + // The next three lines define the desired axis rotations. + // To Do: EDIT these values to match YOUR mounting configuration. + double xRotation = 0; // enter the desired X rotation angle here. + double yRotation = 0; // enter the desired Y rotation angle here. + double zRotation = 0; // enter the desired Z rotation angle here. + + Orientation imuRotation = xyzOrientation(xRotation, yRotation, zRotation); + + // Now initialize the AndyMark IMU with this mounting orientation. + AndyMarkIMUOrientationOnRobot orientationOnRobot = new AndyMarkIMUOrientationOnRobot(imuRotation); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard. + while (!isStopRequested()) { + telemetry.addData("IMU orientation", "X=%.1f, Y=%.1f, Z=%.1f \n", xRotation, yRotation, zRotation); + + // Check to see if heading reset is requested. + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve rotational angles and velocities. + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java new file mode 100644 index 000000000000..58cee6d3cab9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java @@ -0,0 +1,144 @@ +/* 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.andymark.AndyMarkIMUOrientationOnRobot; +import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.I2cPortFacingDirection; +import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.LogoFacingDirection; +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.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode shows how to use the AndyMark IMU sensor. It assumes that the AndyMark IMU is + * configured with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot. + * + * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) + * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) + * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) + * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller). + * + * This specific sample assumes that the AndyMark IMU is mounted on one of the three orthogonal + * planes (X/Y, X/Z or Y/Z) and that the AndyMark IMU has only been rotated in a range of 90 degree + * increments. + * + * Note: if your AndyMark IMU is mounted on a surface angled at some non-90 Degree multiple (like + * 30), then you should use the SensorAndyMarkIMUNonOrthogonal sample in this folder. + * + * This "Orthogonal" requirement means that: + * + * 1) The AndyMark logo printed on the top of the AndyMark IMU can ONLY be pointing in one of six directions: + * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * 2) The I2C port can only be pointing in one of the same six directions: + * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * So, to fully define how your AndyMark IMU is mounted to the robot, you must simply specify: + * LogoFacingDirection + * I2cPortFacingDirection + * + * 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. + * + * Finally, choose the two correct parameters to define how your AndyMark IMU is mounted and edit + * this OpMode to use those parameters. + */ +@TeleOp(name = "Sensor: AndyMark IMU Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorAndyMarkIMUOrthogonal extends LinearOpMode +{ + // The AndyMark IMU sensor object + private IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the AndyMark IMU. + // This sample expects the AndyMark IMU to be named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the AndyMark IMU is mounted to the robot to get the correct Yaw, Pitch, and + * Roll values. + * + * Two input parameters are required to fully specify the Orientation. + * The first parameter specifies the direction the AndyMark logo on the IMU is pointing. + * The second parameter specifies the direction the I2C port on the IMU is pointing. + * All directions are relative to the robot, and left/right is as-viewed from behind the robot. + */ + + /* The next two lines define the IMU orientation. + * To Do: EDIT these two lines to match YOUR mounting configuration. + */ + LogoFacingDirection logoDirection = LogoFacingDirection.UP; + I2cPortFacingDirection i2cDirection = I2cPortFacingDirection.FORWARD; + + AndyMarkIMUOrientationOnRobot orientationOnRobot = new AndyMarkIMUOrientationOnRobot(logoDirection, i2cDirection); + + // Now initialize the AndyMark IMU with this mounting orientation. + // Note: if you choose two conflicting directions, this initialization will cause a code exception. + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard. + while (!isStopRequested()) { + telemetry.addData("IMU orientation", "Logo=%s I2C=%s\n ", logoDirection, i2cDirection); + + // Check to see if heading reset is requested. + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve rotational angles and velocities. + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java new file mode 100644 index 000000000000..ce6e943e33e3 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java @@ -0,0 +1,85 @@ +/* +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.andymark.AndyMarkTOF; +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.DistanceSensor; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the AndyMark 2m TOF Lidar Sensor. + * + * The OpMode assumes that the sensor is configured with a name of "sensor_distance". + * + * 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 = "Sensor: AndyMarkTOF", group = "Sensor") +@Disabled +public class SensorAndyMarkTOF extends LinearOpMode { + + private DistanceSensor sensorDistance; + + @Override + public void runOpMode() { + // you can use this as a regular DistanceSensor. + sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_distance"); + + // you can also cast this to a AndyMarkTOF if you want to use added + // methods associated with the AndyMarkTOF class. + AndyMarkTOF sensorTimeOfFlight = (AndyMarkTOF) sensorDistance; + + telemetry.addData(">>", "Press start to continue"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + // generic DistanceSensor methods. + telemetry.addData("deviceName", sensorDistance.getDeviceName() ); + telemetry.addData("range", String.format("%.01f mm", sensorDistance.getDistance(DistanceUnit.MM))); + telemetry.addData("range", String.format("%.01f cm", sensorDistance.getDistance(DistanceUnit.CM))); + telemetry.addData("range", String.format("%.01f m", sensorDistance.getDistance(DistanceUnit.METER))); + telemetry.addData("range", String.format("%.01f in", sensorDistance.getDistance(DistanceUnit.INCH))); + + // AndyMarkTOF specific methods. + telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID())); + telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur())); + + telemetry.update(); + } + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java index 7546c9dea074..d0411af14834 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java @@ -48,18 +48,22 @@ * * There will be some variation in the values measured depending on the specific sensor you are using. * - * You can increase the gain (a multiplier to make the sensor report higher values) by holding down - * the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad. + * If the color sensor supports adjusting the gain, you can increase the gain (a multiplier to make + * the sensor report higher values) by holding down the A button on the gamepad, and decrease the + * gain by holding down the B button on the gamepad. The AndyMark Proximity & Color Sensor does not + * support this. * * If the color sensor has a light which is controllable from software, you can use the X button on * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have - * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2. + * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2. The + * AndyMark Proximity & Color Sensor does not support this. * * If the color sensor also supports short-range distance measurements (usually via an infrared - * proximity sensor), the reported distance will be written to telemetry. As of September 2020, - * the only color sensors that support this are the ones from REV Robotics. These infrared proximity - * sensor measurements are only useful at very small distances, and are sensitive to ambient light - * and surface reflectivity. You should use a different sensor if you need precise distance measurements. + * proximity sensor), the reported distance will be written to telemetry. As of September 2025, + * the only color sensors that support this are the ones from REV Robotics and the AndyMark + * Proximity & Color Sensor. These infrared proximity sensor measurements are only useful at very + * small distances, and are sensitive to ambient light and surface reflectivity. You should use a + * different sensor if you need precise distance measurements. * * 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 diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java index f797c6b0dc72..312d7f5110e3 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java @@ -1,5 +1,5 @@ /* - * Copyright (c) 2024 DigitalChickenLabs + * Copyright (c) 2025 DigitalChickenLabs * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -25,20 +25,28 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.util.ElapsedTime; + import org.firstinspires.ftc.robotcore.external.Telemetry; /* - * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module + * This OpMode illustrates how to use DigitalChickenLabs OctoQuad Quad Encoder & Pulse Width I/F Module * - * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs. - * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder. - * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, + * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or + * Pulse-Width Absolute Encoder inputs. Relative Quadrature encoders are found on most FTC motors, + * and some stand-alone position sensors like the REV Thru-Bore encoder. Pulse-Width encoders are + * less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, * as can several sonar rangefinders such as the MaxBotix MB1000 series. * - * This basic sample shows how an OctoQuad can be used to read the position three Odometry pods fitted - * with REV Thru-Bore encoders. For a more advanced example showing additional OctoQuad capabilities, see the SensorOctoQuadAdv sample. + * Note: SDK 11.0+ requires that the OctoQuad is running firmware V3.0 or greater. + * Visit https://github.com/DigitalChickenLabs/OctoQuad/tree/master/firmware for instruction + * on how to upgrade your OctoQuad's firmware. + * + * This basic sample shows how an OctoQuad can be used to read the position of three Odometry pods + * fitted with REV Thru-Bore encoders. For a more advanced example with additional OctoQuad + * capabilities, see the SensorOctoQuadAdv sample. * - * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. + * This OpMode assumes the OctoQuad is attached to an I2C interface named "octoquad" in the robot config. * * The code assumes the first three OctoQuad inputs are connected as follows * - Chan 0: for measuring forward motion on the left side of the robot. @@ -54,21 +62,24 @@ * * See the sensor's product page: https://www.tindie.com/products/35114/ */ -@TeleOp(name = "OctoQuad Basic", group="OctoQuad") @Disabled +@TeleOp(name = "OctoQuad Basic", group="OctoQuad") public class SensorOctoQuad extends LinearOpMode { // Identify which encoder OctoQuad inputs are connected to each odometry pod. - private final int ODO_LEFT = 0; // Facing forward direction on left side of robot (Axial motion) - private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion) - private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot (Lateral motion) + private final int ODO_LEFT = 0; // Facing forward direction on left side of robot + private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot + private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot - // Declare the OctoQuad object and members to store encoder positions and velocities + // Declare the OctoQuad object; private OctoQuad octoquad; private int posLeft; private int posRight; private int posPerp; + private int velLeft; + private int velRight; + private int velPerp; /** * This function is executed when this OpMode is selected from the Driver Station. @@ -83,12 +94,15 @@ public void runOpMode() { telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion()); // Reverse the count-direction of any encoder that is not what you require. - // e.g. if you push the robot forward and the left encoder counts down, then reverse it so it counts up. + // e.g. if you push the robot forward and the left encoder counts down, then reverse it. octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE); octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD); octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD); - // Any changes that are made should be saved in FLASH just in case there is a sensor power glitch. + // set the interval over which pulses are counted to determine velocity. + octoquad.setAllVelocitySampleIntervals(50); // 50 mSec means 20 velocity updates per second. + + // Save any changes that are made, just in case there is a sensor power glitch. octoquad.saveParametersToFlash(); telemetry.addLine("\nPress START to read encoder values"); @@ -98,7 +112,7 @@ public void runOpMode() { // Configure the telemetry for optimal display of data. telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); - telemetry.setMsTransmissionInterval(50); + telemetry.setMsTransmissionInterval(100); // Set all the encoder inputs to zero. octoquad.resetAllPositions(); @@ -117,25 +131,26 @@ public void runOpMode() { readOdometryPods(); // Display the values. - telemetry.addData("Left ", "%8d counts", posLeft); - telemetry.addData("Right", "%8d counts", posRight); - telemetry.addData("Perp ", "%8d counts", posPerp); + telemetry.addData("Left P", "%7d V :%6d CPS ", posLeft, velLeft); + telemetry.addData("Right P", "%7d V :%6d CPS ", posRight, velRight); + telemetry.addData("Perp P", "%7d V :%6d CPS ", posPerp, velPerp); telemetry.update(); } } private void readOdometryPods() { // For best performance, we should only perform ONE transaction with the OctoQuad each cycle. - // Since this example only needs to read positions from a few channels, we could use either - // readPositionRange(idxFirst, idxLast) to get a select number of sequential channels - // or - // readAllPositions() to get all 8 encoder readings + // This can be achieved in one of two ways: + // 1) by doing a block data read once per control cycle + // or + // 2) by doing individual caching reads, but only reading each encoder value ONCE per cycle. // - // Since both calls take almost the same amount of time, and the actual channels may not end up - // being sequential, we will read all of the encoder positions, and then pick out the ones we need. - int[] positions = octoquad.readAllPositions(); - posLeft = positions[ODO_LEFT]; - posRight = positions[ODO_RIGHT]; - posPerp = positions[ODO_PERP]; + // Since method 2 is simplest, we will use it here. + posLeft = octoquad.readSinglePosition_Caching(ODO_LEFT); + posRight = octoquad.readSinglePosition_Caching(ODO_RIGHT); + posPerp = octoquad.readSinglePosition_Caching(ODO_PERP); + velLeft = octoquad.readSingleVelocity_Caching(ODO_LEFT) * 20; // scale up to cps + velRight = octoquad.readSingleVelocity_Caching(ODO_RIGHT) * 20; // scale up to cps + velPerp = octoquad.readSingleVelocity_Caching(ODO_PERP) * 20; // scale up to cps } } diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java index e763b9aac2da..0e412ef4191c 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java @@ -22,7 +22,6 @@ package org.firstinspires.ftc.robotcontroller.external.samples; import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; -import com.qualcomm.hardware.digitalchickenlabs.OctoQuadBase; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; @@ -37,50 +36,53 @@ import java.util.List; /* - * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module + * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature + * Encoder & Pulse Width Interface Module. The OctoQuad has 8 input channels that can used to read + * either Quadrature Encoder signals (like with most FTC motors) or Pulse-Width style Absolute Encoder + * inputs (eg: REV Through Bore encoder pulse width output). * - * The OctoQuad has 8 input channels that can used to read either Quadrature Encoder signals (like with most FTC motors) - * or Pulse-Width style Absolute Encoder inputs (eg: REV Through Bore encoder pulse width output). + * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width + * measurement and velocity measurement. For a more basic OpMode just showing how to read encoder + * inputs, see the SensorOctoQuad sample. * - * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width measurement and velocity measurement. - * For a more basic OpMode just showing how to read encoder inputs, see the SensorOctoQuad sample. - * - * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. + * This OpMode assumes the OctoQuad is attached to an I2C interface named "octoquad" in the robot config. * * One system that requires a lot of encoder inputs is a Swerve Drive system. - * Such a drive requires two encoders per drive module: one for position & velocity of the Drive motor/wheel, and one for position/angle of the Steering motor. - * The Drive motor usually requires a quadrature encoder, and the Steering motor requires an Absolute encoder. - * This quantity and combination of encoder inputs is a challenge, but a single OctoQuad could be used to read them all. + * Such a drive requires two encoders per drive module: + * One encoder for the Drive motor/wheel position.velocity, and one for Steer motor angle. + * The Drive motor requires a quadrature encoder, and the Steering motor requires an Absolute encoder. * * This sample will configure an OctoQuad for a swerve drive, as follows * - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs * - Configure a velocity sample interval of 25 mSec - * - Configure a pulse width input range of 1-1024 uSec for a REV Through Bore Encoder's Absolute Pulse output. + * - Configure a pulse width input range of 1-1024 uSec for a REV Encoder's Absolute Pulse output. * - * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage the 4 swerve modules. + * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage 4 swerve modules. * An OctoSwerveModule class will be created to configure and read a single swerve module. * * Wiring: - * The OctoQuad will be configured to accept Quadrature encoders on the first four channels and Absolute (pulse width) encoders on the last four channels. + * The OctoQuad will be configured to accept Quadrature encoders on the first four channels and + * Absolute (pulse width) encoders on the last four channels. + * + * The standard encoder cable will connect each Driver Motor encoder to the OctoQuad. (channels 0-3) * - * The standard 4-pin to 4-pin cable can be used to connect each Driver Motor encoder to the OctoQuad. (channels 0-3) + * A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder + * to the OctoQuad. (channels 4-7) * - * A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder to the OctoQuad. (channels 4-7) - * To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the Provided 6-pin to 4-pin cable will need to be modified. - * At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the ABS pin-5 position. This can be done easily - * by using a small flathead screwdriver to lift the small white tab holding the metal wire crimp in place and gently pulling the wire out. + * To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the + * Provided 6-pin to 4-pin cable will need to be modified. + * At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the + * ABS pin-5 position. This can be done easily by using a small flathead screwdriver to lift the + * small white tab holding the metal wire crimp in place and gently pulling the wire out. * See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment. * * 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 * - * Note: If you prefer, you can move the two support classes from this file, and place them in their own files. - * But leaving them in place is simpler for this example. - * * See the sensor's product page: https://www.tindie.com/products/35114/ */ -@TeleOp(name="OctoQuad Advanced", group="OctoQuad") @Disabled +@TeleOp(name="OctoQuad Advanced", group="OctoQuad") public class SensorOctoQuadAdv extends LinearOpMode { @Override @@ -161,22 +163,25 @@ public OctoSwerveDrive(OctoQuad octoquad) { // Create the four OctoSwerveModules, and add them to a 'list' for easier reference. - // Note: The wheel/channel order is set here. Ensure your encoder cables match. + // Notes: + // The wheel/channel order is set here. Ensure your encoder cables match. // - // Note: The angleOffset must be set for each module so a forward facing wheel shows a steer angle of 0 degrees. - // The process for determining the correct angleOffsets is as follows: - // 1) Set all the angleValues (below) to zero then build and deploy the code. - // 2) Physically rotate all the swerve drives so the wheels are facing forward in the desired 0 degree position - // 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module. - // 4) Update the code by entering the recorded Degrees value for each module as the angleOffset (last) parameter in the lines below. + // The angleOffset must be set for each module so a forward facing wheel shows a steer + // angle of 0 degrees. The process for determining the correct angleOffsets is as follows: + // 1) Set all the angleValues (below) to zero then build and deploy the code. + // 2) Rotate all the swerve drives so the wheels are forward in the desired 0 degree position + // 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module. + // 4) Update the code by entering the recorded Degrees value for each module as the + // angleOffset (last) parameter in the lines below. // - // Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when the wheels are facing forward. - // Also verify that the correct module values change appropriately when you manually spin (drive) and rotate (steer) a wheel. + // Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when + // the wheels are facing forward. Also verify that the correct module values change + // appropriately when you manually spin (drive) and rotate (steer) a wheel. - allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive = 0, Steer = 4 - allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive = 1, Steer = 5 - allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive = 2, Steer = 6 - allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive = 3, Steer = 7 + allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive=0, Steer=4 + allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive=1, Steer=5 + allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive=2, Steer=6 + allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive=3, Steer=7 // now make sure the settings persist through any power glitches. octoquad.saveParametersToFlash(); @@ -186,7 +191,7 @@ public OctoSwerveDrive(OctoQuad octoquad) { * Updates all 4 swerve modules */ public void updateModules() { - // Read full OctoQuad data block and then pass DataBlock to each swerve module to update themselves. + // Read full OctoQuad data block and then pass it to each swerve module to update themselves. octoquad.readAllEncoderData(encoderDataBlock); for (OctoSwerveModule module : allModules) { module.updateModule(encoderDataBlock); @@ -219,39 +224,41 @@ class OctoSwerveModule { private final String name; private final int channel; private final double angleOffset; - private final double steerDirMult; - private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates per second. - private static final double DEGREES_PER_US = (360.0 / 1024.0); // based on REV Through Bore Encoder + private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates/Sec. + private static final double DEGREES_PER_US = (360.0 / 1024.0); // REV Through Bore Encoder private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS); - // The correct drive and turn directions must be set for the Swerve Module based on the specific hardware geometry. + // The correct drive and turn directions must be set for the Swerve Module. // Forward motion must generate an increasing drive count. // Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees) - private static final boolean INVERT_DRIVE_ENCODER = false; // Set true if forward motion decreases drive "Count" - private static final boolean INVERT_STEER_ENCODER = false; // Set true if counter clockwise steer action decreases steer "Degree" + private static final boolean INVERT_DRIVE_ENCODER = false; + private static final boolean INVERT_STEER_ENCODER = false; /*** * @param octoquad provide access to configure OctoQuad * @param name name used for telemetry display * @param quadChannel Quadrature encoder channel. Pulse Width channel is this + 4 - * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. (see comments above) + * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. */ public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) { this.name = name; this.channel = quadChannel; this.angleOffset = angleOffset; - this.steerDirMult = INVERT_STEER_ENCODER ? -1 : 1 ; // create a multiplier to flip the steer angle. - // Set the drive encoder direction. Note the absolute encoder does not have built-in direction inversion. - octoquad.setSingleEncoderDirection(channel, INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + // Set both encoder directions. + octoquad.setSingleEncoderDirection(channel, + INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + octoquad.setSingleEncoderDirection(channel + 4, + INVERT_STEER_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); // Set the velocity sample interval on both encoders octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS); octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS); // Setup Absolute encoder pulse range to match REV Through Bore encoder. - octoquad.setSingleChannelPulseWidthParams (channel + 4, new OctoQuad.ChannelPulseWidthParams(1,1024)); + octoquad.setSingleChannelPulseWidthParams (channel + 4, + new OctoQuad.ChannelPulseWidthParams(1,1024)); } /*** @@ -259,13 +266,15 @@ public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double * @param encoderDataBlock most recent full data block read from OctoQuad. */ public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) { - driveCounts = encoderDataBlock.positions[channel]; // get Counts. - driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; // convert counts/interval to counts/sec + driveCounts = encoderDataBlock.positions[channel]; + driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; // convert uS to degrees. Add in any possible direction flip. - steerDegrees = AngleUnit.normalizeDegrees((encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US * steerDirMult) - angleOffset); + steerDegrees = AngleUnit.normalizeDegrees( + (encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US) - angleOffset); // convert uS/interval to deg per sec. Add in any possible direction flip. - steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * DEGREES_PER_US * steerDirMult * VELOCITY_SAMPLES_PER_S; + steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * + DEGREES_PER_US * VELOCITY_SAMPLES_PER_S; } /** @@ -273,6 +282,7 @@ public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) { * @param telemetry OpMode Telemetry object */ public void show(Telemetry telemetry) { - telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec); + telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", + driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec); } } diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java new file mode 100644 index 000000000000..486c4681b62e --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java @@ -0,0 +1,255 @@ +/* + * Copyright (c) 2025 DigitalChickenLabs + * + * 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.digitalchickenlabs.OctoQuad; +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; + +/* + * This OpMode demonstrates how to use the "absolute localizer" feature of the + * Digital Chicken OctoQuad FTC Edition MK2. + * + * Note: The MK2 OctoQuad has an Inertial Measurement Unit (IMU) that the MK1 does not, so this + * code will ONLY work with the MK2 version. + * + * It is STRONGLY recommended to read the Quick Start guide for the localizer feature, located here: + * https://github.com/DigitalChickenLabs/OctoQuad/blob/master/documentation/ + * + * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the + * robot configuration. + * + * 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.tindie.com/products/37799/ + */ + +@Disabled +@TeleOp(name="OctoQuad Localizer", group="OctoQuad") +public class SensorOctoQuadLocalization extends LinearOpMode +{ + // ##################################################################################### + // + // YOU MUST ADJUST THESE CONSTANTS FOR YOUR ROBOT! + // SEE THE QUICKSTART GUIDE: + // https://github.com/DigitalChickenLabs/OctoQuad/blob/master/documentation/ + // + // AND THE TUNING OPMODES: + // https://github.com/DigitalChickenLabs/OctoQuad/blob/master/code_examples/FTC + // + // ##################################################################################### + static final int DEADWHEEL_PORT_X = 0; // encoder port on OctoQuad + static final int DEADWHEEL_PORT_Y = 1; // encoder port on OctoQuad + static final OctoQuad.EncoderDirection DEADWHEEL_X_DIR = OctoQuad.EncoderDirection.FORWARD; + static final OctoQuad.EncoderDirection DEADWHEEL_Y_DIR = OctoQuad.EncoderDirection.REVERSE; + static final float X_TICKS_PER_MM = 12.34f; // eg. 19.89f for a goBILDA 4-Bar Odometry Pod + static final float Y_TICKS_PER_MM = 12.34f; // eg. 19.89f for a goBILDA 4-Bar Odometry Pod + static final float TCP_OFFSET_X_MM = 123.4f; // eg. 147.0f from QuickStart Guide diagram + static final float TCP_OFFSET_Y_MM = 123.4f; // eg.-158.0f from QuickStart Guide diagram + static final float IMU_SCALAR = 1.0f; // Rotational scale factor. + // ##################################################################################### + + // Conversion factor for radians --> degrees + static final double RAD2DEG = 180/Math.PI; + + // For tracking the number of CRC mismatches + int badPackets = 0; + int totalPackets = 0; + boolean warn = false; + + // Data structure which will store the localizer data read from the OctoQuad + OctoQuad.LocalizerDataBlock localizer = new OctoQuad.LocalizerDataBlock(); + + /* + * Main OpMode function + */ + public void runOpMode() + { + // Begin by retrieving a handle to the device from the hardware map. + OctoQuad oq = hardwareMap.get(OctoQuad.class, "octoquad"); + + // Increase the telemetry update frequency to make the data display a bit less laggy + telemetry.setMsTransmissionInterval(50); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + // Configure a range of parameters for the absolute localizer + // --> Read the quick start guide for an explanation of these!! + // IMPORTANT: these parameter changes will not take effect until the localizer is reset! + oq.setSingleEncoderDirection(DEADWHEEL_PORT_X, DEADWHEEL_X_DIR); + oq.setSingleEncoderDirection(DEADWHEEL_PORT_Y, DEADWHEEL_Y_DIR); + oq.setLocalizerPortX(DEADWHEEL_PORT_X); + oq.setLocalizerPortY(DEADWHEEL_PORT_Y); + oq.setLocalizerCountsPerMM_X(X_TICKS_PER_MM); + oq.setLocalizerCountsPerMM_Y(Y_TICKS_PER_MM); + oq.setLocalizerTcpOffsetMM_X(TCP_OFFSET_X_MM); + oq.setLocalizerTcpOffsetMM_Y(TCP_OFFSET_Y_MM); + oq.setLocalizerImuHeadingScalar(IMU_SCALAR); + oq.setLocalizerVelocityIntervalMS(25); + oq.setI2cRecoveryMode(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR); + + // Resetting the localizer will apply the parameters configured above. + // This function will NOT block until calibration of the IMU is complete - + // for that you need to look at the status returned by getLocalizerStatus() + oq.resetLocalizerAndCalibrateIMU(); + + /* + * Init Loop + */ + while (opModeInInit()) + { + telemetry.addData("OQ Firmware Version", oq.getFirmwareVersionString()); + telemetry.addData("Localizer Status", oq.getLocalizerStatus()); + telemetry.addData("Heading Axis Detection", oq.getLocalizerHeadingAxisChoice()); + telemetry.addLine(" "); + + warnIfNotTuned(); + + telemetry.addLine("Press Play to start navigating"); + telemetry.update(); + + sleep(100); + } + + /* + * Don't proceed to the main loop until calibration is complete + */ + while (!isStopRequested() && (oq.getLocalizerStatus() != OctoQuad.LocalizerStatus.RUNNING)) + { + telemetry.addLine("Waiting for IMU Calibration to complete\n"); + telemetry.addData("Localizer Status", oq.getLocalizerStatus()); + telemetry.update(); + sleep(100); + } + + // Establish a starting position for the robot. This will be 0,0,0 by default so this line + // is rather redundant, but you could change it to be anything you want + oq.setLocalizerPose(0, 0, 0); + + // Not required for localizer, but left in here since raw counts are displayed + // on telemetry for debugging + oq.resetAllPositions(); + + /* + * MAIN LOOP + */ + while (opModeIsActive()) + { + // Use the Gamepad A/Cross button to teleport to a new location and heading + if (gamepad1.crossWasPressed()) + { + oq.setLocalizerPose(200, 200, (float)(Math.PI/2.0f)); + } + + // Read updated data from the OctoQuad into the 'localizer' data structure + oq.readLocalizerData(localizer); + + // ################################################################################# + // IMPORTANT! Check whether the received CRC for the data is correct. An incorrect + // CRC indicates that the data was corrupted in transit and cannot be + // trusted. This can be caused by internal or external ESD. + // + // If the CRC is NOT reported as OK, you should throw away the data and + // try to read again. + // + // NOTE: Raw odometry pod counts are displayed to verify correct direction of rotation + // When the robot is pushed FWD, the X pod counts must increase in value + // When the robot is pushed LEFT, the Y pod counts must increase in value + // Use the setSingleEncoderDirection() method to make any reversals. + // ################################################################################# + if (localizer.crcOk) + { + warnIfNotTuned(); + + // Display Robot position data + telemetry.addData("Localizer Status", localizer.localizerStatus); + telemetry.addData("Heading deg", "%.2f", localizer.heading_rad * RAD2DEG); + telemetry.addData("Rotation dps", "%.2f", localizer.velHeading_radS * RAD2DEG); + telemetry.addData("X Pos mm", localizer.posX_mm); + telemetry.addData("Y Pos mm", localizer.posY_mm); + telemetry.addData("X Vel mm/s", localizer.velX_mmS); + telemetry.addData("Y Vel mm/s", localizer.velY_mmS); + telemetry.addLine("\nPress Gamepad A (Cross) to teleport"); + + // ############################################################################# + // IMPORTANT!! + // + // These two encoder status lines will slow the loop down, + // so they can be removed once the encoder direction has been verified. + // ############################################################################# + telemetry.addData("\nRaw X Pod Counts", oq.readSinglePosition_Caching(DEADWHEEL_PORT_X)); + telemetry.addData("Raw Y Pod Counts", oq.readSinglePosition_Caching(DEADWHEEL_PORT_Y)); + } + else + { + badPackets++; + telemetry.addLine("Data CRC not valid"); + } + totalPackets++; + + // Print some statistics about CRC validation + telemetry.addLine(String.format("%d CRC error(s) in %d packets", badPackets, totalPackets)); + + // Send updated telemetry to the Driver Station + telemetry.update(); + } + } + + long lastWarnFlashTimeMs = 0; + boolean warnFlash = false; + + void warnIfNotTuned() + { + String warnString = ""; + if (IMU_SCALAR == 1.0f) + { + warnString += "WARNING: IMU_SCALAR not tuned.
"; + warn = true; + } + if (X_TICKS_PER_MM == 12.34f || Y_TICKS_PER_MM == 12.34f) + { + warnString += "WARNING: TICKS_PER_MM not tuned.
"; + warn = true; + } + if (TCP_OFFSET_X_MM == 123.4f || TCP_OFFSET_Y_MM == 123.4f) + { + warnString += "WARNING: TCP_OFFSET not tuned.
"; + warn = true; + } + if (warn) + { + warnString +="
 - Read the code COMMENTS for tuning help.
"; + + if (System.currentTimeMillis() - lastWarnFlashTimeMs > 1000) + { + lastWarnFlashTimeMs = System.currentTimeMillis(); + warnFlash = !warnFlash; + } + + telemetry.addLine(String.format("%s", + warnFlash ? "red" : "white", warnString)); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java index a962919fcfe7..60be6c9f2c36 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java @@ -69,6 +69,7 @@ public class UtilityOctoQuadConfigMenu extends LinearOpMode TelemetryMenu.MenuElement menuAbsParams = new TelemetryMenu.MenuElement("Abs. Encoder Pulse Width Params", false); TelemetryMenu.IntegerOption[] optionsAbsParamsMax = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; TelemetryMenu.IntegerOption[] optionsAbsParamsMin = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; + TelemetryMenu.BooleanOption[] optionsAbsParamsWrapTracking = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS]; TelemetryMenu.OptionElement optionProgramToFlash; TelemetryMenu.OptionElement optionSendToRAM; @@ -161,9 +162,16 @@ void onClick() // called on OpMode thread OctoQuad.MIN_PULSE_WIDTH_US, OctoQuad.MAX_PULSE_WIDTH_US, params.min_length_us); + + optionsAbsParamsWrapTracking[i] = new TelemetryMenu.BooleanOption( + String.format("Chan %d wrap tracking enabled", i), + octoquad.getSingleChannelPulseWidthTracksWrap(i), + "yes", + "no"); } menuAbsParams.addChildren(optionsAbsParamsMin); menuAbsParams.addChildren(optionsAbsParamsMax); + menuAbsParams.addChildren(optionsAbsParamsWrapTracking); optionProgramToFlash = new TelemetryMenu.OptionElement() { @@ -266,6 +274,7 @@ void sendSettingsToRam() params.min_length_us = optionsAbsParamsMin[i].getValue(); octoquad.setSingleChannelPulseWidthParams(i, params); + octoquad.setSingleChannelPulseWidthTracksWrap(i, optionsAbsParamsWrapTracking[i].val); } octoquad.setI2cRecoveryMode((OctoQuad.I2cRecoveryMode) optionI2cResetMode.getValue()); diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java similarity index 100% rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java similarity index 100% rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java diff --git a/README.md b/README.md index 5b785a68f6bf..522fb8048e75 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## NOTICE -This repository contains the public FTC SDK for the INTO THE DEEP (2024-2025) competition season. +This repository contains the public FTC SDK for the DECODE (2025-2026) competition season. ## Welcome! This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. @@ -59,6 +59,40 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 11.0 (20250827-105138) + +### Enhancements + +* OnBotJava now has the concept of a project. + A project is a collection of related files. A project may be chosen by selecting 'Example Project' + from the 'File type:' dropdown. Doing so will populate the dropdown to the immediate right with + a list of projects to choose from. + When selecting a project all of the related files appear in the left pane of the workspace + underneath a directory with the chosen project name. + This is useful for example for ConceptExternalHardwareClass which has a dependency upon + RobotHardware. This feature simplifies the usage of this Concept example by automatically + pulling in dependent classes. +* Adds support for AndyMark ToF, IMU, and Color sensors. +* The Driver Station app indicates if WiFi is disabled on the device. +* Adds several features to the Color Processing software: + * DECODE colors `ARTIFACT_GREEN` and `ARTIFACT_PURPLE` + * Choice of the order of pre-processing steps Erode and Dilate + * Best-fit preview shape called `circleFit`, an alternate to the existing `boxFit` + * Sample OpMode `ConceptVisionColorLocator_Circle`, an alternate to the renamed `ConceptVisionColorLocator_Rectangle` +* The Driver Station app play button has a green background with a white play symbol if + * the driver station and robot controller are connected and have the same team number + * there is at least one gamepad attached + * the timer is enabled (for an Autonomous OpMode) +* Updated AprilTag Library for DECODE. Notably, getCurrentGameTagLibrary() now returns DECODE tags. + * Since the AprilTags on the Obelisk should not be used for localization, the ConceptAprilTagLocalization samples only use those tags without the name 'Obelisk' in them. +* OctoQuad I2C driver updated to support firmware v3.x + * Adds support for odometry localizer on MK2 hardware revision + * Adds ability to track position for an absolute encoder across multiple rotations + * Note that some driver APIs have changed; minor updates to user software may be required + * Requires firmware v3.x. For instructions on updating firmware, see + https://github.com/DigitalChickenLabs/OctoQuad/blob/master/documentation/OctoQuadDatasheet_Rev_3.0C.pdf + + ## Version 10.3 (20250625-090416) ### Breaking Changes diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java index e2ad3f297782..9f83c2eaea99 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java @@ -1,53 +1,53 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.VisionPipelines.WebcamPipelineNotes; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvWebcam; - -@Autonomous -public class WebcamInitNotes extends OpMode -{ - - // why does Android studio have a spell checker - // webcamdude is the webcam. I'm not naming it anything else - OpenCvWebcam webcamdude = null; - - @Override // This stuff happens when you click the init button - public void init() - { - WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); - // Acquire the camera ID - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName() ); - //set the cam name and id to the webcam. - webcamdude = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); - - //set webcam Pipeline - webcamdude.setPipeline(new WebcamPipelineNotes()); - - //You're creating an instance of the AsyncCameraOpenListener class. The class contains the two methods, onOpened and onError, which you are overriding with your code. That instance is passed to the openCameraDeviceAsync method as a parameter. - webcamdude.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - webcamdude.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); - } - - @Override - public void onError(int errorCode) { - telemetry.addLine("Webcam not working"); - } - }); - } - - @Override // This stuff happens when you click the play button - public void loop() - { - telemetry.addLine("Why did you click this lmao"); - } -} - +//package org.firstinspires.ftc.teamcode; +// +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.OpMode; +// +//import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +////import org.firstinspires.ftc.teamcode.VisionPipelines.WebcamPipelineNotes; +//import org.openftc.easyopencv.OpenCvCamera; +//import org.openftc.easyopencv.OpenCvCameraFactory; +//import org.openftc.easyopencv.OpenCvCameraRotation; +//import org.openftc.easyopencv.OpenCvWebcam; +// +//@Autonomous +//public class WebcamInitNotes extends OpMode +//{ +// +// // why does Android studio have a spell checker +// // webcamdude is the webcam. I'm not naming it anything else +// OpenCvWebcam webcamdude = null; +// +// @Override // This stuff happens when you click the init button +// public void init() +// { +// WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); +// // Acquire the camera ID +// int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName() ); +// //set the cam name and id to the webcam. +// webcamdude = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); +// +// //set webcam Pipeline +// webcamdude.setPipeline(new WebcamPipelineNotes()); +// +// //You're creating an instance of the AsyncCameraOpenListener class. The class contains the two methods, onOpened and onError, which you are overriding with your code. That instance is passed to the openCameraDeviceAsync method as a parameter. +// webcamdude.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { +// @Override +// public void onOpened() { +// webcamdude.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); +// } +// +// @Override +// public void onError(int errorCode) { +// telemetry.addLine("Webcam not working"); +// } +// }); +// } +// +// @Override // This stuff happens when you click the play button +// public void loop() +// { +// telemetry.addLine("Why did you click this lmao"); +// } +//} +// diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 29e8dcd68635..342eb2534845 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -1,29 +1,55 @@ package org.firstinspires.ftc.teamcode.Modules; +// --- CONSTANTS & OTHER STUFF --- // +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.riptideUtil; + +// --- CAMERA --- // +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.openftc.easyopencv.OpenCvWebcam; + +// --- PORTALS & PROCESSORS & SIMILAR STUFF --- // +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; -//opencv for vision stuff -import org.opencv.calib3d.Calib3d; -import org.opencv.core.*; -import org.opencv.imgproc.Imgproc; -import org.openftc.apriltag.AprilTagDetection; -import org.openftc.apriltag.AprilTagDetectorJNI; -import org.openftc.easyopencv.OpenCvPipeline; +import android.annotation.SuppressLint; +import android.graphics.Color; + +import org.firstinspires.ftc.vision.opencv.Circle; + +// --- LISTS --- // import java.util.ArrayList; +import java.util.Arrays; import java.util.List; + +import android.util.Size; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.SortOrder; + +/* + Resources: + - https://github.com/FIRST-Tech-Challenge/FtcRobotController/blob/master/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java + - + */ + /* - * This is the pipeline which will be processing the cameras feed - * It should be able to: - * - detect and recognize april tags - * - detect and locate (if I will we able to) artifacts - * - get the current state of the ramp + * We have to have two classes because java does not support multiple inheritance + * + * This class is the hardware aspect of the camera, the camera class will be used for the pipeline (what processes the feed) * - * welp lets see how well I make this */ -// omg this is going to need so much cleaning up -public class Camera extends OpenCvPipeline { +public class Camera { /////////////////////////////////////////////// //// ///// @@ -31,71 +57,23 @@ public class Camera extends OpenCvPipeline { //// ///// ////////////////////////////////////////////// - // -------------- ARTIFACTS -------------- // - - /* - Purple lower: (0, 136.0, 120.4) - Purple upper: (255, 255, 255) - */ - public Scalar lower = new Scalar(0, 136.0, 120.4); - public Scalar upper = new Scalar(255, 255, 255); - - - private final Mat ycrcbmat = new Mat(); - private final Mat binaryMat = new Mat(); - private final Mat maskedInputMat = new Mat(); - private final Mat grayscaleMat = new Mat(); - - // Contour Vars - - List contours = new ArrayList<>(); - public double lowerContourThreshold = 0; - public double upperContourThreshold = 300; - public Scalar contourColors = new Scalar(0,255,0); - public double noiseSensitivity = 153; - public int contourSize = 1; - - private final Mat edgeDetectorFrame = new Mat(); - private int onlyContours = 1; - - public static double currentLargest = 0; - - Mat activeMat; - Mat emptyMat = Mat.zeros(edgeDetectorFrame.size(), CvType.CV_8UC3); - int index = 0; - double contourArea; - double largestArea = 0; - - // -------------- APRIL TAGS ------------- // - - private long nativeApriltagPtr; - private Mat grey = new Mat(); - private ArrayList detections = new ArrayList<>(); - - - private ArrayList detectionsUpdate = new ArrayList<>(); - private final Object detectionsUpdateSync = new Object(); - - Mat cameraMatrix; - - Scalar blue = new Scalar(7,197,235,255); - Scalar red = new Scalar(255,0,0,255); - Scalar green = new Scalar(0,255,0,255); - Scalar white = new Scalar(255,255,255,255); - - double fx; - double fy; - double cx; - double cy; - - // UNITS ARE METERS - double tagsize; - double tagsizeX; - double tagsizeY; - - private float decimation; - private boolean needToSetDecimation; - private final Object decimationSync = new Object(); + // this used to be called webcamdude, I'm looking at you Aaron (actually Aroon) + OpenCvWebcam webcam = null; + + AprilTagProcessor tag_processor; + ColorBlobLocatorProcessor blob_processor_purple; + ColorBlobLocatorProcessor blob_processor_green; + VisionPortal vision_portal; + ArrayList detections; + ArrayList> blobs = new ArrayList<>(); + String goalTag; + CameraName cameraname; + public enum processors_enabled { + NONE, + TAG, + COLOR, + ALL + } /////////////////////////////////////////////// //// ///// @@ -103,373 +81,327 @@ public class Camera extends OpenCvPipeline { //// ///// ////////////////////////////////////////////// - public Camera(double tagsize, double fx, double fy, double cx, double cy) { - this.tagsize = tagsize; - this.tagsizeX = tagsize; - this.tagsizeY = tagsize; - this.fx = fx; - this.fy = fy; - this.cx = cx; - this.cy = cy; + public Camera(HardwareMap hardwareMap) { + cameraname = hardwareMap.get(WebcamName.class, "Webcam 1"); + tag_processor = new AprilTagProcessor.Builder() + .setTagLibrary(riptideUtil.getLibrary()) + .setDrawAxes(true) + .setDrawCubeProjection(true) + .setDrawTagID(true) + .setDrawTagOutline(true) + .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + //.setCameraPose(cameraPosition, cameraOrientation) + .build(); + + blob_processor_purple = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1)) + .setDrawContours(true) + .setBoxFitColor(0) + .setCircleFitColor(Color.rgb(255, 0, 255)) + .setBlurSize(5) + .setDilateSize(15) + .setErodeSize(15) + .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) + .build(); + + blob_processor_green = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.ARTIFACT_GREEN) + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1)) + .setDrawContours(true) + .setBoxFitColor(0) + .setCircleFitColor(Color.rgb(0, 255, 0)) + .setBlurSize(5) + .setDilateSize(15) + .setErodeSize(15) + .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) + .build(); + + // can also be 640 and 488 + // there is also YUY2 + vision_portal = new VisionPortal.Builder() + .addProcessors(tag_processor, blob_processor_purple, blob_processor_green) + //.addProcessor(new CameraPipeline(0.047, 578.272, 578.272, 402.145, 221.506, hardwareMap.get(WebcamName.class, "Webcam 1"))) + .setCamera(cameraname) + .setCameraResolution(new Size(riptideUtil.CAMERA_WIDTH, riptideUtil.CAMERA_HEIGHT)) + .setStreamFormat(VisionPortal.StreamFormat.MJPEG) + //.enableCameraMonitoring(true) + .setAutoStopLiveView(true) + .build(); + } - constructMatrix(); + public void setGoalTag(String goalTag) { + this.goalTag = goalTag; + } - // Allocate a native context object. See the corresponding deletion in the finalizer - nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3); + public String getGoalTag() { + return goalTag; + } + public ArrayList getTagDetections() { + detections = tag_processor.getDetections(); + detections.removeIf(detection -> System.nanoTime() - detection.frameAcquisitionNanoTime > riptideUtil.DETECTION_TIMEOUT); + return detections; } - @Override - public Mat processFrame(Mat input) { + public ArrayList> getBlobDetections() { + /* + * Returns an ArrayList of ArrayLists with 5 elements describing the artifact + * They are (in order): + * - x location (on the camera screen) + * - y location (on the camera screen) + * - distance away from the camera (needs tuning) + * - contour area + * - circularity + */ + blobs.clear(); + List blobs_detected = blob_processor_purple.getBlobs(); + blobs_detected.addAll(blob_processor_green.getBlobs()); + + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + 250, + 9000, + blobs_detected + ); + + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY, + 0.1, + 1, + blobs_detected + ); - // Convert to greyscale - Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY); + ColorBlobLocatorProcessor.Util.sortByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + SortOrder.DESCENDING, + blobs_detected + ); - synchronized (decimationSync) - { - if(needToSetDecimation) - { - AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation); - needToSetDecimation = false; - } + for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { + // the circle which fits the artifacts + Circle circle_fit = blob.getCircle(); + double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) + / (circle_fit.getRadius() * 2 * riptideUtil.SENSOR_HEIGHT); + ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), distance, (double) blob.getContourArea(), blob.getCircularity())); + blobs.add(temp); } - // Run AprilTag - detections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy); +// if(blobs_detected.isEmpty()) {return new ArrayList<>();} +// Circle circle_fit = blobs_detected.get(0).getCircle(); +// blobs.add(new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY() /*blob.getCircularity(), distance*/))); + + return blobs; + } + + public boolean isGoalTag(AprilTagDetection detection) { + // For red alliance goal + return detection.id == 24; + } - synchronized (detectionsUpdateSync) - { - detectionsUpdate = detections; + public AprilTagDetection getGoalApriltag() { + detections = getTagDetections(); + for(AprilTagDetection detection : detections) { + if(isGoalTag(detection)/* detection.metadata.name.equals(goalTag) */) { + return detection; + } } + return null; + } - // For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using - // OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV. - for(AprilTagDetection detection : detections) - { - Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY); - drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix); - draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix); + public EditablePose2D getGoalApriltagLocation() { + detections = getTagDetections(); + for(AprilTagDetection detection : detections) { + if (!detection.metadata.name.contains("Obelisk")) { + return new EditablePose2D( + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, 0, + DistanceUnit.INCH); + } } + return null; + } + + public double getAprilTagDistance(AprilTagDetection tag) { + double x = tag.robotPose.getPosition().x; + double y = tag.robotPose.getPosition().y; + double z = tag.robotPose.getPosition().z; + return Math.sqrt(x * x + y * y + z * z); + } - //Takes our "input" mat as an input, and outputs to a separate Mat buffer "ycrcbMat" - Imgproc.cvtColor(input, ycrcbmat, Imgproc.COLOR_RGB2YCrCb); - //Order is source, lowerBound, upperbound, dst. - Core.inRange(ycrcbmat, lower, upper, binaryMat); + public double getTagHorizontalAngle(AprilTagDetection tag) { /* - * Release the reusable Mat so that old data doesn't - * affect the next step in the current processing + * Gets the angle of the april tag relative to the camera. */ - maskedInputMat.release(); - //Order: src1, src2, dst, mask. - Core.bitwise_and(input, input, maskedInputMat, binaryMat); + // essentially setting the origin to the center of the screen + double delta_x = (double) riptideUtil.CAMERA_WIDTH / 2 - tag.center.x; - // now the masked input mat is the filtered image with colors and shit. - // Sorry Alok I ripped u off ;P + // relative to the camera + double horizontal_angle = delta_x * riptideUtil.CAM_FOV / riptideUtil.CAMERA_WIDTH; - // filter maskedinputmat to grey. - Imgproc.cvtColor(maskedInputMat, grayscaleMat, Imgproc.COLOR_RGB2GRAY); + //the angle of the april tag relative to the camera + return horizontal_angle; + } - // Order: input image, output edges(Array), lowerthreshold, upperthreshold - Imgproc.Canny(grayscaleMat, edgeDetectorFrame, lowerContourThreshold, upperContourThreshold); + public EditablePose2D findNearestArtifact() { + return nearestArtifact(0); + } - contours.clear(); - //Order : input image, the contours from output, hierarchy, mode, and method + public EditablePose2D findNearestArtifact(double turret_angle) { + return nearestArtifact(turret_angle); + } - Imgproc.findContours(edgeDetectorFrame, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + private EditablePose2D nearestArtifact(double turret_angle) { + /* + * This is just so the code looks nice and if there is no turret_angle, it is: + * findNearestArtifact(); + * instead of: + * findNearestArtifact(0); + * This is litterly a minor, insignificant, useless detail and Aaron will probably yell + * at me for this and I will probably change this in the future + * Future Aaron, I predicted your response, and I chose to do this useless thing. + * hahhahahahahhhahahahahahhahahahahahhahahahahahhashashahahahahahahahahahahahahah + * ahahahahahahahahahahahahahahahhahahahahahahahahahhahahahahahahahahahahahahahahah + * ahahhahahahahahhahahhhahahahahahahah + */ + if(blobs.isEmpty()) {return null;} + ArrayList largest_contour = new ArrayList<>(blobs.get(0)); - MatOfPoint2f[] contoursPoly = new MatOfPoint2f[contours.size()]; + double distance = largest_contour.get(2); - Rect[] boundRect = new Rect[contours.size()]; - //Might be useful later, idk so I am going to leave this here - //Point[] centers = new Point[contours.size()]; + // essentially setting the origin to the center of the screen + double delta_x = (double) riptideUtil.CAMERA_WIDTH / 2 - largest_contour.get(0); + double delta_y = (double) riptideUtil.CAMERA_HEIGHT / 2 - largest_contour.get(1); + // relative to the camera + double horizontal_distance = delta_x * riptideUtil.CAM_FOV / riptideUtil.CAMERA_WIDTH; + double vertical_distance = delta_y * riptideUtil.CAM_FOV / riptideUtil.CAMERA_HEIGHT; - for (int i = 0; i < contours.size(); i++) { - contoursPoly[i] = new MatOfPoint2f(); - Imgproc.approxPolyDP(new MatOfPoint2f(contours.get(i).toArray()), contoursPoly[i], 3, true); - boundRect[i] = Imgproc.boundingRect(contours.get(i)); - //centers[i] = new Point(); - } + double horizontal_angle_error = Math.atan(horizontal_distance/distance); + double vertical_angle_error = Math.atan(vertical_distance/distance); + // difference between the errors and the camera angle, so this is relative to what ever + // the angle of the camera is relative to + double horizontal_angle = turret_angle - horizontal_angle_error; + double vertical_angle = riptideUtil.CAMERA_ANGLE - vertical_angle_error; - List contoursPolyList = new ArrayList<>(contoursPoly.length); - for (MatOfPoint2f poly : contoursPoly) { - //Just to make sure that no problems really come up. - if(poly == null) - { - break; - } + // relative to whatever the turret_angle is + // 0 degrees means relative to the camera or robot (if they are looking in the same direction) + // anything else would imply relative to the robot + double horizontal_dist = Math.sin(horizontal_angle_error) * distance; - contoursPolyList.add(new MatOfPoint(poly.toArray())); - } + // relative to the ground + double height = Math.sin(vertical_angle) * distance; + return new EditablePose2D( + horizontal_dist, + height, + 0, + DistanceUnit.INCH + ); + } - // now this can be removed during the meet. - if (onlyContours == 1) - { - activeMat = maskedInputMat; - } - else if (onlyContours == 2) - { - activeMat = emptyMat; - } - else { - activeMat = input; + // the default is ALL + public void setPipeline(processors_enabled processors) { + switch (processors) { + case TAG: + vision_portal.setProcessorEnabled(tag_processor, true); + vision_portal.setProcessorEnabled(blob_processor_purple, false); + vision_portal.setProcessorEnabled(blob_processor_green, false); + break; + case COLOR: + vision_portal.setProcessorEnabled(tag_processor, false); + vision_portal.setProcessorEnabled(blob_processor_purple, true); + vision_portal.setProcessorEnabled(blob_processor_green, true); + break; + case NONE: + vision_portal.setProcessorEnabled(tag_processor, false); + vision_portal.setProcessorEnabled(blob_processor_purple, false); + vision_portal.setProcessorEnabled(blob_processor_green, false); + break; + default: + vision_portal.setProcessorEnabled(tag_processor, true); + vision_portal.setProcessorEnabled(blob_processor_purple, true); + vision_portal.setProcessorEnabled(blob_processor_green, true); } + } - largestArea = 0; - - for (int i = 0; i < contours.size(); i++) { + public void stopStreaming() { + vision_portal.stopStreaming(); + } - MatOfPoint contour = contours.get(i); + public void resumeStreaming() { + vision_portal.resumeStreaming(); + } - contourArea = Imgproc.contourArea(contour); - // A simple filter I cam up with ;P - if (contourArea < noiseSensitivity) - { - continue; - } + public void stop() { + vision_portal.close(); + } - if (contourArea > largestArea) - { - largestArea = contourArea; - index = i; + @SuppressLint("DefaultLocale") + public void distanceFromGoal(double x, double y, double z){ + + //telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); + for (AprilTagDetection detection : detections) { + if (detection.metadata != null) { + //telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); + if (!detection.metadata.name.contains("Obelisk")) { + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, + detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("Distance %f (inch)", + getAprilTagDistance(detection))); + } + } else { + telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); } - } - currentLargest = largestArea; - - - // pretty sure that we actually don't need this for the meet either, - // but is good for demonstration purposes and debugging - if (contours.size() != 0 && largestArea != 0) - { - // Adding Text3 - Imgproc.putText ( - activeMat, // Matrix obj of the image - largestArea + "", // Text to be added - new Point(boundRect[index].tl().x, boundRect[index].tl().y), // point - Imgproc.FONT_HERSHEY_SIMPLEX , // front face - 1, // front scale - contourColors, // Scalar object for color - 2 // Thickness - ); - Imgproc.drawContours(activeMat, contoursPolyList, index, contourColors, contourSize); - Imgproc.rectangle(activeMat, boundRect[index].tl(), boundRect[index].br(), contourColors, 2); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + x = detection.robotPose.getPosition().x; + y = detection.robotPose.getPosition().y; + z = detection.robotPose.getPosition().z; } - return activeMat; - } - - // I think we could have just returned a Mat frame and have the FSM and Auton do the computing, but that is like - // not smart - public static double getLargestSize() - { - return currentLargest; - } - - // boilerplate code if there is a better way to do this, please tell me - @Override - public void onViewportTapped() - { - if (onlyContours == 1 || onlyContours == 2) - onlyContours++; - else - onlyContours = 1; - } + telemetry.addLine(String.format(" --- %d Artifacts Detected --- ", blobs.size())); - @Override - public void finalize() - { - // Might be null if createApriltagDetector() threw an exception - if(nativeApriltagPtr != 0) - { - // Delete the native context we created in the constructor - AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr); - nativeApriltagPtr = 0; - } - else - { - System.out.println("AprilTagDetectionPipeline.finalize(): nativeApriltagPtr was NULL"); + for (List blob : blobs) { + telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); + telemetry.addLine(String.format("Circularity: %f", blob.get(4))); + telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); + telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(2))); } } - public void setDecimation(float decimation) - { - synchronized (decimationSync) - { - this.decimation = decimation; - needToSetDecimation = true; - } - } + public void tagAngle(double angle){ - public ArrayList getLatestDetections() - { - return detections; } - public ArrayList getDetectionsUpdate() - { - synchronized (detectionsUpdateSync) - { - ArrayList ret = detectionsUpdate; - detectionsUpdate = null; - return ret; + public Double getGoalDistance() { + AprilTagDetection goalDetection = getGoalApriltag(); + if (goalDetection == null) { + return null; } - } - void constructMatrix() - { - // Construct the camera matrix. - // - // -- -- - // | fx 0 cx | - // | 0 fy cy | - // | 0 0 1 | - // -- -- - // - - cameraMatrix = new Mat(3,3, CvType.CV_32FC1); - - cameraMatrix.put(0,0, fx); - cameraMatrix.put(0,1,0); - cameraMatrix.put(0,2, cx); - - cameraMatrix.put(1,0,0); - cameraMatrix.put(1,1,fy); - cameraMatrix.put(1,2,cy); - - cameraMatrix.put(2, 0, 0); - cameraMatrix.put(2,1,0); - cameraMatrix.put(2,2,1); - } - - /** - * Draw a 3D axis marker on a detection. (Similar to what Vuforia does) - * - * @param buf the RGB buffer on which to draw the marker - * @param length the length of each of the marker 'poles' - * @param rvec the rotation vector of the detection - * @param tvec the translation vector of the detection - * @param cameraMatrix the camera matrix used when finding the detection - */ - void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) - { - // The points in 3D space we wish to project onto the 2D image plane. - // The origin of the coordinate space is assumed to be in the center of the detection. - MatOfPoint3f axis = new MatOfPoint3f( - new Point3(0,0,0), - new Point3(length,0,0), - new Point3(0,length,0), - new Point3(0,0,-length) - ); + double x = goalDetection.robotPose.getPosition().x; + double y = goalDetection.robotPose.getPosition().y; + double z = goalDetection.robotPose.getPosition().z; - // Project those points - MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); - Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); - Point[] projectedPoints = matProjectedPoints.toArray(); - // Draw the marker! - Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness); - Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness); - Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness); - Imgproc.circle(buf, projectedPoints[0], thickness, white, -1); + return Math.sqrt(x * x + y * y + z * z); } - void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) - { - //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0], - // [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ]) - - // The points in 3D space we wish to project onto the 2D image plane. - // The origin of the coordinate space is assumed to be in the center of the detection. - MatOfPoint3f axis = new MatOfPoint3f( - new Point3(-tagWidth/2, tagHeight/2,0), - new Point3( tagWidth/2, tagHeight/2,0), - new Point3( tagWidth/2,-tagHeight/2,0), - new Point3(-tagWidth/2,-tagHeight/2,0), - new Point3(-tagWidth/2, tagHeight/2,-length), - new Point3( tagWidth/2, tagHeight/2,-length), - new Point3( tagWidth/2,-tagHeight/2,-length), - new Point3(-tagWidth/2,-tagHeight/2,-length)); - - // Project those points - MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); - Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); - Point[] projectedPoints = matProjectedPoints.toArray(); - - // Pillars - for(int i = 0; i < 4; i++) - { - Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness); + public Double getGoalAngleError() { + AprilTagDetection goalDetection = getGoalApriltag(); + if (goalDetection == null) { + return null; } - // Base lines - //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness); - //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness); - //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness); - //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness); - - // Top lines - Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness); - Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness); - Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness); - Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness); + return getTagHorizontalAngle(goalDetection); } - - /** - * Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the - * original size of the tag. - * - * @param points the points which form the trapezoid - * @param cameraMatrix the camera intrinsics matrix - * @param tagsizeX the original width of the tag - * @param tagsizeY the original height of the tag - * @return the 6DOF pose of the camera relative to the tag - */ - Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY) - { - // The actual 2d points of the tag detected in the image - MatOfPoint2f points2d = new MatOfPoint2f(points); - - // The 3d points of the tag in an 'ideal projection' - Point3[] arrayPoints3d = new Point3[4]; - arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0); - arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0); - arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0); - arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0); - MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d); - - // Using this information, actually solve for pose - Pose pose = new Pose(); - Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false); - - return pose; - } - - /* - * A simple container to hold both rotation and translation - * vectors, which together form a 6DOF pose. - */ - class Pose - { - Mat rvec; - Mat tvec; - - public Pose() - { - rvec = new Mat(); - tvec = new Mat(); - } - - public Pose(Mat rvec, Mat tvec) - { - this.rvec = rvec; - this.tvec = tvec; - } - } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraHardware.java deleted file mode 100644 index 9248622829a7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraHardware.java +++ /dev/null @@ -1,77 +0,0 @@ -package org.firstinspires.ftc.teamcode.Modules; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.HardwareMap; - -// vision stuff -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvWebcam; - -import org.firstinspires.ftc.teamcode.Modules.Camera; - -/* - * We have to have two classes because java does not support multiple inheritance - * - * This class is the hardware aspect of the camera, the camera class will be used for the pipeline (what processes the feed) - * - */ -@Autonomous -public class CameraHardware { - - /////////////////////////////////////////////// - //// ///// - //// VARIABLES ///// - //// ///// - ////////////////////////////////////////////// - - // this used to be called webcamdude, I'm looking at you Aaron - OpenCvWebcam webcam = null; - - /////////////////////////////////////////////// - //// ///// - //// FUNCTIONS ///// - //// ///// - ////////////////////////////////////////////// - - public CameraHardware(HardwareMap hardwareMap) { - - // get webcam name - WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); - - // get camera ID - // this is to display the camera feed on the robot controller screen - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - - // set webcam to the camera name and id - // can omit cameraMonitorViewId if you don't want to display the live feed - webcam = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); - - // set webcam pipeline - webcam.setPipeline(new Camera(0.047, 578.272, 578.272, 402.145, 221.506)); - - // this is the asyncrounous camera open listener class - // it has the methods : onOpened and onError - // we are overriding these methods - webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - // starts the webcam when click play (?) - @Override - public void onOpened() { - webcam.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); -// telemetry.addLine("Webcam started streaming..."); - } - - // called if the camera could not be opened - @Override - public void onError(int errorCode) { -// telemetry.addLine("Welp you screwed up."); -// telemetry.addLine("Or maybe I\'m stupid."); -// telemetry.addLine("Or maybe there\'s just an error with the camera."); - - } - }); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index 7049813f49f9..910d17643a36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -14,8 +14,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.Modules.EditablePose2D; -import org.firstinspires.ftc.teamcode.Modules.OdometryLocalizer; + // ----- READY TO TRANSFER ----- // @@ -49,10 +48,10 @@ public Drivetrain(HardwareMap hardwareMap) { brWheel = hardwareMap.dcMotor.get("brWheel"); blWheel = hardwareMap.dcMotor.get("blWheel"); - blWheel.setDirection(DcMotorSimple.Direction.REVERSE); + blWheel.setDirection(DcMotorSimple.Direction.FORWARD); flWheel.setDirection(DcMotorSimple.Direction.REVERSE); frWheel.setDirection(DcMotorSimple.Direction.FORWARD); - brWheel.setDirection(DcMotorSimple.Direction.FORWARD); + brWheel.setDirection(DcMotorSimple.Direction.REVERSE); brWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); blWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -72,8 +71,8 @@ public Drivetrain(HardwareMap hardwareMap) { imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); imu.resetYaw(); imu.initialize(parameters); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Outtake.java new file mode 100644 index 000000000000..2eb55de3d4fb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Outtake.java @@ -0,0 +1,221 @@ +package org.firstinspires.ftc.teamcode.Modules; + +import static org.firstinspires.ftc.teamcode.riptideUtil.KPBottom; +import static org.firstinspires.ftc.teamcode.riptideUtil.KPTop; +import static org.firstinspires.ftc.teamcode.riptideUtil.TOP_FLYWHEEL_KP; +import static org.firstinspires.ftc.teamcode.riptideUtil.BOTTOM_FLYWHEEL_KP; +import static org.firstinspires.ftc.teamcode.riptideUtil.angularVelocity; +import static org.firstinspires.ftc.teamcode.riptideUtil.econserved; +import static org.firstinspires.ftc.teamcode.riptideUtil.tolerance; + +import android.annotation.SuppressLint; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.util.LinkedList; + +public class Outtake { +// private final DcMotor topFlywheel; +// private final DcMotor bottomFlywheel; + + private final PIDController flywheelVelocityControllerBottom = new PIDController(TOP_FLYWHEEL_KP, 0, 0); + private final PIDController flywheelVelocityControllerTop = new PIDController(BOTTOM_FLYWHEEL_KP, 0, 0); + + private boolean updatePID = false; + + + + // OUTTAKE TUNER THINGS + LinkedList topRecords = new LinkedList<>(); + LinkedList bottomRecords = new LinkedList<>(); + public static int queueSize = 3; + private static double rpmTopPrev = 360; + private static double rpmBottomPrev = 360; + + private double currPosTop; + private double currPosBottom; + private double startTime = System.nanoTime() / 1e9; + private PIDController RPMControllerTop = new PIDController(KPTop, 0, 0); + private PIDController RPMControllerBottom = new PIDController(KPBottom, 0, 0); + private double prevPosTop = 0; + private double prevPosBottom = 0; + public void runOuttakePID(double rpmTop, double rpmBottom, Telemetry tele){ + + pidtunedmotor(rpmTop, rpmBottom, tele); + + tele.addData("goalRPMTop", rpmTop); + tele.addData("goalRPMBottom", rpmBottom); + tele.update(); + if (rpmTopPrev != rpmTop) { + rpmTopPrev = rpmTop; + RPMControllerTop = new PIDController(KPTop, 0, 0); + + } + if (rpmBottomPrev != rpmBottom) { + rpmBottomPrev = rpmBottom; + RPMControllerBottom = new PIDController(KPBottom, 0, 0); + } + tele.update(); + } + + private boolean atGoalSpeed = false; + + + public void pidtunedmotor(double rpmTop, double rpmBottom, Telemetry telemetry) { + + prevPosTop = currPosTop; + prevPosBottom = currPosBottom; + +// currPosTop = currPosL(); +// currPosBottom = currPosR(); + + double dThetaTop = (currPosTop - prevPosTop)/28; + double dThetaBottom = (currPosBottom - prevPosBottom)/28; + + double dt = System.nanoTime() / 1e9 - startTime; + startTime = System.nanoTime() / 1e9; + + double currRPMTop = dThetaTop / (dt / 60); + double currRPMBottom = dThetaBottom / (dt / 60); + + topRecords.add(currRPMTop); + while (topRecords.size() > queueSize) + topRecords.remove(0); + + bottomRecords.add(currRPMBottom); + while (bottomRecords.size() > queueSize) + bottomRecords.remove(0); + + double undividedAverageBottom = 0; + double undividedAverageTop = 0; + + for (int i = 0; i < topRecords.size(); i++) { + undividedAverageTop += topRecords.get(i); + undividedAverageBottom += bottomRecords.get(i); + } + double averageTop; + double averageBottom; + if (topRecords.size() == queueSize) { + averageTop = undividedAverageTop / queueSize; + averageBottom = undividedAverageBottom / queueSize; + } else { + averageTop = currRPMTop; + averageBottom = currRPMBottom; + } + + //double averageTop = topRecords.size() >= queueSize ? (topRecords.get(0)+topRecords.get(1)+topRecords.get(2)+topRecords.get(3)+topRecords.get(4))/5 : currRPMTop; + //double averageBottom = bottomRecords.size() >= queueSize ? (bottomRecords.get(0)+bottomRecords.get(1)+bottomRecords.get(2)+bottomRecords.get(3)+bottomRecords.get(4))/5 : currRPMBottom; + + double wantedWheelPowerTopAverage = RPMControllerTop.calculate(averageTop - 200, rpmTop); + double wantedWheelPowerBottomAverage = RPMControllerBottom.calculate(averageBottom - 200, rpmBottom); + + + //setFlyWheelPower(rpmTop != 0 ? wantedWheelPowerTopAverage:0,rpmBottom != 0 ? wantedWheelPowerBottomAverage:0); + + telemetry.addData("ready", bottomRecords.size() >= queueSize); + telemetry.addData("top", averageTop); + telemetry.addData("bottom", averageBottom); + + + boolean atTopRPM = Math.abs(averageTop - rpmTop) <= tolerance; + boolean atBotRPM = Math.abs(averageBottom - rpmBottom) <= tolerance; + atGoalSpeed = atTopRPM && atBotRPM; + } + + public Outtake(HardwareMap hardwareMap){ + +// topFlywheel = hardwareMap.dcMotor.get("topFlywheel"); +// bottomFlywheel = hardwareMap.dcMotor.get("bottomFlywheel"); + +// topFlywheel.setDirection(DcMotorSimple.Direction.REVERSE); +// bottomFlywheel.setDirection(DcMotorSimple.Direction.REVERSE); + } + +// public void start(double speed){ +//// bottomFlywheel.setPower(speed); +//// topFlywheel.setPower(speed); +// } +//// +// //This one method is only to set motors individually +// public void setFlyWheelPower(double speedL, double speedR) { +// bottomFlywheel.setPower(speedR); +// topFlywheel.setPower(speedL); +// +// } +// +// public void stop(){ +// bottomFlywheel.setPower(0); +// topFlywheel.setPower(0); +// } + + + public boolean isAtGoalSpeed(){ + return atGoalSpeed; + } + + + public void startFlywheel(){ + this.startTime = System.nanoTime() / 1e9; // Current Time in Seconds + } + +// public double currPos(){ +// double currPos = (double) (bottomFlywheel.getCurrentPosition() + topFlywheel.getCurrentPosition()) / 2; +// return currPos; +// } +// +// //Created to allow OuttakePIDTuner to access individual positions +// public double currPosL(){ +// return topFlywheel.getCurrentPosition(); +// } +// public double currPosR(){ +// return bottomFlywheel.getCurrentPosition(); +// } + + + @SuppressLint("DefaultLocale") + public void setPowerOnDist(Double dist /* INCHES */, boolean run, Telemetry tele){ + + + double rBottom = 1.41732; // INCHES + double rTop = 1.41732; // INCHES + double angle = Math.toRadians(75); + double y = 25 - rBottom; + + double term = dist * Math.tan(angle) - y; + if (term <= 0) { + tele.addLine("Invalid term: target too close or below launch point"); + tele.addData("term", term); + pidtunedmotor(0, 0, tele); + return; + } + + double numerator = 9.8 * 39.3701 * dist * dist; + double denominator = 2 * Math.pow(Math.cos(angle), 2) * term; + + double ballVelocity = Math.sqrt(numerator / denominator); + + double rpmTop; + double rpmBottom; + + if (run) { + rpmTop = ((ballVelocity * 60) / (2 * Math.PI * rTop)) * econserved - (angularVelocity * 30 / Math.PI) * econserved; + rpmBottom = ((ballVelocity * 60) / (2 * Math.PI * rBottom)) * econserved - (angularVelocity * 30 / Math.PI) * econserved; + } + + else {rpmTop = 0; rpmBottom = 0;} + pidtunedmotor(rpmTop, rpmBottom, tele); +// tele.addLine(String.format("Calculated rpm top: %f", rpmTop)); +// tele.addLine(String.format("Calculated rpm bottom: %f", rpmBottom)); + tele.update(); + } + + private double rpmTopGoal; + private double rpmBottomGoal; + + public void setOuttakeRPM (double rpmTop, double rpmBottom){rpmTopGoal = rpmTop; rpmBottomGoal = rpmBottom;} + public void runOuttakePID(Telemetry tele){pidtunedmotor(rpmTopGoal, rpmBottomGoal, tele);} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java new file mode 100644 index 000000000000..ad41034db306 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.Modules; + +import org.firstinspires.ftc.teamcode.riptideUtil; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.ArrayList; + + +public class TurnTable { + //Servo turntable_servo; + //DcMotor turntable_motor; + PIDController tablePID; + + double tableKP = 0, tableKI = 0, tableKD = 0; // needs tuning + + // This is the name of the tag which the turn table will be following + // the goal_tag can be either "Red Goal" or "Blue Goal" + //String goal_tag; + + public TurnTable(double kp, double ki, double kd/*HardwareMap hardwareMap, String goal_tag*/) { + //this.camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); + //this.turntable_servo = hardwareMap.servo.get("turntables"); // turn table servo + //this.turntable_motor = hardwareMap.dcMotor.get("turntablem"); // turn table motor + tableKP = kp; + tableKI = ki; + tableKD = kd; + this.tablePID = new PIDController(tableKP, tableKI, tableKD); // gotta tune + //this.goal_tag = goal_tag; + + // this is to try to stop oscillation, if the difference in angle < this value + // the pid will return 0 + tablePID.setDeadZone(2); + } + + public void setPID(double kp, double ki, double kd) { + tableKP = kp; + tableKI = ki; + tableKD = kd; + this.tablePID = new PIDController(tableKP, tableKI, tableKD); + } + +// public void setPosition(double pos) { +// /* +// * This would be relative to the robot, +// * I don't know if we are using a servo or a motor so the +// * code might be a bit different +// */ +// // for the servo +// turntable_servo.setPosition(tablePID.calculate(turntable_servo.getPosition(), pos)); +// +// // for the motor +// turntable_motor.setPower(tablePID.calculate(0, pos)); +// } + +// public void followTag() { +// /* +// * This is going to be constantly called to update the servo position or +// * motor power. It looks for the april tag and finds the position needed +// * so that the turn table faces the tag. +// */ +// ArrayList detections = camera.getTagDetections(); +// +// for (AprilTagDetection detection : detections) { +// if (detection.metadata.name.equals(goal_tag)) { +// double horiz_angle = camera.getTagHorizontalAngle(detection); +// setPosition(horiz_angle * riptideUtil.MOTOR_POS_CONST * 2 / riptideUtil.CAMERA_WIDTH); +// } +// } +// } + + public double alignTag(double goalAngle) { + return tablePID.calculate(0, goalAngle); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 045d73cc46ce..0ab70c074cd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -2,8 +2,10 @@ import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.teamcode.Modules.Drivetrain; -import org.firstinspires.ftc.teamcode.Modules.Slides; +import org.firstinspires.ftc.teamcode.Modules.Outtake; public class Robot { @@ -11,14 +13,32 @@ public class Robot { Drivetrain drivetrain; + Outtake outtake; + + Camera camera; + public Robot (HardwareMap hardwareMap){ this.hardwareMap = hardwareMap; drivetrain = new Drivetrain(hardwareMap); + + outtake = new Outtake(hardwareMap); + + camera = new Camera(hardwareMap); } public Drivetrain getDrivetrain(){ return drivetrain; } -} + public Outtake getOuttake() {return outtake;} + + public Camera getCamera() {return camera;} + + public void setFlyWheelPowerOnDistance(boolean run, Telemetry tele){ + Double distance = camera.getGoalDistance(); + if (distance != null) { + outtake.setPowerOnDist(distance, run, tele); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 0f2b893aca2f..3bc2c31d7ff8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -5,20 +5,22 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.Modules.PIDController; import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.ArrayList; -@Config @TeleOp(name = "Meet 0 FSM") public class Meet0FSM extends LinearOpMode { Robot robot; boolean hasrun = false; - - - - + public static boolean align = false; + public static double kp = 0, ki = 0, kd = 0; + String goalTag = "Red Goal"; public enum states{ IDLE, @@ -30,6 +32,7 @@ public enum states{ @Override public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); hasrun = false; telemetry.addData("Robot status:", "succesfully initiated"); @@ -43,9 +46,11 @@ public void runOpMode() throws InterruptedException { telemetry.update(); while(opModeIsActive()){ + FSM(); + fieldCentricDrive(); telemetry.addData("Current State:", currentState); + telemetry.addData("Robot angle", robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES)); telemetry.update(); - fieldCentricDrive(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java new file mode 100644 index 000000000000..b757b1e5ac43 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java @@ -0,0 +1,115 @@ +package org.firstinspires.ftc.teamcode.Teleop; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +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.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.Modules.PIDController; +import org.firstinspires.ftc.teamcode.Robot; + + +@Config +@TeleOp(name = "Angle PID Tuner", group = "Tuning") +public class AnglePIDTuner extends LinearOpMode { + + Robot robot; + PIDController anglePID; + private double initHeading = 0; + + public static boolean align = false; + boolean alignDebounce = false; + public static double kp = 0, ki = 0, kd = 0; + + public static double goal = 0; + private double prevGoal = 0; + public static double error = 0; + private Telemetry t = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + boolean prevAlign = false; + + anglePID = new PIDController(kp, ki, kd); + + waitForStart(); + if (isStopRequested()) return; + + while(opModeIsActive()) { + if (gamepad1.x && !alignDebounce) { + align = true; + alignDebounce = true; + } + + if(align && !alignDebounce) { + initHeading = currHeading() % 360; + } + + if (gamepad1.triangle) { + align = false; + alignDebounce = false; + } + + if(gamepad1.circle) { + initHeading = currHeading() % 360; + } + + if(prevAlign != align) { + anglePID.setPID(kp, ki, kd); + robot.getDrivetrain().resetImu(); + } + + if(prevGoal != goal) { + initHeading = currHeading(); + anglePID.setPID(kp, ki, kd); + } + + fieldCentricDrive(); + prevAlign = align; + prevGoal = goal; + } + } + + public double currHeading() { + return robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); + } + + private void fieldCentricDrive() { + double slowdown = gamepad1.right_trigger > 0 ? 0.25 : 1; + double y = -gamepad1.left_stick_y * slowdown; + double x = gamepad1.left_stick_x * 1.1 * slowdown; + + double currHeading = currHeading(); + double goalHeading = initHeading*2 - currHeading(); + error = goalHeading - currHeading; + if(Math.abs(error) > 180) { error = (error + -Math.signum(error) * 180); } + double alignVal = anglePID.calculate(0, error); + if(alignVal == 0 || Math.abs(error) <= 2) { align = false; alignDebounce = false;} + t.addData("Current Heading", currHeading); + t.addData("Initial Heading", initHeading); + t.addData("Turn Power", alignVal); + t.addData("Error", error); + t.update(); + + double rx = align ? alignVal : gamepad1.right_stick_x * slowdown; + + double rotX = x * Math.cos(-currHeading) - y * Math.sin(-currHeading); + double rotY = x * Math.sin(-currHeading) + y * Math.cos(-currHeading); + + double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double frWheelPower = (rotY - rotX - rx) / denominator; + double flWheelPower = (rotY + rotX + rx) / denominator; + double brWheelPower = (rotY + rotX - rx) / denominator; + double blWheelPower = (rotY - rotX + rx) / denominator; + + robot.getDrivetrain().setWheelPowers(flWheelPower, frWheelPower, brWheelPower, blWheelPower); + + if (gamepad1.y) { + robot.getDrivetrain().resetImu(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java index 2c9163c99b17..3beb208d464f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java @@ -49,9 +49,9 @@ public void runOpMode() throws InterruptedException { } slideController.setPID(kp, ki, kd); - currentPosition = robot.getSlides().getSlidePosition(); +// currentPosition = robot.getSlides().getSlidePosition(); - robot.getSlides().setSlidePower(slideController.calculate(currentPosition, goal) + kf); + //robot.getSlides().setSlidePower(slideController.calculate(currentPosition, goal) + kf); t.addData("Current Position", currentPosition); t.addData("Goal Position", goal); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java new file mode 100644 index 000000000000..454532754ce1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -0,0 +1,112 @@ +package org.firstinspires.ftc.teamcode.Tuning; + +import android.annotation.SuppressLint; + +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.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.Modules.Camera; +import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.ArrayList; +import java.util.List; + +@TeleOp(name="Test Vision") +public class TestVision extends LinearOpMode { + Robot robot; + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + + Camera.processors_enabled processor = Camera.processors_enabled.ALL; + robot.getCamera().setPipeline(processor); + boolean a_debounce = false; + + telemetry.setMsTransmissionInterval(100); // speed up telemetry updates for debugging + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + waitForStart(); + while (!isStopRequested() && opModeIsActive()) { + // this will switch the viewed pipeline on the driver hub + // in this order: ALL -> TAG -> COLOR + if (gamepad1.a) { + if(gamepad1.a ^ a_debounce) { + switch (processor) { + case ALL: + processor = Camera.processors_enabled.TAG; + robot.getCamera().setPipeline(processor); + break; + case TAG: + processor = Camera.processors_enabled.COLOR; + robot.getCamera().setPipeline(processor); + break; + case COLOR: + processor = Camera.processors_enabled.ALL; + robot.getCamera().setPipeline(processor); + break; + } + } + + + + a_debounce = true; + } else { + a_debounce = false; + } + telemetryStuff(robot.getCamera()); + robot.setFlyWheelPowerOnDistance(true, telemetry); + } + + robot.getCamera().stop(); + } + + @SuppressLint("DefaultLocale") + public void telemetryStuff(Camera camera) { + List detections = camera.getTagDetections(); + ArrayList> color_blobs = camera.getBlobDetections(); + + //telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); + + for (AprilTagDetection detection : detections) { + if (detection.metadata != null) { + // telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); + if (!detection.metadata.name.contains("Obelisk")) { +// telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", +// detection.robotPose.getPosition().x, +// detection.robotPose.getPosition().y, +// detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("Distance %f (inch)", + camera.getAprilTagDistance(detection))); + telemetry.addLine(String.format("Goal Angle Error %f (Degrees)", + camera.getGoalAngleError())); +// telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", +// detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), +// detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), +// detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } + } else { + // telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); + } + // telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + + } + +// telemetry.addLine(String.format(" --- %d Artifacts Detected --- ", color_blobs.size())); +// +// for (List blob : color_blobs) { +// telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); +// telemetry.addLine(String.format("Circularity: %f", blob.get(4))); +// telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); +// telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(2))); +// } + + telemetry.update(); + sleep(100); + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/PowerOnDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/PowerOnDistance.java new file mode 100644 index 000000000000..c4abb085ec49 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/PowerOnDistance.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.UnitTests; + +import static org.firstinspires.ftc.teamcode.riptideUtil.angularVelocity; +import static org.firstinspires.ftc.teamcode.riptideUtil.econserved; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +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.teamcode.Modules.Camera; +import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +@Config +@TeleOp(name = "Power on Distance") +public class PowerOnDistance extends LinearOpMode { + Robot robot; + Telemetry tele = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + @Override + public void runOpMode(){ + robot = new Robot(hardwareMap); + + Camera.processors_enabled processor = Camera.processors_enabled.ALL; + robot.getCamera().setPipeline(processor); + + + waitForStart(); + while(opModeIsActive()){ + robot.setFlyWheelPowerOnDistance(true, tele); + tele.update(); + + for (AprilTagDetection d : robot.getCamera().getTagDetections()) { + telemetry.addLine("Detected tag name: " + d.metadata.name + " id: " + d.id); + } + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index 5708f0ba7af3..7a19ad542f05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -2,6 +2,9 @@ import com.acmerobotics.dashboard.config.Config; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; + @Config public class riptideUtil { @@ -12,7 +15,26 @@ public class riptideUtil { */ /** General constants */ + // COLOR SENSING + // Green + public static final float GREEN_R = 0.136f; + public static final float GREEN_R_STDEV = 0.05f; + public static final float GREEN_G = 0.578f; + public static final float GREEN_G_STDEV = 0.15f; + public static final float GREEN_B = 0.404f; + public static final float GREEN_B_STDEV = 0.13f; + // Purple + public static final float PURPLE_R = 0.346f; + public static final float PURPLE_R_STDEV = 0.02f; + public static final float PURPLE_G = 0.376f; + public static final float PURPLE_G_STDEV = 0.03f; + public static final float PURPLE_B = 0.686f; + public static final float PURPLE_B_STDEV = 0.04f; + + //flywheel + public static double TOP_FLYWHEEL_KP = 0.001; + public static double BOTTOM_FLYWHEEL_KP = 0.001; /** Autonomous Constants */ public static final double POINT_TOLERANCE = 2; // UNDETERMINED @@ -39,4 +61,78 @@ public class riptideUtil { // Speed relationships public static double MAX_A = 72; // UNDETERMINED public static double MAX_V = 96; // UNDETERMINED -} + + public static double LONG_DIST_TOP = 3250; // UNDETERMINED + public static double LONG_DIST_BOT = 3730; // UNDETERMINED + + public static double MID_DIST_TOP = 2650; // UNDETERMINED + public static double MID_DIST_BOT = 3000;// UNDETERMINED + + public static double SHORT_DIST_TOP = 2950; // UNDETERMINED + public static double SHORT_DIST_BOT = 3480;// UNDETERMINED + + public static double transferOpen = 0.8; // UNDETERMINED + public static double transferClosed = -0.8; // UNDETERMINED + + public static double KPTop = 0.0032; //0.004 + public static double KPBottom = 0.004; // 0.0045 + + public static double tolerance = 50; + + + // Vision + public static int CAMERA_WIDTH = 640; + public static int CAMERA_HEIGHT = 480; + public static double CAMERA_ANGLE = 0; + + public static double DETECTION_TIMEOUT = 25e+7; + public static double ARTIFACT_SIZE_INCHES = 5; + + public static double LENS_FOCAL_LEN_INCHES = 0.15748; + public static double SENSOR_HEIGHT = 0.086; + public static double CAM_FOV = 55; + + public static double MOTOR_POS_CONST = 1; // needs tuning + + public static AprilTagLibrary getLibrary() { + return new AprilTagLibrary.Builder() + .addTag( + 20, + "Blue Goal", + 6.5, + //new VectorF(-0f, -0f, 0f), + DistanceUnit.INCH//, + //Quaternion.identityQuaternion() + ) + .addTag( + 21, + "Obelisk Green Purple Purple (GPP)", + 6.5, + DistanceUnit.INCH + ) + .addTag( + 22, + "Obelisk Purple Green Purple (PGP)", + 6.5, + DistanceUnit.INCH + ) + .addTag( + 23, + "Obelisk Purple Purple Green (PPG)", + 6.5, + DistanceUnit.INCH + ) + .addTag( + 24, + "Red Goal", + 6.5, + //new VectorF(-0f, -0f, -0f), + DistanceUnit.INCH//, + //Quaternion.identityQuaternion() + ) + .build(); + } + + public static double angularVelocity = 37.5; + public static double econserved = 1; +} \ No newline at end of file diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 1a2710768fcd..fc108cf6e9c5 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -5,14 +5,14 @@ repositories { } dependencies { - 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 'org.firstinspires.ftc:Inspection:11.0.0' + implementation 'org.firstinspires.ftc:Blocks:11.0.0' + implementation 'org.firstinspires.ftc:RobotCore:11.0.0' + implementation 'org.firstinspires.ftc:RobotServer:11.0.0' + implementation 'org.firstinspires.ftc:OnBotJava:11.0.0' + implementation 'org.firstinspires.ftc:Hardware:11.0.0' + implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' + implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.acmerobotics.dashboard:dashboard:0.4.17' }