From 0e67cf2e9c1e45e2d6ebe01fc6043806fc0781b9 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 3 Aug 2025 12:12:03 -0400 Subject: [PATCH 001/123] added a point3d class --- .../teamcode/blucru/common/util/Point3d.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point3d.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point3d.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point3d.java new file mode 100644 index 00000000..61c51f7f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point3d.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.blucru.common.util; + +public class Point3d { + private double x; + private double y; + private double z; + public Point3d(double x, double y, double z){ + this.x = x; + this.y = y; + this.z = z; + } + + public double getX(){ + return x; + } + + public double getY() { + return y; + } + + public double getZ() { + return z; + } + + public void setX(double x) { + this.x = x; + } + + public void setY(double y) { + this.y = y; + } + + public void setZ(double z) { + this.z = z; + } +} From 0707cf636e9f3f7b7810439b5027afb3cfa9d7da Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 6 Sep 2025 23:47:57 -0400 Subject: [PATCH 002/123] obelisk tag detector added --- .../common/util/ObeliskTagDetector.java | 117 ++++++++++++++++++ 1 file changed, 117 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java new file mode 100644 index 00000000..6b065954 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java @@ -0,0 +1,117 @@ +package org.firstinspires.ftc.teamcode.blucru.common.util; + +import android.graphics.Canvas; +import android.util.Log; +import android.util.Size; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.opencv.core.Mat; + +import java.util.ArrayList; +import java.util.Arrays; + +public class ObeliskTagDetector implements BluSubsystem { + + private static ObeliskTagDetector instance; + private final String cameraName = "atagCam"; + + private final int cameraResWidth = 1000; + private final int cameraResHeight = 1000; + VisionPortal obeliskTagPortal; + + AprilTagProcessor obeliskDetection; + + private String[] pattern = {"p","p","p"}; + + + public static ObeliskTagDetector getInstance(){ + if (instance == null){ + instance = new ObeliskTagDetector(); + } + + return instance; + } + + + private ObeliskTagDetector(){ + int[] id = VisionPortal.makeMultiPortalView(1, VisionPortal.MultiPortalLayout.VERTICAL); + + obeliskDetection = new AprilTagProcessor.Builder() + .setDrawAxes(false) + .setDrawTagID(true) + .setDrawTagOutline(true) + .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + .build(); + + + obeliskTagPortal = new VisionPortal.Builder() + .setCamera(Globals.hwMap.get(WebcamName.class, cameraName)) + .setCameraResolution(new Size(cameraResWidth, cameraResHeight)) + .setStreamFormat(VisionPortal.StreamFormat.MJPEG) + .addProcessor(obeliskDetection) + .setLiveViewContainerId(id[0]) + .build(); + + obeliskTagPortal.setProcessorEnabled(obeliskDetection, true); + + //stop tag streaming bc uneccessary + try{ + obeliskTagPortal.stopStreaming(); + } catch (Exception e){ + Log.e("AprilTags", "Could not stop obelisk tag portal streaming at init"); + } + } + + public void detectTags(){ + obeliskTagPortal.resumeStreaming(); + + //enable processor + obeliskTagPortal.setProcessorEnabled(obeliskDetection, true); + } + + @Override + public void init() { + read(); + } + + @Override + public void read() { + if (obeliskTagPortal.getProcessorEnabled(obeliskDetection)){ + ArrayList ids = obeliskDetection.getDetections(); + for (AprilTagDetection tag: ids){ + int id = tag.id; + if (id > 21 && id < 23){ + //tag is obelisk, set correct array pos + Globals.telemetry.addData("id", id); + pattern[id-21] = "g"; + } + } + } + } + + @Override + public void write() { + + } + + @Override + public void telemetry(Telemetry telemetry) { + telemetry.addData("pattern", Arrays.toString(pattern)); + telemetry.addData("streaming?", obeliskTagPortal.getProcessorEnabled(obeliskDetection)); + } + + @Override + public void reset() { + obeliskTagPortal.stopStreaming(); + } + + public String[] getPattern(){ + return pattern; + } +} From 84ccd944eb6046b8bd754f9565cea2cbf2cac114 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 7 Sep 2025 12:18:55 -0400 Subject: [PATCH 003/123] changed tag detector to give pattern and index of green --- .../teamcode/blucru/common/util/ObeliskTagDetector.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java index 6b065954..c8615c4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java @@ -28,6 +28,7 @@ public class ObeliskTagDetector implements BluSubsystem { AprilTagProcessor obeliskDetection; private String[] pattern = {"p","p","p"}; + int greenIndex; public static ObeliskTagDetector getInstance(){ @@ -89,7 +90,8 @@ public void read() { if (id > 21 && id < 23){ //tag is obelisk, set correct array pos Globals.telemetry.addData("id", id); - pattern[id-21] = "g"; + greenIndex = id-21; + pattern[greenIndex] = "g"; } } } @@ -114,4 +116,7 @@ public void reset() { public String[] getPattern(){ return pattern; } + public int getGreenIndex(){ + return greenIndex; + } } From f872e0c83d7a5ef750269cf440ed7564ebf9fd83 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Mon, 8 Sep 2025 21:39:18 -0400 Subject: [PATCH 004/123] updated to ftc controller v11.0 by copying every file except teamcode ones --- .../src/main/AndroidManifest.xml | 4 +- .../samples/ConceptAprilTagLocalization.java | 19 +- .../samples/ConceptGamepadEdgeDetection.java | 2 +- .../ConceptVisionColorLocator_Circle.java | 245 +++++++++++++++++ ... ConceptVisionColorLocator_Rectangle.java} | 140 +++++----- .../samples/ConceptVisionColorSensor.java | 59 ++-- .../SensorAndyMarkIMUNonOrthogonal.java | 193 +++++++++++++ .../samples/SensorAndyMarkIMUOrthogonal.java | 144 ++++++++++ .../external/samples/SensorAndyMarkTOF.java | 85 ++++++ .../external/samples/SensorColor.java | 18 +- .../external/samples/SensorOctoQuad.java | 73 +++-- .../external/samples/SensorOctoQuadAdv.java | 116 ++++---- .../samples/SensorOctoQuadLocalization.java | 255 ++++++++++++++++++ .../samples/UtilityOctoQuadConfigMenu.java | 9 + .../ConceptExternalHardwareClass.java | 0 .../{ => externalhardware}/RobotHardware.java | 0 README.md | 36 ++- build.dependencies.gradle | 16 +- 18 files changed, 1217 insertions(+), 197 deletions(-) create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java rename FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/{ConceptVisionColorLocator.java => ConceptVisionColorLocator_Rectangle.java} (50%) create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java create mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java rename FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/{ => externalhardware}/ConceptExternalHardwareClass.java (100%) rename FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/{ => externalhardware}/RobotHardware.java (100%) diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index f13a2a01..c873221b 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 d90261ef..3cb4d9fb 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 d5e0032f..90243ac5 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 00000000..4ec17aab --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java @@ -0,0 +1,245 @@ +/* + * 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 + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Locator (Circle)", group = "Concept") +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 e143d462..5681f712 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 86195cfc..5cce468d 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 00000000..cd678efe --- /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 00000000..58cee6d3 --- /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 00000000..ce6e943e --- /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 7546c9de..d0411af1 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 f797c6b0..312d7f51 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 e763b9aa..0e412ef4 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 00000000..486c4681 --- /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 a962919f..60be6c9f 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 5b785a68..522fb804 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/build.dependencies.gradle b/build.dependencies.gradle index 61062eeb..1284b2ee 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -13,14 +13,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' implementation 'com.github.StateFactory-Dev:StateFactory:0.3.9' From 2630224ada7a3203b864858981cdf2eef7937c0c Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Mon, 8 Sep 2025 21:45:11 -0400 Subject: [PATCH 005/123] basic shooter code --- .../common/subsytems/shooter/Shooter.java | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java new file mode 100644 index 00000000..1209e33d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; + +public class Shooter implements BluSubsystem { + + BluMotorWithEncoder shooter; + + public Shooter(){ + shooter = new BluMotorWithEncoder("shooter"); + } + + @Override + public void init() { + shooter.init(); + } + + @Override + public void read() { + shooter.read(); + } + + @Override + public void write() { + shooter.write(); + } + + public void shoot(double power){ + shooter.setPower(power); + } + + public void rampDownShooter(){ + shooter.setPower(0.5); + } + + @Override + public void telemetry(Telemetry telemetry) { + telemetry.addData("Shooter power", shooter.getPower()); + telemetry.addData("Shooter Pos", shooter.getCurrentPos()); + } + + @Override + public void reset() { + shooter.reset(); + shooter.read(); + } +} From 0c30456b23ac2545f2fdb33676fcd52c749390eb Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Tue, 9 Sep 2025 23:43:37 -0400 Subject: [PATCH 006/123] basic 6wd, no localization --- .../sixWheelDrive/SixWheelDrive.java | 60 +++++++++++++++++++ .../sixWheelDrive/SixWheelKinematics.java | 19 ++++++ 2 files changed, 79 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelKinematics.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java new file mode 100644 index 00000000..3e4c813a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; + +public class SixWheelDrive implements BluSubsystem { + + BluMotor[] dtMotors; + + public SixWheelDrive(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ + dtMotors = new BluMotor[]{fl, fr, bl, br}; + fr.setDirection(DcMotorSimple.Direction.REVERSE); + br.setDirection(DcMotorSimple.Direction.REVERSE); + } + + @Override + public void init() { + for (BluMotor motors: dtMotors){ + motors.init(); + } + } + + @Override + public void read() { + for (BluMotor motors: dtMotors){ + motors.read(); + } + } + + @Override + public void write() { + for (BluMotor motors: dtMotors){ + motors.write(); + } + } + + public void drive(double x, double r){ + double[] powers = SixWheelKinematics.getPowers(x,r); + for(int i = 0; i<4; i+=2){ + dtMotors[i].setPower(powers[0]); + dtMotors[i+1].setPower(powers[1]); + } + } + + @Override + public void telemetry(Telemetry telemetry) { + for (BluMotor motor:dtMotors){ + motor.telemetry(); + } + } + + @Override + public void reset() { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelKinematics.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelKinematics.java new file mode 100644 index 00000000..de5d7e19 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelKinematics.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive; + +public class SixWheelKinematics { + + public static double[] getPowers(double x, double rot){ + double left = x+rot; + double right = x-rot; + + double max = Math.max(Math.abs(left), Math.abs(right)); + + if (max>0){ + left /= max; + right /= max; + } + + return new double[]{left, right}; + } + +} From 3e9d9f8e6a3be1e7cb92877752e1b9b754923275 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Tue, 9 Sep 2025 23:46:35 -0400 Subject: [PATCH 007/123] pinpoint gobilda code --- .../localization/GoBildaPinpointDriver.java | 738 ++++++++++++++++++ 1 file changed, 738 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/GoBildaPinpointDriver.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/GoBildaPinpointDriver.java new file mode 100644 index 00000000..98b51c3d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/GoBildaPinpointDriver.java @@ -0,0 +1,738 @@ +/* MIT License + * Copyright (c) [2025] [Base 10 Assets, LLC] + * + * 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.teamcode.blucru.common.subsytems.sixWheelDrive.localization; + +import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; + +import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; +import com.qualcomm.robotcore.hardware.I2cAddr; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; +import com.qualcomm.robotcore.util.TypeConversion; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.Arrays; + + +@I2cDeviceType +@DeviceProperties( + name = "goBILDA® Pinpoint Odometry Computer", + xmlTag = "goBILDAPinpoint", + description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" +) + +public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { + + private int deviceStatus = 0; + private int loopTime = 0; + private int xEncoderValue = 0; + private int yEncoderValue = 0; + private float xPosition = 0; + private float yPosition = 0; + private float hOrientation = 0; + private float xVelocity = 0; + private float yVelocity = 0; + private float hVelocity = 0; + + private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod + private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod + + //i2c address of the device + public static final byte DEFAULT_ADDRESS = 0x31; + + public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { + super(deviceClient, deviceClientIsOwned); + + this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS)); + super.registerArmingStateCallback(false); + } + + + @Override + public Manufacturer getManufacturer() { + return Manufacturer.Other; + } + + @Override + protected synchronized boolean doInitialize() { + ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K); + return true; + } + + @Override + public String getDeviceName() { + return "goBILDA® Pinpoint Odometry Computer"; + } + + + //Register map of the i2c device + private enum Register { + DEVICE_ID (1), + DEVICE_VERSION (2), + DEVICE_STATUS (3), + DEVICE_CONTROL (4), + LOOP_TIME (5), + X_ENCODER_VALUE (6), + Y_ENCODER_VALUE (7), + X_POSITION (8), + Y_POSITION (9), + H_ORIENTATION (10), + X_VELOCITY (11), + Y_VELOCITY (12), + H_VELOCITY (13), + MM_PER_TICK (14), + X_POD_OFFSET (15), + Y_POD_OFFSET (16), + YAW_SCALAR (17), + BULK_READ (18); + + private final int bVal; + + Register(int bVal){ + this.bVal = bVal; + } + } + + //Device Status enum that captures the current fault condition of the device + public enum DeviceStatus{ + NOT_READY (0), + READY (1), + CALIBRATING (1 << 1), + FAULT_X_POD_NOT_DETECTED (1 << 2), + FAULT_Y_POD_NOT_DETECTED (1 << 3), + FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), + FAULT_IMU_RUNAWAY (1 << 4), + FAULT_BAD_READ (1 << 5); + + private final int status; + + DeviceStatus(int status){ + this.status = status; + } + } + + //enum that captures the direction the encoders are set to + public enum EncoderDirection{ + FORWARD, + REVERSED; + } + + //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used + public enum GoBildaOdometryPods { + goBILDA_SWINGARM_POD, + goBILDA_4_BAR_POD; + } + //enum that captures a limited scope of read data. More options may be added in future update + public enum ReadData { + ONLY_UPDATE_HEADING, + } + + + /** Writes an int to the i2c device + @param reg the register to write the int to + @param i the integer to write to the register + */ + private void writeInt(final Register reg, int i){ + deviceClient.write(reg.bVal, TypeConversion.intToByteArray(i,ByteOrder.LITTLE_ENDIAN)); + } + + /** + * Reads an int from a register of the i2c device + * @param reg the register to read from + * @return returns an int that contains the value stored in the read register + */ + private int readInt(Register reg){ + return byteArrayToInt(deviceClient.read(reg.bVal,4), ByteOrder.LITTLE_ENDIAN); + } + + /** + * Converts a byte array to a float value + * @param byteArray byte array to transform + * @param byteOrder order of byte array to convert + * @return the float value stored by the byte array + */ + private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ + return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); + } + + /** + * Reads a float from a register + * @param reg the register to read + * @return the float value stored in that register + */ + private float readFloat(Register reg){ + return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN); + } + + /** + * Converts a float to a byte array + * @param value the float array to convert + * @return the byte array converted from the float + */ + private byte [] floatToByteArray (float value, ByteOrder byteOrder) { + return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); + } + + /** + * Writes a byte array to a register on the i2c device + * @param reg the register to write to + * @param bytes the byte array to write + */ + private void writeByteArray (Register reg, byte[] bytes){ + deviceClient.write(reg.bVal,bytes); + } + + /** + * Writes a float to a register on the i2c device + * @param reg the register to write to + * @param f the float to write + */ + private void writeFloat (Register reg, float f){ + byte[] bytes = ByteBuffer.allocate(4).order(ByteOrder.LITTLE_ENDIAN).putFloat(f).array(); + deviceClient.write(reg.bVal,bytes); + } + + /** + * Looks up the DeviceStatus enum corresponding with an int value + * @param s int to lookup + * @return the Odometry Computer state + */ + private DeviceStatus lookupStatus (int s){ + if ((s & DeviceStatus.CALIBRATING.status) != 0){ + return DeviceStatus.CALIBRATING; + } + boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; + boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; + + if(!xPodDetected && !yPodDetected){ + return DeviceStatus.FAULT_NO_PODS_DETECTED; + } + if (!xPodDetected){ + return DeviceStatus.FAULT_X_POD_NOT_DETECTED; + } + if (!yPodDetected){ + return DeviceStatus.FAULT_Y_POD_NOT_DETECTED; + } + if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ + return DeviceStatus.FAULT_IMU_RUNAWAY; + } + if ((s & DeviceStatus.READY.status) != 0){ + return DeviceStatus.READY; + } + if ((s & DeviceStatus.FAULT_BAD_READ.status) != 0){ + return DeviceStatus.FAULT_BAD_READ; + } + else { + return DeviceStatus.NOT_READY; + } + } + + /** + * Confirm that the number received is a number, and does not include a change above the threshold + * @param oldValue the reading from the previous cycle + * @param newValue the new reading + * @param threshold the maximum change between this reading and the previous one + * @param bulkUpdate true if we are updating the loopTime variable. If not it should be false. + * @return newValue if the position is good, oldValue otherwise + */ + private Float isPositionCorrupt(float oldValue, float newValue, int threshold, boolean bulkUpdate){ + boolean noData = bulkUpdate && (loopTime < 1); + + boolean isCorrupt = noData || Float.isNaN(newValue) || Math.abs(newValue - oldValue) > threshold; + + if(!isCorrupt){ + return newValue; + } + + deviceStatus = DeviceStatus.FAULT_BAD_READ.status; + return oldValue; + } + + /** + * Confirm that the number received is a number, and does not include a change above the threshold + * @param oldValue the reading from the previous cycle + * @param newValue the new reading + * @param threshold the velocity allowed to be reported + * @return newValue if the velocity is good, oldValue otherwise + */ + private Float isVelocityCorrupt(float oldValue, float newValue, int threshold){ + boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue) > threshold; + boolean noData = (loopTime <= 1); + + if(!isCorrupt){ + return newValue; + } + + deviceStatus = DeviceStatus.FAULT_BAD_READ.status; + return oldValue; + } + + /** + * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. + */ + public void update(){ + final int positionThreshold = 5000; //more than one FTC field in mm + final int headingThreshold = 120; //About 20 full rotations in Radians + final int velocityThreshold = 10000; //10k mm/sec is faster than an FTC robot should be going... + final int headingVelocityThreshold = 120; //About 20 rotations per second + + float oldPosX = xPosition; + float oldPosY = yPosition; + float oldPosH = hOrientation; + float oldVelX = xVelocity; + float oldVelY = yVelocity; + float oldVelH = hVelocity; + + byte[] bArr = deviceClient.read(Register.BULK_READ.bVal, 40); + deviceStatus = byteArrayToInt(Arrays.copyOfRange (bArr, 0, 4), ByteOrder.LITTLE_ENDIAN); + loopTime = byteArrayToInt(Arrays.copyOfRange (bArr, 4, 8), ByteOrder.LITTLE_ENDIAN); + xEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 8, 12), ByteOrder.LITTLE_ENDIAN); + yEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 12,16), ByteOrder.LITTLE_ENDIAN); + xPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 16,20), ByteOrder.LITTLE_ENDIAN); + yPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 20,24), ByteOrder.LITTLE_ENDIAN); + hOrientation = byteArrayToFloat(Arrays.copyOfRange(bArr, 24,28), ByteOrder.LITTLE_ENDIAN); + xVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN); + yVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN); + hVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN); + + /* + * Check to see if any of the floats we have received from the device are NaN or are too large + * if they are, we return the previously read value and alert the user via the DeviceStatus Enum. + */ + xPosition = isPositionCorrupt(oldPosX, xPosition, positionThreshold, true); + yPosition = isPositionCorrupt(oldPosY, yPosition, positionThreshold, true); + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, true); + xVelocity = isVelocityCorrupt(oldVelX, xVelocity, velocityThreshold); + yVelocity = isVelocityCorrupt(oldVelY, yVelocity, velocityThreshold); + hVelocity = isVelocityCorrupt(oldVelH, hVelocity, headingVelocityThreshold); + + } + + /** + * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function + * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING + * is supported. + * @param data GoBildaPinpointDriver.ReadData.ONLY_UPDATE_HEADING + */ + public void update(ReadData data) { + if (data == ReadData.ONLY_UPDATE_HEADING) { + final int headingThreshold = 120; + + float oldPosH = hOrientation; + + hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); + + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, false); + + if (deviceStatus == DeviceStatus.FAULT_BAD_READ.status){ + deviceStatus = DeviceStatus.READY.status; + } + } + } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public void setOffsets(double xOffset, double yOffset){ + writeFloat(Register.X_POD_OFFSET, (float) xOffset); + writeFloat(Register.Y_POD_OFFSET, (float) yOffset); + } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + * @param distanceUnit the unit of distance used for offsets. + */ + public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit){ + writeFloat(Register.X_POD_OFFSET, (float) distanceUnit.toMm(xOffset)); + writeFloat(Register.Y_POD_OFFSET, (float) distanceUnit.toMm(yOffset)); + } + + /** + * Recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} + + /** + * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);} + + /** + * Can reverse the direction of each encoder. + * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward + * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left + */ + public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ + if (xEncoder == EncoderDirection.FORWARD){ + writeInt(Register.DEVICE_CONTROL,1<<5); + } + if (xEncoder == EncoderDirection.REVERSED) { + writeInt(Register.DEVICE_CONTROL,1<<4); + } + + if (yEncoder == EncoderDirection.FORWARD){ + writeInt(Register.DEVICE_CONTROL,1<<3); + } + if (yEncoder == EncoderDirection.REVERSED){ + writeInt(Register.DEVICE_CONTROL,1<<2); + } + } + + /** + * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

+ * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD + */ + public void setEncoderResolution(GoBildaOdometryPods pods){ + if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { + writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN))); + } + if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN))); + } + } + + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public void setEncoderResolution(double ticks_per_mm){ + writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * @param ticks_per_unit should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + * @param distanceUnit unit used for distance + */ + public void setEncoderResolution(double ticks_per_unit, DistanceUnit distanceUnit){ + double resolution = distanceUnit.toMm(ticks_per_unit); + writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) resolution,ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Tuning this value should be unnecessary.
+ * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

+ * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

+ * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. + * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

+ * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com + * @param yawOffset A scalar for the robot's heading. + */ + public void setYawScalar(double yawOffset){ + writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. You can use this to + * update the estimated position of your robot with new external sensor data, or to run a robot + * in field coordinates.

+ * This overrides the current position.

+ * Using this feature to track your robot's position in field coordinates:
+ * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
+ * Say you're on the red alliance, your robot is against the wall and closer to the audience side, + * and the front of your robot is pointing towards the center of the field. + * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always + * keep track of how far away from the center of the field you are.

+ * Using this feature to update your position with additional sensors:
+ * Some robots have a secondary way to locate their robot on the field. This is commonly + * Apriltag localization in FTC, but it can also be something like a distance sensor. + * Often these external sensors are absolute (meaning they measure something about the field) + * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific + * position on the field to use them. In that case, spend most of your time relying on the Pinpoint + * to determine your location. Then when you pull a new position from your secondary sensor, + * send a setPosition command with the new position. The Pinpoint will then track your movement + * relative to that new, more accurate position. + * @param pos a Pose2D describing the robot's new position. + */ + public Pose2D setPosition(Pose2D pos){ + writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); + writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); + return pos; + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param posX the new X position you'd like the Pinpoint to track your robot relive to. + * @param distanceUnit the unit for posX + */ + public void setPosX(double posX, DistanceUnit distanceUnit){ + writeByteArray(Register.X_POSITION,(floatToByteArray((float) distanceUnit.toMm(posX), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param posY the new Y position you'd like the Pinpoint to track your robot relive to. + * @param distanceUnit the unit for posY + */ + public void setPosY(double posY, DistanceUnit distanceUnit){ + writeByteArray(Register.Y_POSITION,(floatToByteArray((float) distanceUnit.toMm(posY), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a heading that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param heading the new heading you'd like the Pinpoint to track your robot relive to. + * @param angleUnit Radians or Degrees + */ + public void setHeading(double heading, AngleUnit angleUnit){ + writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) angleUnit.toRadians(heading), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Checks the deviceID of the Odometry Computer. Should return 1. + * @return 1 if device is functional. + */ + public int getDeviceID(){return readInt(Register.DEVICE_ID);} + + /** + * @return the firmware version of the Odometry Computer + */ + public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } + + /** + * @return a scalar that the IMU measured heading is multiplied by. This is tuned for each unit + * and should not need adjusted. + */ + public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } + + /** + * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: + * @return one of the following states:
+ * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
+ * READY - The device is currently functioning as normal. GREEN LED
+ * CALIBRATING - The device is currently recalibrating the gyro. RED LED
+ * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
+ * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
+ * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
+ * FAULT_BAD_READ - The Java code has detected a bad I²C read, the result reported is a + * duplicate of the last good read. + */ + public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); } + + /** + * Checks the Odometry Computer's most recent loop time.

+ * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return loop time in microseconds (1/1,000,000 seconds) + */ + public int getLoopTime(){return loopTime; } + + /** + * Checks the Odometry Computer's most recent loop frequency.

+ * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return Pinpoint Frequency in Hz (loops per second), + */ + public double getFrequency(){ + if (loopTime != 0){ + return 1000000.0/loopTime; + } + else { + return 0; + } + } + + /** + * @return the raw value of the X (forward) encoder in ticks + */ + public int getEncoderX(){return xEncoderValue; } + + /** + * @return the raw value of the Y (strafe) encoder in ticks + */ + public int getEncoderY(){return yEncoderValue; } + + /** + * @return the estimated X (forward) position of the robot in mm + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getPosX(){ + return xPosition; + } + + /** + * @return the estimated X (forward) position of the robot in specified unit + * @param distanceUnit the unit that the estimated position will return in + */ + public double getPosX(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(xPosition); + } + + /** + * @return the estimated Y (Strafe) position of the robot in mm + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getPosY(){ + return yPosition; + } + + /** + * @return the estimated Y (Strafe) position of the robot in specified unit + * @param distanceUnit the unit that the estimated position will return in + */ + public double getPosY(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(yPosition); + } + + /** + * @return the unnormalized estimated H (heading) position of the robot in radians + * unnormalized heading is not constrained from -180° to 180°. It will continue counting multiple rotations. + * @deprecated two overflows for this function exist with AngleUnit parameter. These minimize the possibility of unit confusion. + */ + public double getHeading(){ + return hOrientation; + } + + /** + * @return the normalized estimated H (heading) position of the robot in specified unit + * normalized heading is wrapped from -180°, to 180°. + */ + public double getHeading(AngleUnit angleUnit){ + return angleUnit.fromRadians((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI; + } + + /** + * @return the unnormalized estimated H (heading) position of the robot in specified unit + * unnormalized heading is not constrained from -180° to 180°. It will continue counting + * multiple rotations. + */ + public double getHeading(UnnormalizedAngleUnit unnormalizedAngleUnit){ + return unnormalizedAngleUnit.fromRadians(hOrientation); + } + + /** + * @return the estimated X (forward) velocity of the robot in mm/sec + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getVelX(){ + return xVelocity; + } + + /** + * @return the estimated X (forward) velocity of the robot in specified unit/sec + */ + public double getVelX(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(xVelocity); + } + + /** + * @return the estimated Y (strafe) velocity of the robot in mm/sec + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. + */ + public double getVelY(){ + return yVelocity; + } + + /** + * @return the estimated Y (strafe) velocity of the robot in specified unit/sec + */ + public double getVelY(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(yVelocity); + } + + /** + * @return the estimated H (heading) velocity of the robot in radians/sec + * @deprecated The overflow for this function has an AngleUnit, which can reduce the chance of unit confusion. + */ + public double getHeadingVelocity() { + return hVelocity; + } + + /** + * @return the estimated H (heading) velocity of the robot in specified unit/sec + */ + public double getHeadingVelocity(UnnormalizedAngleUnit unnormalizedAngleUnit){ + return unnormalizedAngleUnit.fromRadians(hVelocity); + } + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the X (forward) pod in specified unit + */ + public float getXOffset(DistanceUnit distanceUnit){ + return (float) distanceUnit.fromMm(readFloat(Register.X_POD_OFFSET)); + } + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the Y (strafe) pod + */ + public float getYOffset(DistanceUnit distanceUnit){ + return (float) distanceUnit.fromMm(readFloat(Register.Y_POD_OFFSET)); + } + + /** + * @return a Pose2D containing the estimated position of the robot + */ + public Pose2D getPosition(){ + return new Pose2D(DistanceUnit.MM, + xPosition, + yPosition, + AngleUnit.RADIANS, + //this wraps the hOrientation variable from -180° to +180° + ((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); + } + + /** + * @deprecated This function is not recommended, as velocity is wrapped from -180° to 180°. + * instead use individual getters. + * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second + */ + public Pose2D getVelocity(){ + return new Pose2D(DistanceUnit.MM, + xVelocity, + yVelocity, + AngleUnit.RADIANS, + ((hVelocity + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); + } +} From 943945be03089c64270031462419bce55c76bbd0 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Wed, 10 Sep 2025 00:23:39 -0400 Subject: [PATCH 008/123] added pinpoint code --- .../sixWheelDrive/localization/Pinpoint.java | 126 ++++++++++++++++++ .../localization/RobotLocalizer.java | 19 +++ 2 files changed, 145 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/RobotLocalizer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java new file mode 100644 index 00000000..59a75cb1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java @@ -0,0 +1,126 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.RobotLocalizer; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; + +public class Pinpoint implements RobotLocalizer { + //TODO TUNE PER ROBOT + public static double parallelYOffset = -144.675, perpXOffset = -70; + org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver pinpoint; + double headingOffset; + + Pose2D pinpointPose; + + public Pinpoint(String name){ + this(Globals.hwMap.get(org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver.class, name)); + } + + public Pinpoint(org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver pinpoint){ + this.pinpoint = pinpoint; + headingOffset = 0; + //TODO UPDATE PER ROBOT + pinpoint.setEncoderDirections( + org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver.EncoderDirection.FORWARD, + org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver.EncoderDirection.REVERSED); + pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + pinpoint.setOffsets(parallelYOffset, perpXOffset, DistanceUnit.MM); + + pinpoint.resetPosAndIMU(); + pinpointPose = pinpoint.getPosition(); + } + + @Override + public void read() { + pinpoint.update(); + pinpointPose = pinpoint.getPosition(); + } + + /** + * returns pose in x,y,h in inch,inch,radian + * */ + @Override + public Pose2d getPose() { + + //Pose2D is different from Pose2d, Pose2D is ftc's Pose where Pose2d is the custom pose + return new Pose2d(pinpointPose.getX(DistanceUnit.INCH), pinpointPose.getY(DistanceUnit.INCH), pinpointPose.getHeading(AngleUnit.RADIANS)); + } + + /** + * in inches + * */ + @Override + public double getX() { + return pinpointPose.getX(DistanceUnit.INCH); + } + + /** + * in inches + * */ + @Override + public double getY() { + return pinpointPose.getY(DistanceUnit.INCH); + } + + /** + * in radians + * */ + @Override + public double getHeading() { + return pinpointPose.getHeading(AngleUnit.RADIANS) - headingOffset; + } + + /** + * for setting offsets, inch, inch, radian + * */ + @Override + public void setOffset(double x, double y, double h) { + pinpoint.setOffsets(x,y,DistanceUnit.INCH); + headingOffset = h; + } + + /** + * inch, inch, radian + * */ + public void setPosition(double x, double y, double h){ + pinpoint.setPosX(x, DistanceUnit.INCH); + pinpoint.setPosY(y, DistanceUnit.INCH); + pinpoint.setHeading(h, AngleUnit.RADIANS); + read(); + } + + @Override + public void setPosition(Pose2d pose) { + + + Globals.telemetry.addData("Pose", "X: " + pose.getX() + ",Y: " + pose.getY() + ",H: " + pose.getH()); + + pinpoint.setPosX(pose.getX(), DistanceUnit.INCH); + pinpoint.setPosY(pose.getY(), DistanceUnit.INCH); + pinpoint.setHeading(pose.getH(), AngleUnit.RADIANS); + read(); + } + + @Override + public Pose2d getVel() { + return new Pose2d(pinpoint.getVelX(DistanceUnit.INCH), pinpoint.getVelY(DistanceUnit.INCH), pinpoint.getHeadingVelocity(UnnormalizedAngleUnit.RADIANS)); + } + + @Override + public void setHeading(double heading) { + pinpoint.setHeading(heading, AngleUnit.RADIANS); + } + + @Override + public void telemetry(Telemetry telemetry) { + + telemetry.addData("Pinpoint heading", pinpoint.getHeading(AngleUnit.RADIANS)); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/RobotLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/RobotLocalizer.java new file mode 100644 index 00000000..3baf2d10 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/RobotLocalizer.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; + +public interface RobotLocalizer { + + public void read(); + public Pose2d getPose(); + public double getX(); + public double getY(); + public double getHeading(); + public void setOffset(double x, double y, double h); + public void setPosition(double x, double y, double h); + public void setPosition(Pose2d pose); + public Pose2d getVel(); + public void setHeading(double heading); + public void telemetry(Telemetry telemetry); +} From caeaebdce2799ea00f7f0f430edda57759e60625 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Wed, 10 Sep 2025 17:54:33 -0400 Subject: [PATCH 009/123] created pure pursuit computer --- .../purePursuit/PurePursuitComputer.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java new file mode 100644 index 00000000..a1916503 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.purePursuit; + +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; + +public class PurePursuitComputer { + double[][] points; + double lookAheadDist; + + public PurePursuitComputer(double[][] points, double lookAheadDist){ + this.points = points; + this.lookAheadDist = lookAheadDist; + } + +} From 01649bc2038d55ca712b45ee32ef24fa3e3c2fcc Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 12 Sep 2025 19:15:40 -0400 Subject: [PATCH 010/123] basic turret code --- .../blucru/common/subsytems/Robot.java | 9 ++++ .../common/subsytems/turret/Turret.java | 48 +++++++++++++++++++ .../blucru/opmodes/BluLinearOpMode.java | 3 ++ .../blucru/opmodes/test/TurretTest.java | 22 +++++++++ 4 files changed, 82 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 8b01c2e8..a2228975 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -5,7 +5,9 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.Turret; import java.util.ArrayList; import java.util.List; @@ -19,6 +21,7 @@ public class Robot { //list of subsystems static ArrayList subsystems; public Drivetrain drivetrain; + public Turret turret; private static Robot instance; HardwareMap hwMap; List hubs; @@ -109,6 +112,12 @@ public Drivetrain addDrivetrain(){ return drivetrain; } + public Turret addTurret(){ + turret = new Turret(new BluMotorWithEncoder("turret")); + subsystems.add(turret); + return turret; + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java new file mode 100644 index 00000000..575b1ac7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; + +public class Turret implements BluSubsystem { + BluMotorWithEncoder turret; + public Turret(BluMotorWithEncoder turret){ + this.turret = turret; + } + + private enum State{ + POWER_MODE, + PID_MODE, + GOAL_LOCK_MODE; + + } + + @Override + public void init() { + turret.init(); + } + + @Override + public void read() { + turret.read(); + } + + @Override + public void write() { + turret.write(); + } + + public void moveWithPower(double power){ + turret.setPower(power); + } + + @Override + public void telemetry(Telemetry telemetry) { + turret.telemetry(); + } + + @Override + public void reset() { + turret.reset(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index 60ac7e0d..67180ab2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.hardware.SinglePressGamepad; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.Turret; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public abstract class BluLinearOpMode extends LinearOpMode { @@ -17,6 +18,7 @@ public abstract class BluLinearOpMode extends LinearOpMode { protected boolean reportTelemetry = true; public Drivetrain drivetrain; + public Turret turret; //add all of the subsystems that need to be added to the robot here @@ -164,6 +166,7 @@ public void end(){ } public void addDrivetrain(){drivetrain = robot.addDrivetrain();} + public void addTurret(){turret = robot.addTurret();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java new file mode 100644 index 00000000..242f42d2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +public class TurretTest extends BluLinearOpMode { + + public void initialize(){ + robot.clear(); + addTurret(); + } + + public void periodic(){ + //positive power is left + turret.moveWithPower(-gamepad1.left_stick_y); + } + + public void telemetry(){ + turret.telemetry(telemetry); + } + +} From 6434b7f15bcc651444f308e885883fa477d4207d Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 13 Sep 2025 12:39:58 -0400 Subject: [PATCH 011/123] started pure pursuit computer --- .../purePursuit/PurePursuitComputer.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java index a1916503..d217d97d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java @@ -11,4 +11,17 @@ public PurePursuitComputer(double[][] points, double lookAheadDist){ this.lookAheadDist = lookAheadDist; } + public void getLineIntersections(double[] p1, double[] p2, Pose2d robotPose){ + + double[] p1Shifted = {p1[0]-robotPose.getX(), p1[1]-robotPose.getY()}; + double[] p2Shifted = {p2[0] - robotPose.getX(), p2[1] - robotPose.getY()}; + //robot pose is now 0,0,h, and because heading doesnt matter for line intersections, the robot is equivalently at 0,0 + + double dx = p2[0] - p1[0]; + double dy = p2[1] - p1[1]; + + double dr = Math.sqrt(dx*dx + dy*dy); + + } + } From 09172f8b02f15d1ef28df451878192022cfce8d3 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 13 Sep 2025 14:57:22 -0400 Subject: [PATCH 012/123] tags work --- .../blucru/common/subsytems/Robot.java | 8 +++++ .../common/util/ObeliskTagDetector.java | 22 ++++++++++--- .../blucru/opmodes/BluLinearOpMode.java | 3 ++ .../blucru/opmodes/test/ObeliskTagTest.java | 31 +++++++++++++++++++ 4 files changed, 59 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ObeliskTagTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 8b01c2e8..a35ebbc8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -6,6 +6,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.util.ObeliskTagDetector; import java.util.ArrayList; import java.util.List; @@ -19,6 +20,7 @@ public class Robot { //list of subsystems static ArrayList subsystems; public Drivetrain drivetrain; + public ObeliskTagDetector obeliskTagDetector; private static Robot instance; HardwareMap hwMap; List hubs; @@ -109,6 +111,12 @@ public Drivetrain addDrivetrain(){ return drivetrain; } + public ObeliskTagDetector addObeliskTagDetector(){ + obeliskTagDetector = ObeliskTagDetector.getInstance(); + subsystems.add(obeliskTagDetector); + return obeliskTagDetector; + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java index c8615c4c..84f0b8fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/ObeliskTagDetector.java @@ -21,8 +21,8 @@ public class ObeliskTagDetector implements BluSubsystem { private static ObeliskTagDetector instance; private final String cameraName = "atagCam"; - private final int cameraResWidth = 1000; - private final int cameraResHeight = 1000; + private final int cameraResWidth = 1280; + private final int cameraResHeight = 720; VisionPortal obeliskTagPortal; AprilTagProcessor obeliskDetection; @@ -41,7 +41,8 @@ public static ObeliskTagDetector getInstance(){ private ObeliskTagDetector(){ - int[] id = VisionPortal.makeMultiPortalView(1, VisionPortal.MultiPortalLayout.VERTICAL); + + int[] ids = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.HORIZONTAL); obeliskDetection = new AprilTagProcessor.Builder() .setDrawAxes(false) @@ -56,7 +57,8 @@ private ObeliskTagDetector(){ .setCameraResolution(new Size(cameraResWidth, cameraResHeight)) .setStreamFormat(VisionPortal.StreamFormat.MJPEG) .addProcessor(obeliskDetection) - .setLiveViewContainerId(id[0]) + .enableLiveView(true) + .setLiveViewContainerId(ids[0]) .build(); obeliskTagPortal.setProcessorEnabled(obeliskDetection, true); @@ -64,6 +66,7 @@ private ObeliskTagDetector(){ //stop tag streaming bc uneccessary try{ obeliskTagPortal.stopStreaming(); + obeliskTagPortal.setProcessorEnabled(obeliskDetection, false); } catch (Exception e){ Log.e("AprilTags", "Could not stop obelisk tag portal streaming at init"); } @@ -75,6 +78,13 @@ public void detectTags(){ //enable processor obeliskTagPortal.setProcessorEnabled(obeliskDetection, true); } + public void stopTagDetection(){ + obeliskTagPortal.stopStreaming(); + obeliskTagPortal.setProcessorEnabled(obeliskDetection, false); + } + public boolean enabled(){ + return obeliskTagPortal.getProcessorEnabled(obeliskDetection); + } @Override public void init() { @@ -85,9 +95,11 @@ public void init() { public void read() { if (obeliskTagPortal.getProcessorEnabled(obeliskDetection)){ ArrayList ids = obeliskDetection.getDetections(); + Globals.telemetry.addData("detection sizes", ids.size()); for (AprilTagDetection tag: ids){ int id = tag.id; - if (id > 21 && id < 23){ + Globals.telemetry.addData("id", id); + if (id >= 21 && id <= 23){ //tag is obelisk, set correct array pos Globals.telemetry.addData("id", id); greenIndex = id-21; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index 60ac7e0d..fe910e97 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -10,6 +10,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.ObeliskTagDetector; public abstract class BluLinearOpMode extends LinearOpMode { @@ -17,6 +18,7 @@ public abstract class BluLinearOpMode extends LinearOpMode { protected boolean reportTelemetry = true; public Drivetrain drivetrain; + public ObeliskTagDetector obeliskTagDetector; //add all of the subsystems that need to be added to the robot here @@ -164,6 +166,7 @@ public void end(){ } public void addDrivetrain(){drivetrain = robot.addDrivetrain();} + public void addObeliskTagDetector(){obeliskTagDetector = robot.addObeliskTagDetector();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ObeliskTagTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ObeliskTagTest.java new file mode 100644 index 00000000..1f4487a2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ObeliskTagTest.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.common.util.ObeliskTagDetector; +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +import java.util.Arrays; + +@TeleOp(group = "test") +public class ObeliskTagTest extends BluLinearOpMode { + + public void initialize(){ + addObeliskTagDetector(); + } + + public void periodic(){ + telemetry.addData("Pattern", Arrays.toString(obeliskTagDetector.getPattern())); + telemetry.addData("Enabled?", obeliskTagDetector.enabled()); + + if (driver1.pressedA()){ + obeliskTagDetector.stopTagDetection(); + } + + if (driver1.pressedB()){ + obeliskTagDetector.detectTags(); + } + } + +} From 27e93c5a9efe30b23778ff1f2d24c8b7c343b4e7 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Mon, 15 Sep 2025 20:22:36 -0400 Subject: [PATCH 013/123] line circle intersections --- .../purePursuit/PurePursuitComputer.java | 49 +++++++++++++++++-- .../teamcode/blucru/common/util/Point2d.java | 26 ++++++++++ 2 files changed, 70 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point2d.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java index d217d97d..c25731eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.purePursuit; +import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; public class PurePursuitComputer { @@ -11,17 +12,55 @@ public PurePursuitComputer(double[][] points, double lookAheadDist){ this.lookAheadDist = lookAheadDist; } - public void getLineIntersections(double[] p1, double[] p2, Pose2d robotPose){ + public double sgn(double x){ + if (x<0){ + return -1; + } + return 1; + } + + public Point2d[] getLineIntersections(Point2d p1, Point2d p2, Pose2d robotPose, double lookAheadDist){ - double[] p1Shifted = {p1[0]-robotPose.getX(), p1[1]-robotPose.getY()}; - double[] p2Shifted = {p2[0] - robotPose.getX(), p2[1] - robotPose.getY()}; + double[] p1Shifted = {p1.getX() -robotPose.getX(), p1.getY()-robotPose.getY()}; + double[] p2Shifted = {p2.getX() - robotPose.getX(), p2.getY() - robotPose.getY()}; //robot pose is now 0,0,h, and because heading doesnt matter for line intersections, the robot is equivalently at 0,0 - double dx = p2[0] - p1[0]; - double dy = p2[1] - p1[1]; + double dx = p2.getX() - p1.getX(); + double dy = p2.getY() - p1.getY(); double dr = Math.sqrt(dx*dx + dy*dy); + double D = p1.getX()*p1.getX() - p2.getY()*p2.getX(); + + double discriminant = lookAheadDist * lookAheadDist * dr * dr - D * D; + + if (discriminant < 0){ + return new Point2d[0]; + } + + Point2d sol1 = new Point2d((D * dy + sgn(dy) * dx * Math.sqrt(discriminant))/(dr * dr), + (-D * dx + Math.abs(dy) * Math.sqrt(discriminant))/(dr * dr)); + + Point2d sol2 = new Point2d((D * dy - sgn(dy) * dx * Math.sqrt(discriminant))/(dr * dr), + (-D * dx - Math.abs(dy) * Math.sqrt(discriminant))/(dr * dr)); + + boolean sol1XInRange = Math.min(p1.getX(), p2.getX()) <= sol1.getX() && sol1.getX() <= Math.max(p1.getX(), p2.getX()); + boolean sol1YInRange = Math.min(p1.getY(), p2.getY()) <= sol1.getY() && sol1.getY() <= Math.max(p1.getY(), p2.getY()); + boolean sol2XInRange = Math.min(p1.getX(), p2.getX()) <= sol2.getX() && sol2.getX() <= Math.max(p1.getX(), p2.getX()); + boolean sol2YInRange = Math.min(p1.getY(), p2.getY()) <= sol2.getY() && sol2.getY() <= Math.max(p1.getY(), p2.getY()); + + boolean sol1InRange = sol1XInRange && sol1YInRange; + boolean sol2InRange = sol2XInRange && sol2YInRange; + + if (sol1InRange && sol2InRange){ + return new Point2d[]{sol1, sol2}; + } else if (sol1InRange && !sol2InRange){ + return new Point2d[]{sol1}; + } else if (!sol1InRange && !sol2InRange){ + return new Point2d[0]; + } else { + return new Point2d[]{sol2}; + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point2d.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point2d.java new file mode 100644 index 00000000..41f7b0aa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Point2d.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.blucru.common.util; + +public class Point2d { + double x; + double y; + public Point2d(double x, double y){ + this.x = x; + this.y = y; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public void setX(double x) { + this.x = x; + } + + public void setY(double y) { + this.y = y; + } +} From 9974433499db4e3abc29436aab43b3b6f0ca114b Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Tue, 16 Sep 2025 18:21:39 -0400 Subject: [PATCH 014/123] added obelisk tag + localization for limelight --- .../util/LimelightObeliskTagDetector.java | 67 +++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java new file mode 100644 index 00000000..7be80bfe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.teamcode.blucru.common.util; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; + +import java.util.ArrayList; +import java.util.List; + +public class LimelightObeliskTagDetector implements BluSubsystem { + Limelight3A limelight; + String[] pattern; + int greenIndex; + Pose2d botpose; + public LimelightObeliskTagDetector(){ + limelight = Globals.hwMap.get(Limelight3A.class, "limelight"); + limelight.setPollRateHz(100); + limelight.start(); + limelight.pipelineSwitch(0); + pattern = new String[]{"p","p","p"}; + } + + @Override + public void init() { + + } + + @Override + public void read() { + LLResult result = limelight.getLatestResult(); + if (result != null && result.isValid()){ + List tags = result.getFiducialResults(); + for (LLResultTypes.FiducialResult res: tags){ + int id = res.getFiducialId(); + if (id >= 21 && id <= 23){ + //pattern id + greenIndex = id-21; + pattern[greenIndex] = "g"; + } else { + //location tag + Pose3D bot = result.getBotpose(); + botpose = new Pose2d(bot.getPosition().x, bot.getPosition().y, bot.getOrientation().getYaw(AngleUnit.RADIANS)); + } + } + } + } + + @Override + public void write() { + + } + + @Override + public void telemetry(Telemetry telemetry) { + + } + + @Override + public void reset() { + + } +} From 18817b3ae68c7c0076f59bd6232d7f21447fe675 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Thu, 18 Sep 2025 22:06:18 -0400 Subject: [PATCH 015/123] ll obelisk works --- .../blucru/common/subsytems/Robot.java | 8 +++++++ .../util/LimelightObeliskTagDetector.java | 5 +++++ .../blucru/opmodes/BluLinearOpMode.java | 3 +++ .../blucru/opmodes/test/LLObeliskTagTest.java | 21 +++++++++++++++++++ 4 files changed, 37 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index a35ebbc8..536a4a0d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -6,6 +6,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.util.LimelightObeliskTagDetector; import org.firstinspires.ftc.teamcode.blucru.common.util.ObeliskTagDetector; import java.util.ArrayList; @@ -21,6 +22,7 @@ public class Robot { static ArrayList subsystems; public Drivetrain drivetrain; public ObeliskTagDetector obeliskTagDetector; + public LimelightObeliskTagDetector llTagDetector; private static Robot instance; HardwareMap hwMap; List hubs; @@ -117,6 +119,12 @@ public ObeliskTagDetector addObeliskTagDetector(){ return obeliskTagDetector; } + public LimelightObeliskTagDetector addLLTagDetector(){ + llTagDetector = new LimelightObeliskTagDetector(); + subsystems.add(llTagDetector); + return llTagDetector; + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java index 7be80bfe..d1bbf7e6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java @@ -60,6 +60,11 @@ public void telemetry(Telemetry telemetry) { } + public String[] getPattern(){ + return pattern; + } + + @Override public void reset() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index fe910e97..d5cb307d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -10,6 +10,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.LimelightObeliskTagDetector; import org.firstinspires.ftc.teamcode.blucru.common.util.ObeliskTagDetector; public abstract class BluLinearOpMode extends LinearOpMode { @@ -19,6 +20,7 @@ public abstract class BluLinearOpMode extends LinearOpMode { public Drivetrain drivetrain; public ObeliskTagDetector obeliskTagDetector; + public LimelightObeliskTagDetector llTagDetector; //add all of the subsystems that need to be added to the robot here @@ -167,6 +169,7 @@ public void end(){ public void addDrivetrain(){drivetrain = robot.addDrivetrain();} public void addObeliskTagDetector(){obeliskTagDetector = robot.addObeliskTagDetector();} + public void addLLTagDetector(){llTagDetector = robot.addLLTagDetector();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java new file mode 100644 index 00000000..75c48aa9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +import java.util.Arrays; + +@TeleOp(group = "test") +public class LLObeliskTagTest extends BluLinearOpMode { + + public void initialize(){ + addLLTagDetector(); + } + + public void periodic(){ + telemetry.addData("Pattern", Arrays.toString(llTagDetector.getPattern())); + } + +} From e060e7062f6620dee46abb3b2c586d61e3003d54 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 20 Sep 2025 11:19:18 -0400 Subject: [PATCH 016/123] finding best goal point --- .../purePursuit/PurePursuitComputer.java | 81 +++++++++++++++++++ 1 file changed, 81 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java index c25731eb..4699534a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java @@ -1,15 +1,27 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.purePursuit; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; + + +/** + * Guide from https://wiki.purduesigbots.com/software/control-algorithms/basic-pure-pursuit + * */ public class PurePursuitComputer { double[][] points; double lookAheadDist; + int lastFoundIndex; public PurePursuitComputer(double[][] points, double lookAheadDist){ this.points = points; this.lookAheadDist = lookAheadDist; + lastFoundIndex = 0; + } + + public void resetLastFoundIndex(){ + lastFoundIndex = 0; } public double sgn(double x){ @@ -59,8 +71,77 @@ public Point2d[] getLineIntersections(Point2d p1, Point2d p2, Pose2d robotPose, } else if (!sol1InRange && !sol2InRange){ return new Point2d[0]; } else { + return new Point2d[]{sol2}; } } + private double findDistBetween2Points(Point2d p1, Point2d p2){ + double deltaX = p2.getX() - p1.getX(); + double deltaY = p2.getY() - p1.getY(); + + return Math.sqrt(deltaX * deltaX + deltaY * deltaY); + } + public Point2d findOptimalGoToPoint(Pose2d robotPose, Point2d[] path, double lookAheadDist){ + boolean foundIntersection = false; + Point2d goalPoint = null; + Point2d[][] pointsSols = new Point2d[path.length-1][2]; + for (int i = 0; i < path.length - 1; i++) { + pointsSols[i] = getLineIntersections(path[i], path[i+1],robotPose, lookAheadDist); + } + + for (int i=0; i + findDistBetween2Points(new Point2d(robotPose.getX(), robotPose.getY()), path[i+1])){ + //going there would be bad, dont pick it + //there should be a better point + //setting lastFoundIndex to always be the point ahead in case the robot cant find a point in later sols + lastFoundIndex = i + 1; + } else { + lastFoundIndex = i; + goalPoint = sols[0]; + break; + } + } else { + //2 vals + //find closer point + + Point2d closerPoint = sols[1]; + + if (findDistBetween2Points(sols[0], path[i+1]) < findDistBetween2Points(sols[1], path[i+1])){ + closerPoint = sols[0]; + } + + if (findDistBetween2Points(closerPoint, path[i+1]) > + findDistBetween2Points(new Point2d(robotPose.getX(), robotPose.getY()), closerPoint)){ + //going to point would be bad, dont pick it + //there should be a better point + //setting lastFoundIndex to always be the point ahead in case the robot cant find a point in later sols + lastFoundIndex = i + 1; + } else { + lastFoundIndex = i; + goalPoint = closerPoint; + break; + } + + } + } + + if (goalPoint == null){ + //no goal point chosen, then go to last found index of intersection on path + goalPoint = path[lastFoundIndex]; + } + + return goalPoint; + } + + public void moveTowardsTargetPoint(Pose2d robotPose, Point2d goalPoint){ + + //get robot heading between 0 and 360 + double robotHeading = Globals.normalize(robotPose.getH()); + } + } From fdb61ac7b0ebbe71f947164fed263d6869d8c9e2 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 20 Sep 2025 14:15:20 -0400 Subject: [PATCH 017/123] detect pos as well for ll --- .../blucru/common/util/LimelightObeliskTagDetector.java | 3 +++ .../ftc/teamcode/blucru/opmodes/BluLinearOpMode.java | 1 + .../ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java | 3 +++ 3 files changed, 7 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java index d1bbf7e6..118c240b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java @@ -23,6 +23,7 @@ public LimelightObeliskTagDetector(){ limelight.start(); limelight.pipelineSwitch(0); pattern = new String[]{"p","p","p"}; + botpose = new Pose2d(0,0,0); } @Override @@ -64,6 +65,8 @@ public String[] getPattern(){ return pattern; } + public Pose2d getLLBotPose(){return botpose;} + @Override public void reset() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index d5cb307d..280bab24 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -55,6 +55,7 @@ public final void runOpMode() throws InterruptedException { robot = Robot.getInstance(); robot.setHwMap(Globals.hwMap); Globals.updateVoltage(robot.getVoltage()); + robot.clear(); initialize(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java index 75c48aa9..1a8b76c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LLObeliskTagTest.java @@ -3,6 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; import java.util.Arrays; @@ -16,6 +17,8 @@ public void initialize(){ public void periodic(){ telemetry.addData("Pattern", Arrays.toString(llTagDetector.getPattern())); + Pose2d llBotPose = llTagDetector.getLLBotPose(); + telemetry.addData("LL BotPose", "X: " + llBotPose.getX() + ",Y: " + llBotPose.getY() + ",H: " + llBotPose.getH()); } } From fe4dcddf12f3cd717cc01fc9c4da58e7bc9b0c86 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 21 Sep 2025 12:39:50 -0400 Subject: [PATCH 018/123] shooter --- .../blucru/common/subsytems/Robot.java | 8 +++++++ .../common/subsytems/shooter/Shooter.java | 14 ++++++++++++- .../teamcode/blucru/common/util/Globals.java | 3 +++ .../blucru/opmodes/BluLinearOpMode.java | 3 +++ .../blucru/opmodes/test/ShooterTest.java | 21 +++++++++++++++++++ 5 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 8b01c2e8..74400553 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -6,6 +6,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.Shooter; import java.util.ArrayList; import java.util.List; @@ -19,6 +20,7 @@ public class Robot { //list of subsystems static ArrayList subsystems; public Drivetrain drivetrain; + public Shooter shooter; private static Robot instance; HardwareMap hwMap; List hubs; @@ -109,6 +111,12 @@ public Drivetrain addDrivetrain(){ return drivetrain; } + public Shooter addShooter(){ + shooter = new Shooter(); + subsystems.add(shooter); + return shooter; + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index 1209e33d..ceb210f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -2,14 +2,18 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public class Shooter implements BluSubsystem { BluMotorWithEncoder shooter; + BluServo hood; public Shooter(){ shooter = new BluMotorWithEncoder("shooter"); + hood = new BluServo("hood"); } @Override @@ -25,6 +29,7 @@ public void read() { @Override public void write() { shooter.write(); + hood.write(); } public void shoot(double power){ @@ -34,6 +39,13 @@ public void shoot(double power){ public void rampDownShooter(){ shooter.setPower(0.5); } + public void setHoodAngle(double angle){ + hood.setPos(Globals.convertAngleToServoPos(255, angle)); + } + public void setHoodServoPos(double pos){ + hood.setPos(pos); + } + @Override public void telemetry(Telemetry telemetry) { @@ -46,4 +58,4 @@ public void reset() { shooter.reset(); shooter.read(); } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java index 41e92ed6..23ee80df 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java @@ -76,6 +76,9 @@ public static double normalize(double angle) { public static double convertServoPosToAngle(double range, double servoPos){ return (servoPos - 0.5) * range; } + public static double convertAngleToServoPos(double range, double angle){ + return (angle)/range + 0.5; + } public static double convertMotorPositionToRotations(double rpm, double pos){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index 60ac7e0d..fe40e2cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.hardware.SinglePressGamepad; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.Shooter; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public abstract class BluLinearOpMode extends LinearOpMode { @@ -17,6 +18,7 @@ public abstract class BluLinearOpMode extends LinearOpMode { protected boolean reportTelemetry = true; public Drivetrain drivetrain; + public Shooter shooter; //add all of the subsystems that need to be added to the robot here @@ -164,6 +166,7 @@ public void end(){ } public void addDrivetrain(){drivetrain = robot.addDrivetrain();} + public void addShooter(){shooter = robot.addShooter();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java new file mode 100644 index 00000000..dd540ebe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; +@TeleOp(group = "test") +public class ShooterTest extends BluLinearOpMode { + double hoodAngle; + public void initialize(){ + addShooter(); + hoodAngle = 0.5; + } + + public void periodic(){ + shooter.shoot(-gamepad1.left_stick_y); + + hoodAngle += -gamepad1.right_stick_y * 2.0; + shooter.setHoodAngle(hoodAngle); + } + +} From 6a7562bbbe50db51fb56415d6218aebcb1aa4ca4 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Thu, 25 Sep 2025 17:32:15 -0400 Subject: [PATCH 019/123] shooter test --- .../ftc/teamcode/blucru/opmodes/test/ShooterTest.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java index dd540ebe..c4fcc9de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java @@ -7,6 +7,7 @@ public class ShooterTest extends BluLinearOpMode { double hoodAngle; public void initialize(){ + robot.clear(); addShooter(); hoodAngle = 0.5; } From c22018793cc716e37975c212735399525b4707c4 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 26 Sep 2025 22:51:21 -0400 Subject: [PATCH 020/123] completed pure pursuit computer --- .../purePursuit/PurePursuitComputer.java | 28 ++++++---- .../purePursuit/SixWheelPID.java | 52 +++++++++++++++++++ 2 files changed, 70 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/SixWheelPID.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java index 4699534a..1876da3b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.purePursuit; -import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; @@ -10,13 +9,8 @@ * Guide from https://wiki.purduesigbots.com/software/control-algorithms/basic-pure-pursuit * */ public class PurePursuitComputer { - double[][] points; - double lookAheadDist; int lastFoundIndex; - - public PurePursuitComputer(double[][] points, double lookAheadDist){ - this.points = points; - this.lookAheadDist = lookAheadDist; + public PurePursuitComputer(){ lastFoundIndex = 0; } @@ -138,10 +132,24 @@ public Point2d findOptimalGoToPoint(Pose2d robotPose, Point2d[] path, double loo return goalPoint; } - public void moveTowardsTargetPoint(Pose2d robotPose, Point2d goalPoint){ + public double getReqAngleVelTowardsTargetPoint(Pose2d robotPose, Point2d goalPoint, double angleVel, SixWheelPID pid){ + return pid.getHeadingVel(robotPose, goalPoint, angleVel); + } + + public double compute(Point2d[] path, Pose2d robotPose, double angleVel, double lookAheadDist, SixWheelPID pid){ + Point2d goalPoint = findOptimalGoToPoint(robotPose, path, lookAheadDist); + + return getReqAngleVelTowardsTargetPoint(robotPose, goalPoint, angleVel, pid); + } + + public double[] computeRotAndXY(Point2d[] path, Pose2d robotPose, Pose2d robotVel, double lookAheadDist, SixWheelPID pid){ + Point2d goalPoint = findOptimalGoToPoint(robotPose, path, lookAheadDist); + + double rot = getReqAngleVelTowardsTargetPoint(robotPose, goalPoint, robotVel.getH(), pid); + + double linear = pid.getLinearVel(robotPose, goalPoint, robotVel); - //get robot heading between 0 and 360 - double robotHeading = Globals.normalize(robotPose.getH()); + return new double[]{linear, rot}; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/SixWheelPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/SixWheelPID.java new file mode 100644 index 00000000..883f7a72 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/SixWheelPID.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.purePursuit; + +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; +import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; +import org.firstinspires.ftc.teamcode.blucru.common.util.Vector2d; + +public class SixWheelPID { + + PDController xy; + PDController r; + double kXY = 0, dXY = 0; + double kR = 0, dR = 0; + + public SixWheelPID(){ + xy = new PDController(kXY, dXY); + r = new PDController(kR, dR); + } + + public double getLinearVel(Pose2d robotPose, Point2d targetPose, Pose2d robotVel){ + + double robotVelXY = Math.sqrt(robotVel.getX() * robotVel.getX() + robotVel.getY() * robotVel.getY()); + + + double dx = robotPose.getX() - targetPose.getX(); + double dy = robotPose.getY() - targetPose.getY(); + double error = Math.sqrt(dx * dx + dy * dy); + + return xy.calculate(error, -robotVelXY); + } + + + public double getHeadingVel(Pose2d robotPose, Point2d goalPoint, double angleVel){ + double robotHeading = Math.toDegrees(Globals.normalize(robotPose.getH())); + + //get turn req + double turnReq = Math.toDegrees(Globals.normalize(Math.atan2(goalPoint.getX() - robotPose.getX(), goalPoint.getX() - robotPose.getY()))); + + double deltaAngle = Globals.normalize(turnReq - robotHeading); + + //make delta angle be between -180 and 180 + if (deltaAngle > 180){ + deltaAngle -= 180; + } else if (deltaAngle < -180){ + deltaAngle += 180; + } + + return r.calculate(deltaAngle, -angleVel); + } + +} From e3fe12c5c107ae1cbd1f0d1ca4a7e1f525a145e0 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 26 Sep 2025 22:52:13 -0400 Subject: [PATCH 021/123] removed some imports --- .../teamcode/blucru/opmodes/test/TeleDriveToPointTest.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TeleDriveToPointTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TeleDriveToPointTest.java index c2ed79bc..4c1883f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TeleDriveToPointTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TeleDriveToPointTest.java @@ -1,12 +1,8 @@ package org.firstinspires.ftc.teamcode.blucru.opmodes.test; -import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.DrivePID; -import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; @Config From 250a93f6d9c3aaf17dad314a15899973b47b705e Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 26 Sep 2025 22:57:15 -0400 Subject: [PATCH 022/123] progress on 6wd class --- .../sixWheelDrive/SixWheelDrive.java | 52 ++++++++++++++++++- 1 file changed, 50 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java index 3e4c813a..6d7489d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java @@ -1,20 +1,42 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization.RobotLocalizer; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization.Pinpoint; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; public class SixWheelDrive implements BluSubsystem { BluMotor[] dtMotors; - public SixWheelDrive(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ + RobotLocalizer localizer; + + public enum State{ + IDLE, + PID, + TELE_DRIVE + } + + State dtState; + + public SixWheelDrive(){ + this(new BluMotor(Globals.flMotorName), + new BluMotor(Globals.flMotorName), + new BluMotor(Globals.blMotorName), + new BluMotor(Globals.brMotorName)); + } + private SixWheelDrive(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ dtMotors = new BluMotor[]{fl, fr, bl, br}; fr.setDirection(DcMotorSimple.Direction.REVERSE); br.setDirection(DcMotorSimple.Direction.REVERSE); + localizer = new Pinpoint("pinpoint"); + dtState = State.IDLE; } @Override @@ -29,6 +51,7 @@ public void read() { for (BluMotor motors: dtMotors){ motors.read(); } + localizer.read(); } @Override @@ -46,6 +69,31 @@ public void drive(double x, double r){ } } + public void teleDrive(Gamepad g1, double tol){ + double x = -g1.left_stick_y; + double r = g1.left_stick_x; + + if (Math.abs(x) <= tol){ + x = 0; + } + if (Math.abs(r) <= tol){ + r = 0; + } + + if (x == 0 && r == 0){ + if (dtState == State.PID){ + //in pid + } else { + //either stopped driving or idle alr + dtState = State.IDLE; + } + } else { + dtState = State.TELE_DRIVE; + drive(x,r); + } + + } + @Override public void telemetry(Telemetry telemetry) { for (BluMotor motor:dtMotors){ @@ -55,6 +103,6 @@ public void telemetry(Telemetry telemetry) { @Override public void reset() { - + localizer.setPosition(new Pose2d(0,0,0)); } } From 65aa0b5c61e403d69888481a3c6347e4d7be919a Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 26 Sep 2025 23:00:49 -0400 Subject: [PATCH 023/123] refactoring to group all drivetrains under drivetrain package and share the same localization classes --- .../blucru/common/subsytems/Robot.java | 2 +- .../localization/GoBildaPinpointDriver.java | 2 +- .../localization/Pinpoint.java | 2 +- .../localization/PoseHistory.java | 2 +- .../localization/PoseMarker.java | 2 +- .../localization/RobotLocalizer.java | 3 +- .../mecanumDrivetrain/DriveBase.java | 8 +- .../mecanumDrivetrain/Drivetrain.java | 8 +- .../control/DriveKinematics.java | 2 +- .../mecanumDrivetrain/control/DrivePID.java | 2 +- .../control/TurnToPointKinematics.java | 2 +- .../sixWheelDrive/SixWheelDrive.java | 6 +- .../sixWheelDrive/SixWheelKinematics.java | 2 +- .../purePursuit/PurePursuitComputer.java | 2 +- .../purePursuit/SixWheelPID.java | 2 +- .../localization/GoBildaPinpointDriver.java | 738 ------------------ .../localization/RobotLocalizer.java | 18 - .../sixWheelDrive/localization/Pinpoint.java | 126 --- .../blucru/opmodes/BluLinearOpMode.java | 2 +- .../opmodes/test/DriveKinematicsTest.java | 2 +- .../opmodes/test/DrivePIDTunerHeading.java | 2 +- .../blucru/opmodes/test/DrivePIDTunerXY.java | 2 +- 22 files changed, 27 insertions(+), 910 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{sixWheelDrive => drivetrain}/localization/GoBildaPinpointDriver.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{mecanumDrivetrain => drivetrain}/localization/Pinpoint.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{mecanumDrivetrain => drivetrain}/localization/PoseHistory.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{mecanumDrivetrain => drivetrain}/localization/PoseMarker.java (90%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{sixWheelDrive => drivetrain}/localization/RobotLocalizer.java (86%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/mecanumDrivetrain/DriveBase.java (92%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/mecanumDrivetrain/Drivetrain.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/mecanumDrivetrain/control/DriveKinematics.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/mecanumDrivetrain/control/DrivePID.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/mecanumDrivetrain/control/TurnToPointKinematics.java (91%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/sixWheelDrive/SixWheelDrive.java (90%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/sixWheelDrive/SixWheelKinematics.java (79%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/sixWheelDrive/purePursuit/PurePursuitComputer.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => drivetrain}/sixWheelDrive/purePursuit/SixWheelPID.java (94%) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/GoBildaPinpointDriver.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/RobotLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 8b01c2e8..6b270389 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.Drivetrain; import java.util.ArrayList; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/GoBildaPinpointDriver.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/GoBildaPinpointDriver.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/GoBildaPinpointDriver.java index 98b51c3d..f5db5f6f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/GoBildaPinpointDriver.java @@ -20,7 +20,7 @@ * SOFTWARE. */ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization; import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/Pinpoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/Pinpoint.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java index a45b84d9..3ded5afe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/Pinpoint.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/PoseHistory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseHistory.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/PoseHistory.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseHistory.java index f383300b..edaca648 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/PoseHistory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseHistory.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/PoseMarker.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseMarker.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/PoseMarker.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseMarker.java index 843e049a..55a78d46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/PoseMarker.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseMarker.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization; import android.util.Log; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/RobotLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/RobotLocalizer.java similarity index 86% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/RobotLocalizer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/RobotLocalizer.java index 3baf2d10..23685dca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/RobotLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/RobotLocalizer.java @@ -1,10 +1,9 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; public interface RobotLocalizer { - public void read(); public Pose2d getPose(); public double getX(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/DriveBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/DriveBase.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/DriveBase.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/DriveBase.java index 7911c8b8..5f5971ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/DriveBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/DriveBase.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -6,9 +6,9 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.DriveKinematics; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.Pinpoint; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.RobotLocalizer; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control.DriveKinematics; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.Pinpoint; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.RobotLocalizer; import org.firstinspires.ftc.teamcode.blucru.common.util.Alliance; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/Drivetrain.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/Drivetrain.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/Drivetrain.java index 178a2c7e..ab0c0418 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/Drivetrain.java @@ -1,12 +1,12 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain; import com.arcrobotics.ftclib.command.Subsystem; import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.DriveKinematics; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.DrivePID; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.TurnToPointKinematics; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control.DriveKinematics; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control.DrivePID; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control.TurnToPointKinematics; import org.firstinspires.ftc.teamcode.blucru.common.util.Alliance; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/DriveKinematics.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/DriveKinematics.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/DriveKinematics.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/DriveKinematics.java index 3fa07858..2f860171 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/DriveKinematics.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/DriveKinematics.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/DrivePID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/DrivePID.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/DrivePID.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/DrivePID.java index 7a45b39e..04fb01b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/DrivePID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/DrivePID.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.util.Range; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/TurnToPointKinematics.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/TurnToPointKinematics.java similarity index 91% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/TurnToPointKinematics.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/TurnToPointKinematics.java index 4eaf8b2f..58c2a8d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/control/TurnToPointKinematics.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/mecanumDrivetrain/control/TurnToPointKinematics.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Vector2d; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index 6d7489d4..6ca95fa5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; @@ -6,8 +6,8 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization.RobotLocalizer; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization.Pinpoint; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.RobotLocalizer; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.Pinpoint; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelKinematics.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelKinematics.java similarity index 79% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelKinematics.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelKinematics.java index de5d7e19..e42e7a4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/SixWheelKinematics.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelKinematics.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive; public class SixWheelKinematics { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/PurePursuitComputer.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/PurePursuitComputer.java index 1876da3b..eccd53d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/PurePursuitComputer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/PurePursuitComputer.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.purePursuit; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit; import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/SixWheelPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/SixWheelPID.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/SixWheelPID.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/SixWheelPID.java index 883f7a72..6d0bdaf4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/purePursuit/SixWheelPID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/SixWheelPID.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.purePursuit; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/GoBildaPinpointDriver.java deleted file mode 100644 index 64d8cc3b..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/GoBildaPinpointDriver.java +++ /dev/null @@ -1,738 +0,0 @@ -/* MIT License - * Copyright (c) [2025] [Base 10 Assets, LLC] - * - * 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.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization; - -import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; - -import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; -import com.qualcomm.robotcore.hardware.I2cAddr; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; -import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; -import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; -import com.qualcomm.robotcore.util.TypeConversion; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.util.Arrays; - - -@I2cDeviceType -@DeviceProperties( - name = "goBILDA® Pinpoint Odometry Computer", - xmlTag = "goBILDAPinpoint", - description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" -) - -public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { - - private int deviceStatus = 0; - private int loopTime = 0; - private int xEncoderValue = 0; - private int yEncoderValue = 0; - private float xPosition = 0; - private float yPosition = 0; - private float hOrientation = 0; - private float xVelocity = 0; - private float yVelocity = 0; - private float hVelocity = 0; - - private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod - private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod - - //i2c address of the device - public static final byte DEFAULT_ADDRESS = 0x31; - - public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { - super(deviceClient, deviceClientIsOwned); - - this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS)); - super.registerArmingStateCallback(false); - } - - - @Override - public Manufacturer getManufacturer() { - return Manufacturer.Other; - } - - @Override - protected synchronized boolean doInitialize() { - ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K); - return true; - } - - @Override - public String getDeviceName() { - return "goBILDA® Pinpoint Odometry Computer"; - } - - - //Register map of the i2c device - private enum Register { - DEVICE_ID (1), - DEVICE_VERSION (2), - DEVICE_STATUS (3), - DEVICE_CONTROL (4), - LOOP_TIME (5), - X_ENCODER_VALUE (6), - Y_ENCODER_VALUE (7), - X_POSITION (8), - Y_POSITION (9), - H_ORIENTATION (10), - X_VELOCITY (11), - Y_VELOCITY (12), - H_VELOCITY (13), - MM_PER_TICK (14), - X_POD_OFFSET (15), - Y_POD_OFFSET (16), - YAW_SCALAR (17), - BULK_READ (18); - - private final int bVal; - - Register(int bVal){ - this.bVal = bVal; - } - } - - //Device Status enum that captures the current fault condition of the device - public enum DeviceStatus{ - NOT_READY (0), - READY (1), - CALIBRATING (1 << 1), - FAULT_X_POD_NOT_DETECTED (1 << 2), - FAULT_Y_POD_NOT_DETECTED (1 << 3), - FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), - FAULT_IMU_RUNAWAY (1 << 4), - FAULT_BAD_READ (1 << 5); - - private final int status; - - DeviceStatus(int status){ - this.status = status; - } - } - - //enum that captures the direction the encoders are set to - public enum EncoderDirection{ - FORWARD, - REVERSED; - } - - //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used - public enum GoBildaOdometryPods { - goBILDA_SWINGARM_POD, - goBILDA_4_BAR_POD; - } - //enum that captures a limited scope of read data. More options may be added in future update - public enum ReadData { - ONLY_UPDATE_HEADING, - } - - - /** Writes an int to the i2c device - @param reg the register to write the int to - @param i the integer to write to the register - */ - private void writeInt(final Register reg, int i){ - deviceClient.write(reg.bVal, TypeConversion.intToByteArray(i,ByteOrder.LITTLE_ENDIAN)); - } - - /** - * Reads an int from a register of the i2c device - * @param reg the register to read from - * @return returns an int that contains the value stored in the read register - */ - private int readInt(Register reg){ - return byteArrayToInt(deviceClient.read(reg.bVal,4), ByteOrder.LITTLE_ENDIAN); - } - - /** - * Converts a byte array to a float value - * @param byteArray byte array to transform - * @param byteOrder order of byte array to convert - * @return the float value stored by the byte array - */ - private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ - return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); - } - - /** - * Reads a float from a register - * @param reg the register to read - * @return the float value stored in that register - */ - private float readFloat(Register reg){ - return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN); - } - - /** - * Converts a float to a byte array - * @param value the float array to convert - * @return the byte array converted from the float - */ - private byte [] floatToByteArray (float value, ByteOrder byteOrder) { - return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); - } - - /** - * Writes a byte array to a register on the i2c device - * @param reg the register to write to - * @param bytes the byte array to write - */ - private void writeByteArray (Register reg, byte[] bytes){ - deviceClient.write(reg.bVal,bytes); - } - - /** - * Writes a float to a register on the i2c device - * @param reg the register to write to - * @param f the float to write - */ - private void writeFloat (Register reg, float f){ - byte[] bytes = ByteBuffer.allocate(4).order(ByteOrder.LITTLE_ENDIAN).putFloat(f).array(); - deviceClient.write(reg.bVal,bytes); - } - - /** - * Looks up the DeviceStatus enum corresponding with an int value - * @param s int to lookup - * @return the Odometry Computer state - */ - private DeviceStatus lookupStatus (int s){ - if ((s & DeviceStatus.CALIBRATING.status) != 0){ - return DeviceStatus.CALIBRATING; - } - boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; - boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; - - if(!xPodDetected && !yPodDetected){ - return DeviceStatus.FAULT_NO_PODS_DETECTED; - } - if (!xPodDetected){ - return DeviceStatus.FAULT_X_POD_NOT_DETECTED; - } - if (!yPodDetected){ - return DeviceStatus.FAULT_Y_POD_NOT_DETECTED; - } - if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ - return DeviceStatus.FAULT_IMU_RUNAWAY; - } - if ((s & DeviceStatus.READY.status) != 0){ - return DeviceStatus.READY; - } - if ((s & DeviceStatus.FAULT_BAD_READ.status) != 0){ - return DeviceStatus.FAULT_BAD_READ; - } - else { - return DeviceStatus.NOT_READY; - } - } - - /** - * Confirm that the number received is a number, and does not include a change above the threshold - * @param oldValue the reading from the previous cycle - * @param newValue the new reading - * @param threshold the maximum change between this reading and the previous one - * @param bulkUpdate true if we are updating the loopTime variable. If not it should be false. - * @return newValue if the position is good, oldValue otherwise - */ - private Float isPositionCorrupt(float oldValue, float newValue, int threshold, boolean bulkUpdate){ - boolean noData = bulkUpdate && (loopTime < 1); - - boolean isCorrupt = noData || Float.isNaN(newValue) || Math.abs(newValue - oldValue) > threshold; - - if(!isCorrupt){ - return newValue; - } - - deviceStatus = DeviceStatus.FAULT_BAD_READ.status; - return oldValue; - } - - /** - * Confirm that the number received is a number, and does not include a change above the threshold - * @param oldValue the reading from the previous cycle - * @param newValue the new reading - * @param threshold the velocity allowed to be reported - * @return newValue if the velocity is good, oldValue otherwise - */ - private Float isVelocityCorrupt(float oldValue, float newValue, int threshold){ - boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue) > threshold; - boolean noData = (loopTime <= 1); - - if(!isCorrupt){ - return newValue; - } - - deviceStatus = DeviceStatus.FAULT_BAD_READ.status; - return oldValue; - } - - /** - * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. - */ - public void update(){ - final int positionThreshold = 5000; //more than one FTC field in mm - final int headingThreshold = 120; //About 20 full rotations in Radians - final int velocityThreshold = 10000; //10k mm/sec is faster than an FTC robot should be going... - final int headingVelocityThreshold = 120; //About 20 rotations per second - - float oldPosX = xPosition; - float oldPosY = yPosition; - float oldPosH = hOrientation; - float oldVelX = xVelocity; - float oldVelY = yVelocity; - float oldVelH = hVelocity; - - byte[] bArr = deviceClient.read(Register.BULK_READ.bVal, 40); - deviceStatus = byteArrayToInt(Arrays.copyOfRange (bArr, 0, 4), ByteOrder.LITTLE_ENDIAN); - loopTime = byteArrayToInt(Arrays.copyOfRange (bArr, 4, 8), ByteOrder.LITTLE_ENDIAN); - xEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 8, 12), ByteOrder.LITTLE_ENDIAN); - yEncoderValue = byteArrayToInt(Arrays.copyOfRange (bArr, 12,16), ByteOrder.LITTLE_ENDIAN); - xPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 16,20), ByteOrder.LITTLE_ENDIAN); - yPosition = byteArrayToFloat(Arrays.copyOfRange(bArr, 20,24), ByteOrder.LITTLE_ENDIAN); - hOrientation = byteArrayToFloat(Arrays.copyOfRange(bArr, 24,28), ByteOrder.LITTLE_ENDIAN); - xVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN); - yVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN); - hVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN); - - /* - * Check to see if any of the floats we have received from the device are NaN or are too large - * if they are, we return the previously read value and alert the user via the DeviceStatus Enum. - */ - xPosition = isPositionCorrupt(oldPosX, xPosition, positionThreshold, true); - yPosition = isPositionCorrupt(oldPosY, yPosition, positionThreshold, true); - hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, true); - xVelocity = isVelocityCorrupt(oldVelX, xVelocity, velocityThreshold); - yVelocity = isVelocityCorrupt(oldVelY, yVelocity, velocityThreshold); - hVelocity = isVelocityCorrupt(oldVelH, hVelocity, headingVelocityThreshold); - - } - - /** - * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function - * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING - * is supported. - * @param data GoBildaPinpointDriver.ReadData.ONLY_UPDATE_HEADING - */ - public void update(ReadData data) { - if (data == ReadData.ONLY_UPDATE_HEADING) { - final int headingThreshold = 120; - - float oldPosH = hOrientation; - - hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); - - hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, false); - - if (deviceStatus == DeviceStatus.FAULT_BAD_READ.status){ - deviceStatus = DeviceStatus.READY.status; - } - } - } - - /** - * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

- * The most common tracking position is the center of the robot.

- * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
- * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
- * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases - * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - public void setOffsets(double xOffset, double yOffset){ - writeFloat(Register.X_POD_OFFSET, (float) xOffset); - writeFloat(Register.Y_POD_OFFSET, (float) yOffset); - } - - /** - * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

- * The most common tracking position is the center of the robot.

- * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
- * the Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
- * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases - * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases - * @param distanceUnit the unit of distance used for offsets. - */ - public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit){ - writeFloat(Register.X_POD_OFFSET, (float) distanceUnit.toMm(xOffset)); - writeFloat(Register.Y_POD_OFFSET, (float) distanceUnit.toMm(yOffset)); - } - - /** - * Recalibrates the Odometry Computer's internal IMU.

- * Robot MUST be stationary

- * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. - */ - public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} - - /** - * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

- * Robot MUST be stationary

- * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. - */ - public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);} - - /** - * Can reverse the direction of each encoder. - * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward - * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left - */ - public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ - if (xEncoder == EncoderDirection.FORWARD){ - writeInt(Register.DEVICE_CONTROL,1<<5); - } - if (xEncoder == EncoderDirection.REVERSED) { - writeInt(Register.DEVICE_CONTROL,1<<4); - } - - if (yEncoder == EncoderDirection.FORWARD){ - writeInt(Register.DEVICE_CONTROL,1<<3); - } - if (yEncoder == EncoderDirection.REVERSED){ - writeInt(Register.DEVICE_CONTROL,1<<2); - } - } - - /** - * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

- * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD - */ - public void setEncoderResolution(GoBildaOdometryPods pods){ - if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { - writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN))); - } - if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){ - writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN))); - } - } - - /** - * Sets the encoder resolution in ticks per mm of the odometry pods.
- * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. - * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - public void setEncoderResolution(double ticks_per_mm){ - writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Sets the encoder resolution in ticks per mm of the odometry pods.
- * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. - * @param ticks_per_unit should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 - * @param distanceUnit unit used for distance - */ - public void setEncoderResolution(double ticks_per_unit, DistanceUnit distanceUnit){ - double resolution = distanceUnit.toMm(ticks_per_unit); - writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) resolution,ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Tuning this value should be unnecessary.
- * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

- * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

- * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. - * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

- * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com - * @param yawOffset A scalar for the robot's heading. - */ - public void setYawScalar(double yawOffset){ - writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Send a position that the Pinpoint should use to track your robot relative to. You can use this to - * update the estimated position of your robot with new external sensor data, or to run a robot - * in field coordinates.

- * This overrides the current position.

- * Using this feature to track your robot's position in field coordinates:
- * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
- * Say you're on the red alliance, your robot is against the wall and closer to the audience side, - * and the front of your robot is pointing towards the center of the field. - * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always - * keep track of how far away from the center of the field you are.

- * Using this feature to update your position with additional sensors:
- * Some robots have a secondary way to locate their robot on the field. This is commonly - * Apriltag localization in FTC, but it can also be something like a distance sensor. - * Often these external sensors are absolute (meaning they measure something about the field) - * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific - * position on the field to use them. In that case, spend most of your time relying on the Pinpoint - * to determine your location. Then when you pull a new position from your secondary sensor, - * send a setPosition command with the new position. The Pinpoint will then track your movement - * relative to that new, more accurate position. - * @param pos a Pose2D describing the robot's new position. - */ - public Pose2D setPosition(Pose2D pos){ - writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); - writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); - writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); - return pos; - } - - /** - * Send a position that the Pinpoint should use to track your robot relative to. - * You can use this to update the estimated position of your robot with new external - * sensor data, or to run a robot in field coordinates. - * @param posX the new X position you'd like the Pinpoint to track your robot relive to. - * @param distanceUnit the unit for posX - */ - public void setPosX(double posX, DistanceUnit distanceUnit){ - writeByteArray(Register.X_POSITION,(floatToByteArray((float) distanceUnit.toMm(posX), ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Send a position that the Pinpoint should use to track your robot relative to. - * You can use this to update the estimated position of your robot with new external - * sensor data, or to run a robot in field coordinates. - * @param posY the new Y position you'd like the Pinpoint to track your robot relive to. - * @param distanceUnit the unit for posY - */ - public void setPosY(double posY, DistanceUnit distanceUnit){ - writeByteArray(Register.Y_POSITION,(floatToByteArray((float) distanceUnit.toMm(posY), ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Send a heading that the Pinpoint should use to track your robot relative to. - * You can use this to update the estimated position of your robot with new external - * sensor data, or to run a robot in field coordinates. - * @param heading the new heading you'd like the Pinpoint to track your robot relive to. - * @param angleUnit Radians or Degrees - */ - public void setHeading(double heading, AngleUnit angleUnit){ - writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) angleUnit.toRadians(heading), ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Checks the deviceID of the Odometry Computer. Should return 1. - * @return 1 if device is functional. - */ - public int getDeviceID(){return readInt(Register.DEVICE_ID);} - - /** - * @return the firmware version of the Odometry Computer - */ - public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } - - /** - * @return a scalar that the IMU measured heading is multiplied by. This is tuned for each unit - * and should not need adjusted. - */ - public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } - - /** - * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: - * @return one of the following states:
- * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
- * READY - The device is currently functioning as normal. GREEN LED
- * CALIBRATING - The device is currently recalibrating the gyro. RED LED
- * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
- * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
- * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
- * FAULT_BAD_READ - The Java code has detected a bad I²C read, the result reported is a - * duplicate of the last good read. - */ - public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); } - - /** - * Checks the Odometry Computer's most recent loop time.

- * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com - * @return loop time in microseconds (1/1,000,000 seconds) - */ - public int getLoopTime(){return loopTime; } - - /** - * Checks the Odometry Computer's most recent loop frequency.

- * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com - * @return Pinpoint Frequency in Hz (loops per second), - */ - public double getFrequency(){ - if (loopTime != 0){ - return 1000000.0/loopTime; - } - else { - return 0; - } - } - - /** - * @return the raw value of the X (forward) encoder in ticks - */ - public int getEncoderX(){return xEncoderValue; } - - /** - * @return the raw value of the Y (strafe) encoder in ticks - */ - public int getEncoderY(){return yEncoderValue; } - - /** - * @return the estimated X (forward) position of the robot in mm - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - public double getPosX(){ - return xPosition; - } - - /** - * @return the estimated X (forward) position of the robot in specified unit - * @param distanceUnit the unit that the estimated position will return in - */ - public double getPosX(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(xPosition); - } - - /** - * @return the estimated Y (Strafe) position of the robot in mm - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - public double getPosY(){ - return yPosition; - } - - /** - * @return the estimated Y (Strafe) position of the robot in specified unit - * @param distanceUnit the unit that the estimated position will return in - */ - public double getPosY(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(yPosition); - } - - /** - * @return the unnormalized estimated H (heading) position of the robot in radians - * unnormalized heading is not constrained from -180° to 180°. It will continue counting multiple rotations. - * @deprecated two overflows for this function exist with AngleUnit parameter. These minimize the possibility of unit confusion. - */ - public double getHeading(){ - return hOrientation; - } - - /** - * @return the normalized estimated H (heading) position of the robot in specified unit - * normalized heading is wrapped from -180°, to 180°. - */ - public double getHeading(AngleUnit angleUnit){ - return angleUnit.fromRadians((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI; - } - - /** - * @return the unnormalized estimated H (heading) position of the robot in specified unit - * unnormalized heading is not constrained from -180° to 180°. It will continue counting - * multiple rotations. - */ - public double getHeading(UnnormalizedAngleUnit unnormalizedAngleUnit){ - return unnormalizedAngleUnit.fromRadians(hOrientation); - } - - /** - * @return the estimated X (forward) velocity of the robot in mm/sec - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - public double getVelX(){ - return xVelocity; - } - - /** - * @return the estimated X (forward) velocity of the robot in specified unit/sec - */ - public double getVelX(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(xVelocity); - } - - /** - * @return the estimated Y (strafe) velocity of the robot in mm/sec - * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. - */ - public double getVelY(){ - return yVelocity; - } - - /** - * @return the estimated Y (strafe) velocity of the robot in specified unit/sec - */ - public double getVelY(DistanceUnit distanceUnit){ - return distanceUnit.fromMm(yVelocity); - } - - /** - * @return the estimated H (heading) velocity of the robot in radians/sec - * @deprecated The overflow for this function has an AngleUnit, which can reduce the chance of unit confusion. - */ - public double getHeadingVelocity() { - return hVelocity; - } - - /** - * @return the estimated H (heading) velocity of the robot in specified unit/sec - */ - public double getHeadingVelocity(UnnormalizedAngleUnit unnormalizedAngleUnit){ - return unnormalizedAngleUnit.fromRadians(hVelocity); - } - - /** - * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the X (forward) pod in specified unit - */ - public float getXOffset(DistanceUnit distanceUnit){ - return (float) distanceUnit.fromMm(readFloat(Register.X_POD_OFFSET)); - } - - /** - * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the Y (strafe) pod - */ - public float getYOffset(DistanceUnit distanceUnit){ - return (float) distanceUnit.fromMm(readFloat(Register.Y_POD_OFFSET)); - } - - /** - * @return a Pose2D containing the estimated position of the robot - */ - public Pose2D getPosition(){ - return new Pose2D(DistanceUnit.MM, - xPosition, - yPosition, - AngleUnit.RADIANS, - //this wraps the hOrientation variable from -180° to +180° - ((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); - } - - /** - * @deprecated This function is not recommended, as velocity is wrapped from -180° to 180°. - * instead use individual getters. - * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second - */ - public Pose2D getVelocity(){ - return new Pose2D(DistanceUnit.MM, - xVelocity, - yVelocity, - AngleUnit.RADIANS, - ((hVelocity + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/RobotLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/RobotLocalizer.java deleted file mode 100644 index 037f44ad..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/mecanumDrivetrain/localization/RobotLocalizer.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; - -public interface RobotLocalizer { - public void read(); - public Pose2d getPose(); - public double getX(); - public double getY(); - public double getHeading(); - public void setOffset(double x, double y, double h); - public void setPosition(double x, double y, double h); - public void setPosition(Pose2d pose); - public Pose2d getVel(); - public void setHeading(double heading); - public void telemetry(Telemetry telemetry); -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java deleted file mode 100644 index 59a75cb1..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/sixWheelDrive/localization/Pinpoint.java +++ /dev/null @@ -1,126 +0,0 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.sixWheelDrive.localization; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.RobotLocalizer; -import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; -import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; - -public class Pinpoint implements RobotLocalizer { - //TODO TUNE PER ROBOT - public static double parallelYOffset = -144.675, perpXOffset = -70; - org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver pinpoint; - double headingOffset; - - Pose2D pinpointPose; - - public Pinpoint(String name){ - this(Globals.hwMap.get(org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver.class, name)); - } - - public Pinpoint(org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver pinpoint){ - this.pinpoint = pinpoint; - headingOffset = 0; - //TODO UPDATE PER ROBOT - pinpoint.setEncoderDirections( - org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver.EncoderDirection.FORWARD, - org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.localization.GoBildaPinpointDriver.EncoderDirection.REVERSED); - pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); - pinpoint.setOffsets(parallelYOffset, perpXOffset, DistanceUnit.MM); - - pinpoint.resetPosAndIMU(); - pinpointPose = pinpoint.getPosition(); - } - - @Override - public void read() { - pinpoint.update(); - pinpointPose = pinpoint.getPosition(); - } - - /** - * returns pose in x,y,h in inch,inch,radian - * */ - @Override - public Pose2d getPose() { - - //Pose2D is different from Pose2d, Pose2D is ftc's Pose where Pose2d is the custom pose - return new Pose2d(pinpointPose.getX(DistanceUnit.INCH), pinpointPose.getY(DistanceUnit.INCH), pinpointPose.getHeading(AngleUnit.RADIANS)); - } - - /** - * in inches - * */ - @Override - public double getX() { - return pinpointPose.getX(DistanceUnit.INCH); - } - - /** - * in inches - * */ - @Override - public double getY() { - return pinpointPose.getY(DistanceUnit.INCH); - } - - /** - * in radians - * */ - @Override - public double getHeading() { - return pinpointPose.getHeading(AngleUnit.RADIANS) - headingOffset; - } - - /** - * for setting offsets, inch, inch, radian - * */ - @Override - public void setOffset(double x, double y, double h) { - pinpoint.setOffsets(x,y,DistanceUnit.INCH); - headingOffset = h; - } - - /** - * inch, inch, radian - * */ - public void setPosition(double x, double y, double h){ - pinpoint.setPosX(x, DistanceUnit.INCH); - pinpoint.setPosY(y, DistanceUnit.INCH); - pinpoint.setHeading(h, AngleUnit.RADIANS); - read(); - } - - @Override - public void setPosition(Pose2d pose) { - - - Globals.telemetry.addData("Pose", "X: " + pose.getX() + ",Y: " + pose.getY() + ",H: " + pose.getH()); - - pinpoint.setPosX(pose.getX(), DistanceUnit.INCH); - pinpoint.setPosY(pose.getY(), DistanceUnit.INCH); - pinpoint.setHeading(pose.getH(), AngleUnit.RADIANS); - read(); - } - - @Override - public Pose2d getVel() { - return new Pose2d(pinpoint.getVelX(DistanceUnit.INCH), pinpoint.getVelY(DistanceUnit.INCH), pinpoint.getHeadingVelocity(UnnormalizedAngleUnit.RADIANS)); - } - - @Override - public void setHeading(double heading) { - pinpoint.setHeading(heading, AngleUnit.RADIANS); - } - - @Override - public void telemetry(Telemetry telemetry) { - - telemetry.addData("Pinpoint heading", pinpoint.getHeading(AngleUnit.RADIANS)); - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index 60ac7e0d..ee451b53 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -8,7 +8,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.hardware.SinglePressGamepad; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.Drivetrain; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public abstract class BluLinearOpMode extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DriveKinematicsTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DriveKinematicsTest.java index d102d27a..d32a459d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DriveKinematicsTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DriveKinematicsTest.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.DriveKinematics; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control.DriveKinematics; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; @TeleOp diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerHeading.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerHeading.java index 57cecfc3..4a0483ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerHeading.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerHeading.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.DrivePID; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control.DrivePID; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerXY.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerXY.java index d6612932..6ad0d35c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerXY.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/DrivePIDTunerXY.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.control.DrivePID; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.control.DrivePID; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; From e4f4dd2da198933f92b86704062e7d3765f7c12a Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 27 Sep 2025 13:38:42 -0400 Subject: [PATCH 024/123] pid mode added to dt --- .../sixWheelDrive/SixWheelDrive.java | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index 6ca95fa5..f9e17a90 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive; +import android.graphics.Point; + import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; @@ -8,7 +10,10 @@ import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.RobotLocalizer; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.Pinpoint; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.PurePursuitComputer; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.SixWheelPID; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; public class SixWheelDrive implements BluSubsystem { @@ -16,7 +21,10 @@ public class SixWheelDrive implements BluSubsystem { BluMotor[] dtMotors; RobotLocalizer localizer; - + PurePursuitComputer computer; + Point2d[] path; + double lookAheadDist = 10; + SixWheelPID pid; public enum State{ IDLE, PID, @@ -37,6 +45,9 @@ private SixWheelDrive(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ br.setDirection(DcMotorSimple.Direction.REVERSE); localizer = new Pinpoint("pinpoint"); dtState = State.IDLE; + computer = new PurePursuitComputer(); + path = null; + pid = new SixWheelPID(); } @Override @@ -56,6 +67,17 @@ public void read() { @Override public void write() { + + switch(dtState){ + case IDLE: + break; + case PID: + double[] powers = computer.computeRotAndXY(path,localizer.getPose(), localizer.getVel(), lookAheadDist, pid); + drive(powers[0], powers[1]); + case TELE_DRIVE: + break; + } + for (BluMotor motors: dtMotors){ motors.write(); } @@ -94,6 +116,10 @@ public void teleDrive(Gamepad g1, double tol){ } + public void setPath(Point2d[] path){ + this.path = path; + } + @Override public void telemetry(Telemetry telemetry) { for (BluMotor motor:dtMotors){ From 5da784d3c26b43fb8def42a04c9de5892a28ba4c Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 27 Sep 2025 13:44:51 -0400 Subject: [PATCH 025/123] added current detection --- .../ftc/teamcode/blucru/common/hardware/motor/BluMotor.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java index 4152c590..2b2afe6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java @@ -7,12 +7,14 @@ import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.blucru.common.hardware.BluHardwareDevice; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public class BluMotor extends DcMotorImplEx implements BluHardwareDevice { String name; double power=0, lastPower=0; + double current; public BluMotor(String name){ this(name, Direction.FORWARD); } @@ -39,7 +41,7 @@ public void init() { @Override public void read() { - + current = super.getCurrent(CurrentUnit.MILLIAMPS); } @Override @@ -61,6 +63,7 @@ public void telemetry() { public double getPower(){ return power; } + public double getCurrent(){return current;} public double getDcMotorPower(){ return super.getPower(); } From 0ad62f10d2fe44b493c379849f3b1c832cc52c91 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Wed, 1 Oct 2025 17:39:39 -0400 Subject: [PATCH 026/123] added 6wd to path builder --- .../pathing/DrivetrainSetPowerCommand.java | 15 --- .../common/pathing/GoToPointSegment.java | 6 +- .../MecanumDrivetrainSetPowerCommand.java | 15 +++ .../blucru/common/pathing/PIDPath.java | 9 +- .../blucru/common/pathing/PIDPathBuilder.java | 4 +- .../teamcode/blucru/common/pathing/Path.java | 3 +- .../common/pathing/PurePursuitSegment.java | 43 ++++++++ .../SixWheelDrivetrainSetPowerCommand.java | 15 +++ .../pathing/SixWheelPIDPathBuilder.java | 78 ++++++++++++++ .../common/pathing/TurnToPointSegment.java | 9 +- .../blucru/common/subsytems/Robot.java | 17 ++- .../sixWheelDrive/SixWheelDrive.java | 100 +++++------------- .../sixWheelDrive/SixWheelDriveBase.java | 96 +++++++++++++++++ .../teamcode/blucru/common/util/Globals.java | 7 +- .../teamcode/blucru/common/util/Pose2d.java | 4 + .../blucru/opmodes/test/SetPowerTest.java | 11 +- 16 files changed, 319 insertions(+), 113 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/DrivetrainSetPowerCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/MecanumDrivetrainSetPowerCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PurePursuitSegment.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelDrivetrainSetPowerCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelPIDPathBuilder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/DrivetrainSetPowerCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/DrivetrainSetPowerCommand.java deleted file mode 100644 index 8672b111..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/DrivetrainSetPowerCommand.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.firstinspires.ftc.teamcode.blucru.common.pathing; - -import com.arcrobotics.ftclib.command.InstantCommand; - -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; - -public class DrivetrainSetPowerCommand extends InstantCommand { - public DrivetrainSetPowerCommand(double power){ - super(() -> { - Robot.getInstance().drivetrain.setDrivePower(power); - }); - - addRequirements(Robot.getInstance().drivetrain); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/GoToPointSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/GoToPointSegment.java index 397b594d..f028e3e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/GoToPointSegment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/GoToPointSegment.java @@ -34,9 +34,9 @@ public GoToPointSegment(Pose2d targetPose){ public boolean isDone() { boolean velGood = !stopRequiredToEnd || - (Robot.getInstance().drivetrain.vel.vec().getMag() < STOP_VEL); + (Robot.getInstance().mecanumDrivetrain.vel.vec().getMag() < STOP_VEL); - return Robot.getInstance().drivetrain.inRange(xyTol, xyTol * 0.07) + return Robot.getInstance().mecanumDrivetrain.inRange(xyTol, xyTol * 0.07) && velGood; } @@ -60,6 +60,6 @@ public void setTargetPose(Pose2d targetPose) { @Override public void runSegment() { - Robot.getInstance().drivetrain.pidTo(targetPose); + Robot.getInstance().mecanumDrivetrain.pidTo(targetPose); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/MecanumDrivetrainSetPowerCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/MecanumDrivetrainSetPowerCommand.java new file mode 100644 index 00000000..a795a33a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/MecanumDrivetrainSetPowerCommand.java @@ -0,0 +1,15 @@ +package org.firstinspires.ftc.teamcode.blucru.common.pathing; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class MecanumDrivetrainSetPowerCommand extends InstantCommand { + public MecanumDrivetrainSetPowerCommand(double power){ + super(() -> { + Robot.getInstance().mecanumDrivetrain.setDrivePower(power); + }); + + addRequirements(Robot.getInstance().mecanumDrivetrain); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPath.java index cb8320cb..8154a8f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPath.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPath.java @@ -103,11 +103,16 @@ public boolean isDone() { } @Override - public void end() { - Robot.getInstance().drivetrain.switchToIdle(); + public void endMecanum() { + Robot.getInstance().mecanumDrivetrain.switchToIdle(); pathDone = true; } + @Override + public void endSixWheel() { + Robot.getInstance().sixWheelDrivetrain.switchToIdle(); + } + public void telemetry(){ Globals.telemetry.addData("Path done: ", isDone()); Globals.telemetry.addData("Path failed: ", failed()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPathBuilder.java index 294a4fe2..8166f56d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPathBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PIDPathBuilder.java @@ -1,9 +1,7 @@ package org.firstinspires.ftc.teamcode.blucru.common.pathing; import com.arcrobotics.ftclib.command.Command; -import com.arcrobotics.ftclib.command.InstantCommand; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Vector2d; @@ -90,7 +88,7 @@ public PIDPathBuilder schedule(Command command){ } public PIDPathBuilder setPower(double power){ - return schedule(new DrivetrainSetPowerCommand(power)); + return schedule(new MecanumDrivetrainSetPowerCommand(power)); } public PIDPathBuilder callback(Callback callback){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/Path.java index f5fad61d..e186b768 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/Path.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/Path.java @@ -4,5 +4,6 @@ public interface Path { Path start(); public void run(); public boolean isDone(); - public void end(); + public void endMecanum(); + public void endSixWheel(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PurePursuitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PurePursuitSegment.java new file mode 100644 index 00000000..e0545d11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/PurePursuitSegment.java @@ -0,0 +1,43 @@ +package org.firstinspires.ftc.teamcode.blucru.common.pathing; + +import org.firstinspires.ftc.teamcode.R; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; +import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; + +public class PurePursuitSegment implements PathSegment{ + + Point2d[] path; + + double startTime; + double maxTime; + public PurePursuitSegment(Point2d[] path, double maxTime){ + this.path = path; + this.maxTime = maxTime; + } + @Override + public boolean isDone() { + return (Robot.getInstance().sixWheelDrivetrain.getPos(). + getDistTo(new Pose2d(path[path.length-1].getX(), path[path.length-1].getY(), 0))) < 1; + } + + @Override + public void startSegment() { + startTime = System.currentTimeMillis(); + } + + @Override + public boolean failed() { + return System.currentTimeMillis() - startTime > maxTime; + } + + @Override + public Pose2d getPose() { + return Robot.getInstance().sixWheelDrivetrain.getPos(); + } + + @Override + public void runSegment() { + Robot.getInstance().sixWheelDrivetrain.followPath(path); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelDrivetrainSetPowerCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelDrivetrainSetPowerCommand.java new file mode 100644 index 00000000..dac92b4b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelDrivetrainSetPowerCommand.java @@ -0,0 +1,15 @@ +package org.firstinspires.ftc.teamcode.blucru.common.pathing; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class SixWheelDrivetrainSetPowerCommand extends InstantCommand { + public SixWheelDrivetrainSetPowerCommand(double power){ + super(() -> { + Robot.getInstance().sixWheelDrivetrain.setDrivePower(power); + }); + + addRequirements(Robot.getInstance().sixWheelDrivetrain); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelPIDPathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelPIDPathBuilder.java new file mode 100644 index 00000000..43fa5310 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/SixWheelPIDPathBuilder.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.blucru.common.pathing; + + +import com.arcrobotics.ftclib.command.Command; + +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; + +import java.util.ArrayList; +import java.util.HashMap; + +public class SixWheelPIDPathBuilder { + + ArrayList segments; + HashMap> commands; + ArrayList callbacks; + + public SixWheelPIDPathBuilder() { + segments = new ArrayList<>(); + commands = new HashMap<>(); + callbacks = new ArrayList<>(); + } + + public SixWheelPIDPathBuilder addPurePursuitPath(Point2d[] path, double maxTime){ + segments.add(new PurePursuitSegment(path,maxTime)); + return this; + } + + public SixWheelPIDPathBuilder addMappedPurePursuitPath(Point2d[] path, double maxTime){ + for (int i = 0; i new ArrayList()); + commands.get(segments.size()).add(command); + return this; + } + + public SixWheelPIDPathBuilder setPower(double power){ + return schedule(new MecanumDrivetrainSetPowerCommand(power)); + } + + public SixWheelPIDPathBuilder callback(Callback callback){ + while(callbacks.size() <= segments.size()){ + //add a dead callback + callbacks.add(null); + } + callbacks.add(segments.size(), callback); + return this; + } + + /** + *units: milliseconds + */ + public SixWheelPIDPathBuilder waitMilliseconds(double milliseconds){ + //run a wait segment with the last position + segments.add(new WaitSegment(segments.get(segments.size() - 1), milliseconds)); + return this; + } + public PIDPath build(){ + while(callbacks.size() <= segments.size()){ + //fill with null callbacks + callbacks.add(null); + } + return new PIDPath(segments, commands, callbacks); + } + + public Path start(){return this.build().start();} +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/TurnToPointSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/TurnToPointSegment.java index 3a9b9d75..524f32ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/TurnToPointSegment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/pathing/TurnToPointSegment.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.blucru.common.pathing; -import org.firstinspires.ftc.teamcode.R; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Vector2d; @@ -25,10 +24,10 @@ public TurnToPointSegment(Vector2d drivePose, Vector2d turnPose){ @Override public boolean isDone() { boolean velSatisfied = !stopReq || - Robot.getInstance().drivetrain.vel.vec().getMag() < 4.0; + Robot.getInstance().mecanumDrivetrain.vel.vec().getMag() < 4.0; - return Robot.getInstance().drivetrain.inRangeTurnToPoint(drivePose, turnPose, tol, tol*0.1) + return Robot.getInstance().mecanumDrivetrain.inRangeTurnToPoint(drivePose, turnPose, tol, tol*0.1) && velSatisfied; } @@ -44,7 +43,7 @@ public boolean failed() { @Override public Pose2d getPose() { - Pose2d currPose = Robot.getInstance().drivetrain.currPose; + Pose2d currPose = Robot.getInstance().mecanumDrivetrain.currPose; return new Pose2d(currPose.vec(), Math.atan2( turnPose.getY() - currPose.getY(), turnPose.getX() - currPose.getX() @@ -53,6 +52,6 @@ public Pose2d getPose() { @Override public void runSegment() { - Robot.getInstance().drivetrain.pidTurnToPos(drivePose, turnPose); + Robot.getInstance().mecanumDrivetrain.pidTurnToPos(drivePose, turnPose); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 6b270389..30c8024d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -6,6 +6,8 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.SixWheelDrive; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.SixWheelDriveBase; import java.util.ArrayList; import java.util.List; @@ -18,7 +20,8 @@ public class Robot { //list of subsystems static ArrayList subsystems; - public Drivetrain drivetrain; + public Drivetrain mecanumDrivetrain; + public SixWheelDrive sixWheelDrivetrain; private static Robot instance; HardwareMap hwMap; List hubs; @@ -104,9 +107,15 @@ public void clear(){ } public Drivetrain addDrivetrain(){ - drivetrain = new Drivetrain(); - subsystems.add(drivetrain); - return drivetrain; + mecanumDrivetrain = new Drivetrain(); + subsystems.add(mecanumDrivetrain); + return mecanumDrivetrain; + } + + public SixWheelDriveBase addSixWheelDrivetrain(){ + sixWheelDrivetrain = new SixWheelDrive(); + subsystems.add(sixWheelDrivetrain); + return sixWheelDrivetrain; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index f9e17a90..5fcab684 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -1,73 +1,31 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive; -import android.graphics.Point; - -import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.arcrobotics.ftclib.command.Subsystem; import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.RobotLocalizer; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.Pinpoint; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.PurePursuitComputer; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.SixWheelPID; -import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; -public class SixWheelDrive implements BluSubsystem { - - BluMotor[] dtMotors; - - RobotLocalizer localizer; - PurePursuitComputer computer; +public class SixWheelDrive extends SixWheelDriveBase implements Subsystem { + double drivePower; Point2d[] path; - double lookAheadDist = 10; - SixWheelPID pid; - public enum State{ - IDLE, - PID, - TELE_DRIVE - } - - State dtState; - public SixWheelDrive(){ - this(new BluMotor(Globals.flMotorName), - new BluMotor(Globals.flMotorName), - new BluMotor(Globals.blMotorName), - new BluMotor(Globals.brMotorName)); - } - private SixWheelDrive(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ - dtMotors = new BluMotor[]{fl, fr, bl, br}; - fr.setDirection(DcMotorSimple.Direction.REVERSE); - br.setDirection(DcMotorSimple.Direction.REVERSE); - localizer = new Pinpoint("pinpoint"); - dtState = State.IDLE; - computer = new PurePursuitComputer(); + super(); + drivePower = 1; path = null; - pid = new SixWheelPID(); } @Override - public void init() { - for (BluMotor motors: dtMotors){ - motors.init(); - } + public void init(){ + super.init(); } - @Override - public void read() { - for (BluMotor motors: dtMotors){ - motors.read(); - } - localizer.read(); + public void read(){ + super.read(); } - @Override - public void write() { - + public void write(){ switch(dtState){ case IDLE: break; @@ -78,17 +36,7 @@ public void write() { break; } - for (BluMotor motors: dtMotors){ - motors.write(); - } - } - - public void drive(double x, double r){ - double[] powers = SixWheelKinematics.getPowers(x,r); - for(int i = 0; i<4; i+=2){ - dtMotors[i].setPower(powers[0]); - dtMotors[i+1].setPower(powers[1]); - } + super.write(); } public void teleDrive(Gamepad g1, double tol){ @@ -116,19 +64,29 @@ public void teleDrive(Gamepad g1, double tol){ } - public void setPath(Point2d[] path){ + public void setDrivePower(double power){ + this.drivePower = power; + } + public void followPath(Point2d[] path){ this.path = path; + dtState = State.PID; } - @Override - public void telemetry(Telemetry telemetry) { - for (BluMotor motor:dtMotors){ - motor.telemetry(); - } + public void switchToIdle(){ + drive(0,0); + dtState = State.IDLE; + } + + public void setPosition(Pose2d pose){ + localizer.setPosition(pose); + } + + public void setHeading(double heading){ + localizer.setHeading(heading); } @Override - public void reset() { - localizer.setPosition(new Pose2d(0,0,0)); + public void telemetry(Telemetry telemetry) { + super.telemetry(telemetry); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java new file mode 100644 index 00000000..c9a795db --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java @@ -0,0 +1,96 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.RobotLocalizer; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.localization.Pinpoint; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.PurePursuitComputer; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.SixWheelPID; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; + +public class SixWheelDriveBase implements BluSubsystem{ + + BluMotor[] dtMotors; + + RobotLocalizer localizer; + PurePursuitComputer computer; + double lookAheadDist = 10; + SixWheelPID pid; + public enum State{ + IDLE, + PID, + TELE_DRIVE + } + + State dtState; + + public SixWheelDriveBase(){ + this(new BluMotor(Globals.flMotorName), + new BluMotor(Globals.flMotorName), + new BluMotor(Globals.blMotorName), + new BluMotor(Globals.brMotorName)); + } + private SixWheelDriveBase(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ + dtMotors = new BluMotor[]{fl, fr, bl, br}; + fr.setDirection(DcMotorSimple.Direction.REVERSE); + br.setDirection(DcMotorSimple.Direction.REVERSE); + localizer = new Pinpoint("pinpoint"); + dtState = State.IDLE; + computer = new PurePursuitComputer(); + pid = new SixWheelPID(); + } + + @Override + public void init() { + for (BluMotor motors: dtMotors){ + motors.init(); + } + } + + @Override + public void read() { + for (BluMotor motors: dtMotors){ + motors.read(); + } + localizer.read(); + } + + @Override + public void write() { + for (BluMotor motors: dtMotors){ + motors.write(); + } + } + + public void drive(double x, double r){ + double[] powers = SixWheelKinematics.getPowers(x,r); + for(int i = 0; i<4; i+=2){ + dtMotors[i].setPower(powers[0]); + dtMotors[i+1].setPower(powers[1]); + } + } + + public Pose2d getPos(){ + return localizer.getPose(); + } + + @Override + public void telemetry(Telemetry telemetry) { + for (BluMotor motor:dtMotors){ + motor.telemetry(); + } + localizer.telemetry(telemetry); + telemetry.addData("pos", localizer.getPose()); + } + + @Override + public void reset() { + localizer.setPosition(new Pose2d(0,0,0)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java index 41e92ed6..4ba7e1f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java @@ -36,7 +36,6 @@ public class Globals { public static Pose2d mapPose(double x, double y, double heading){ x = x*reflect; - y = y*reflect; if (reflect < 0){ heading += Math.PI; } @@ -54,7 +53,11 @@ public static Pose2d mapPose(Pose2d pose){ return mapPose(pose.getX(), pose.getY(), pose.getH()); } public static Vector2d mapPoint(Vector2d vec){ - return new Vector2d(vec.getX() * reflect, vec.getY()*reflect); + return new Vector2d(vec.getX() * reflect, vec.getY()); + } + + public static Point2d mapPoint(Point2d point){ + return new Point2d(point.getX() * reflect,point.getY()); } public static double getCorrectPower(double power){ return power * 12.0/voltage; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java index 04cede9d..5db88ace 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java @@ -47,5 +47,9 @@ public void set(double x){ this.x = x; } + public double getDistTo(Pose2d pose2d){ + return Math.sqrt(Math.pow(pose2d.getX(),2) + Math.pow(pose2d.getY(),2)); + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SetPowerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SetPowerTest.java index 83b98f35..07bedaf1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SetPowerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SetPowerTest.java @@ -1,9 +1,6 @@ package org.firstinspires.ftc.teamcode.blucru.opmodes.test; -import com.sfdev.assembly.state.StateMachine; -import com.sfdev.assembly.state.StateMachineBuilder; - -import org.firstinspires.ftc.teamcode.blucru.common.pathing.DrivetrainSetPowerCommand; +import org.firstinspires.ftc.teamcode.blucru.common.pathing.MecanumDrivetrainSetPowerCommand; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; @@ -13,16 +10,16 @@ public class SetPowerTest extends BluLinearOpMode { public void initialize(){ addDrivetrain(); drivetrain.setCurrentPose(new Pose2d(0,0,0)); - new DrivetrainSetPowerCommand(0.9).schedule(); + new MecanumDrivetrainSetPowerCommand(0.9).schedule(); } public void periodic(){ drivetrain.teleOpDrive(gamepad1); if (gamepad1.left_bumper){ - new DrivetrainSetPowerCommand(0.5).schedule(); + new MecanumDrivetrainSetPowerCommand(0.5).schedule(); } if (gamepad1.right_bumper){ - new DrivetrainSetPowerCommand(0.9).schedule(); + new MecanumDrivetrainSetPowerCommand(0.9).schedule(); } } From f210c08ab9701ed685797177da68805eeeca25fb Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 1 Oct 2025 17:43:34 -0400 Subject: [PATCH 027/123] partially made intake class --- .../common/hardware/motor/BluMotor.java | 1 + .../hardware/motor/BluMotorWithEncoder.java | 6 +++ .../common/subsytems/Intake/Intake.java | 54 +++++++++++++++++++ 3 files changed, 61 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java index 2b2afe6c..1de2fdde 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java @@ -64,6 +64,7 @@ public double getPower(){ return power; } public double getCurrent(){return current;} + public double getDcMotorPower(){ return super.getPower(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java index 656849e7..94b350e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.blucru.common.hardware.BluHardwareDevice; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; @@ -12,6 +13,7 @@ public class BluMotorWithEncoder extends DcMotorImplEx implements BluHardwareDev String name; double power=0, lastPower=0; double encoderTicks=0, vel=0; + double current = 0; public BluMotorWithEncoder(String name){ this(name, Direction.FORWARD); } @@ -58,6 +60,7 @@ public void init(boolean resetEncoders){ public void read() { encoderTicks = super.getCurrentPosition(); vel = super.getVelocity(); + current = super.getCurrent(CurrentUnit.MILLIAMPS); } @Override @@ -81,6 +84,9 @@ public void telemetry() { public double getPower(){ return power; } + public double getCurrent(){ + return current; + } public double getDcMotorPower(){ return super.getPower(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java new file mode 100644 index 00000000..c17ca63e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java @@ -0,0 +1,54 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake; + +import static org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake.Intake.State.IN; +import static org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake.Intake.State.OUT; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; + +public class Intake implements BluSubsystem { + private BluMotor motor; + public enum State{ + IN, + OUT, + IDlE + } + public State state; + @Override + public void init() { + + } + + @Override + public void read() { + + } + + @Override + public void write() { + switch(state){ + case IN: + motor.setPower(1); + break; + case OUT: + motor.setPower(-1); + break; + case IDlE: + motor.setPower(0); + break; + } + } + + @Override + public void telemetry(Telemetry telemetry) { + + } + + @Override + public void reset() { + + } +} From ff86702bf1a5b91439b85759879e24adcc4cbf30 Mon Sep 17 00:00:00 2001 From: InutilePleb Date: Wed, 1 Oct 2025 17:53:24 -0400 Subject: [PATCH 028/123] Completed Intake Class --- .../common/subsytems/Intake/Intake.java | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java index c17ca63e..dfc73a86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java @@ -11,20 +11,30 @@ public class Intake implements BluSubsystem { private BluMotor motor; + private static final double JAM_CURRENT_THRESHOLD = 2500; // milliamps, adjust as needed public enum State{ IN, OUT, IDlE } public State state; + + public Intake(String motorName) { + motor = new BluMotor(motorName); + state = State.IDlE; + } + @Override public void init() { - + motor.init(); } @Override public void read() { - + motor.read(); + if (state == State.IN && motor.getCurrent() > JAM_CURRENT_THRESHOLD) { + state = State.OUT; // Jam detected, spit out the ball + } } @Override @@ -40,15 +50,17 @@ public void write() { motor.setPower(0); break; } + motor.write(); } @Override public void telemetry(Telemetry telemetry) { - + motor.telemetry(); } @Override public void reset() { - + motor.setPower(0); + motor.write(); } } From ee497abd909cfbf273b1af2a817822802c886dea Mon Sep 17 00:00:00 2001 From: InutilePleb Date: Wed, 1 Oct 2025 18:02:34 -0400 Subject: [PATCH 029/123] Started transfer class --- .../common/subsytems/Transfer/Transfer.java | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java new file mode 100644 index 00000000..81dee602 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java @@ -0,0 +1,61 @@ +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class Transfer implements BluSubsystem { + public BluServo servo1; + public BluServo servo2; + public BluServo servo3; + + public Transfer(String servo1Name, String servo2Name, String servo3Name) { + servo1 = new BluServo(servo1Name); + servo2 = new BluServo(servo2Name); + servo3 = new BluServo(servo3Name); + } + + public Transfer(String servo1Name, BluServo.Direction dir1, + String servo2Name, BluServo.Direction dir2, + String servo3Name, BluServo.Direction dir3) { + servo1 = new BluServo(servo1Name, dir1); + servo2 = new BluServo(servo2Name, dir2); + servo3 = new BluServo(servo3Name, dir3); + } + + @Override + public void init() { + servo1.init(); + servo2.init(); + servo3.init(); + } + + @Override + public void read() { + servo1.read(); + servo2.read(); + servo3.read(); + } + + @Override + public void write() { + servo1.write(); + servo2.write(); + servo3.write(); + } + + @Override + public void telemetry(Telemetry telemetry) { + servo1.telemetry(); + servo2.telemetry(); + servo3.telemetry(); + } + + @Override + public void reset() { + servo1.setPos(0); + servo2.setPos(0); + servo3.setPos(0); + servo1.write(); + servo2.write(); + servo3.write(); + } +} From 8b01a270e6c7515deee86fb987c81566127f3910 Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 4 Oct 2025 12:07:31 -0400 Subject: [PATCH 030/123] Added code for transfer. The transfer class has three objects of the TransferServo class, each with its own positions taken into account when they are set to an angle. You need to take them into account because it is difficult to have each servo be exactly the same at the same positions. --- .../common/subsytems/Transfer/Transfer.java | 61 ------------------ .../subsytems/transfer/LeftTransferServo.java | 18 ++++++ .../transfer/MiddleTransferServo.java | 18 ++++++ .../transfer/RightTransferServo.java | 18 ++++++ .../common/subsytems/transfer/Transfer.java | 64 +++++++++++++++++++ .../subsytems/transfer/TransferServo.java | 15 +++++ 6 files changed, 133 insertions(+), 61 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java deleted file mode 100644 index 81dee602..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Transfer/Transfer.java +++ /dev/null @@ -1,61 +0,0 @@ -import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; -import org.firstinspires.ftc.robotcore.external.Telemetry; - -public class Transfer implements BluSubsystem { - public BluServo servo1; - public BluServo servo2; - public BluServo servo3; - - public Transfer(String servo1Name, String servo2Name, String servo3Name) { - servo1 = new BluServo(servo1Name); - servo2 = new BluServo(servo2Name); - servo3 = new BluServo(servo3Name); - } - - public Transfer(String servo1Name, BluServo.Direction dir1, - String servo2Name, BluServo.Direction dir2, - String servo3Name, BluServo.Direction dir3) { - servo1 = new BluServo(servo1Name, dir1); - servo2 = new BluServo(servo2Name, dir2); - servo3 = new BluServo(servo3Name, dir3); - } - - @Override - public void init() { - servo1.init(); - servo2.init(); - servo3.init(); - } - - @Override - public void read() { - servo1.read(); - servo2.read(); - servo3.read(); - } - - @Override - public void write() { - servo1.write(); - servo2.write(); - servo3.write(); - } - - @Override - public void telemetry(Telemetry telemetry) { - servo1.telemetry(); - servo2.telemetry(); - servo3.telemetry(); - } - - @Override - public void reset() { - servo1.setPos(0); - servo2.setPos(0); - servo3.setPos(0); - servo1.write(); - servo2.write(); - servo3.write(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java new file mode 100644 index 00000000..b0becd43 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +public class LeftTransferServo extends TransferServo{ + public LeftTransferServo(){ + super("transfer left"); + } + + @Override + // TODO: find tick delta for 90 degrees REMEMBER THIS IS +/- ticks of 90 degrees - ticks of 0 degrees / 90 degrees + double getTicksPerDeg() { + return 0; + } + + @Override + double getVerticalPos() { + return 0; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java new file mode 100644 index 00000000..8f52c8c7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +public class MiddleTransferServo extends TransferServo{ + public MiddleTransferServo(){ + super("transfer middle"); + } + + @Override + // TODO: find tick delta for 90 degrees REMEMBER THIS IS +/- ticks of 90 degrees - ticks of 0 degrees / 90 degrees + double getTicksPerDeg() { + return 0; + } + + @Override + double getVerticalPos() { + return 0; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java new file mode 100644 index 00000000..c74a83e2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +public class RightTransferServo extends TransferServo{ + public RightTransferServo(){ + super("transfer right"); + } + + @Override + // TODO: find tick delta for 90 degrees REMEMBER THIS IS +/- ticks of 90 degrees - ticks of 0 degrees / 90 degrees + double getTicksPerDeg() { + return 0; + } + + @Override + double getVerticalPos() { + return 0; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java new file mode 100644 index 00000000..e484b3ca --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class Transfer implements BluSubsystem { + + TransferServo[] transferServos; + public enum State { + DOWN, + UP + } + public State state; + public Transfer(HardwareMap hardwareMap) { + transferServos = new TransferServo[]{new LeftTransferServo(), new MiddleTransferServo(), new RightTransferServo()}; + } + + public void down(){ + + } + + @Override + public void init() { + for(TransferServo servo : transferServos){ + servo.init(); + } + } + + @Override + public void read() { + for(TransferServo servo : transferServos){ + servo.read(); + } + } + + @Override + public void write() { + for(TransferServo servo : transferServos){ + servo.write(); + } + + } + + @Override + public void telemetry(Telemetry telemetry) { + telemetry.addData("Transfer state: ", state); + for(TransferServo servo : transferServos){ + servo.telemetry(); + } + } + + @Override + public void reset() { + + } + public void setAngle(double degrees){ + for(TransferServo servo:transferServos){ + servo.setAngle(degrees); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java new file mode 100644 index 00000000..07c2cd06 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java @@ -0,0 +1,15 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; + +public abstract class TransferServo extends BluServo { + public TransferServo(String name) { + super(name); + } + void setAngle(double degrees){ + double pos = (degrees - 90.0)*getTicksPerDeg() + getVerticalPos(); + setPosition(pos); + } + abstract double getTicksPerDeg(); + abstract double getVerticalPos(); +} From c35616b3874674563f4cae18ebf931b8585fd185 Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 4 Oct 2025 12:18:03 -0400 Subject: [PATCH 031/123] Update Transfer.java --- .../blucru/common/subsytems/transfer/Transfer.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java index e484b3ca..cf3e7ca4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java @@ -19,7 +19,13 @@ public Transfer(HardwareMap hardwareMap) { } public void down(){ + setAngle(-10); //TODO: find correct angle, -10 degrees is just a random number + state = State.DOWN; + } + public void up(){ + setAngle(30); //TODO: find correct angle, 30 degrees is just a random number + state = State.UP; } @Override @@ -54,7 +60,7 @@ public void telemetry(Telemetry telemetry) { @Override public void reset() { - + down(); } public void setAngle(double degrees){ for(TransferServo servo:transferServos){ From 152a4c9f63415b528f1709bdd3bf569d68aecef5 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 5 Oct 2025 09:23:47 -0400 Subject: [PATCH 032/123] turret now uses 2 servos --- .../blucru/common/subsytems/Robot.java | 4 ++- .../common/subsytems/turret/Turret.java | 34 +++++++++++++------ .../teamcode/blucru/common/util/Globals.java | 3 ++ 3 files changed, 30 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index a2228975..df3ec4cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -6,8 +6,10 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluPIDServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.Turret; +import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; import java.util.ArrayList; import java.util.List; @@ -113,7 +115,7 @@ public Drivetrain addDrivetrain(){ } public Turret addTurret(){ - turret = new Turret(new BluMotorWithEncoder("turret")); + turret = new Turret(new BluPIDServo("servo1", new PDController(0,0)), new BluPIDServo("servo2", new PDController(0,0))); subsystems.add(turret); return turret; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java index 575b1ac7..73fabf7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java @@ -1,13 +1,18 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluPIDServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public class Turret implements BluSubsystem { - BluMotorWithEncoder turret; - public Turret(BluMotorWithEncoder turret){ - this.turret = turret; + BluPIDServo servo1; + BluPIDServo servo2; + public Turret(BluPIDServo servo1, BluPIDServo servo2){ + this.servo1 = servo1; + this.servo2 = servo2; } private enum State{ @@ -19,30 +24,39 @@ private enum State{ @Override public void init() { - turret.init(); + servo1.init(); + servo2.init(); } @Override public void read() { - turret.read(); + servo1.read(); + servo2.read(); } @Override public void write() { - turret.write(); + servo1.write(); + servo2.write(); } - public void moveWithPower(double power){ - turret.setPower(power); + public void moveToPos(double pos){ + servo1.setPosition(pos); + servo2.setPosition(1-pos); + } + + public void moveToAngle(double angle){ + double servoPos = Globals.convertServoPosToAngle(255,angle); + moveToPos(servoPos); } @Override public void telemetry(Telemetry telemetry) { - turret.telemetry(); + servo1.telemetry(); + servo2.telemetry(); } @Override public void reset() { - turret.reset(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java index 41e92ed6..ae9eed33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java @@ -76,6 +76,9 @@ public static double normalize(double angle) { public static double convertServoPosToAngle(double range, double servoPos){ return (servoPos - 0.5) * range; } + public static double convertAngleToServoPos(double range, double angle){ + return angle/range + 0.5; + } public static double convertMotorPositionToRotations(double rpm, double pos){ From 32f933cb910ccc4891fba946cc21b2e17d2fdfdc Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 5 Oct 2025 10:17:32 -0400 Subject: [PATCH 033/123] shooterTest now uses commands, made shooter implement subsystem and added method to get hood angle --- .../common/subsytems/shooter/Shooter.java | 8 ++++- .../shooterCommands/SetHoodAngleCommand.java | 18 ++++++++++ .../TurnOffShooterCommand.java | 14 ++++++++ .../shooterCommands/TurnOnShooterCommand.java | 16 +++++++++ .../blucru/opmodes/test/ShooterTest.java | 33 ++++++++++++++++--- 5 files changed, 83 insertions(+), 6 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/SetHoodAngleCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOffShooterCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index ceb210f3..0fd6e70a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -1,12 +1,14 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; +import com.arcrobotics.ftclib.command.Subsystem; + import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; -public class Shooter implements BluSubsystem { +public class Shooter implements BluSubsystem, Subsystem { BluMotorWithEncoder shooter; BluServo hood; @@ -46,6 +48,10 @@ public void setHoodServoPos(double pos){ hood.setPos(pos); } + public double getHoodAngle(){ + return Globals.convertServoPosToAngle(255,hood.getPos()); + } + @Override public void telemetry(Telemetry telemetry) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/SetHoodAngleCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/SetHoodAngleCommand.java new file mode 100644 index 00000000..7e0c4d1c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/SetHoodAngleCommand.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class SetHoodAngleCommand extends InstantCommand { + + public SetHoodAngleCommand(double hoodAngle){ + + super(() -> { + Robot.getInstance().shooter.setHoodAngle(hoodAngle); + }); + + addRequirements(Robot.getInstance().shooter); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOffShooterCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOffShooterCommand.java new file mode 100644 index 00000000..905743ae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOffShooterCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class TurnOffShooterCommand extends InstantCommand { + public TurnOffShooterCommand(){ + super (() -> Robot.getInstance().shooter.shoot(0)); + + addRequirements(Robot.getInstance().shooter); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java new file mode 100644 index 00000000..7f3f58c9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class TurnOnShooterCommand extends InstantCommand { + + public TurnOnShooterCommand(double power) { + super(() -> { + Robot.getInstance().shooter.shoot(1); + }); + + addRequirements(Robot.getInstance().shooter); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java index c4fcc9de..4c5b1c96 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java @@ -1,22 +1,45 @@ package org.firstinspires.ftc.teamcode.blucru.opmodes.test; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.sfdev.assembly.state.StateMachine; +import com.sfdev.assembly.state.StateMachineBuilder; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.SetHoodAngleCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.TurnOffShooterCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.TurnOnShooterCommand; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; @TeleOp(group = "test") public class ShooterTest extends BluLinearOpMode { + enum State{ + IDLE, + SHOOTING + } + StateMachine sm; double hoodAngle; public void initialize(){ + sm = new StateMachineBuilder() + .state(State.IDLE) + .loop( + () -> { + new SetHoodAngleCommand(gamepad1.left_stick_y * 2 + Robot.getInstance().shooter.getHoodAngle()).schedule(); + } + ) + .transition(() -> gamepad1.left_bumper, State.SHOOTING, () -> { + new TurnOnShooterCommand(1).schedule(); + }) + .transition(() -> gamepad1.right_bumper, State.IDLE, () -> { + new TurnOffShooterCommand().schedule(); + }) + .build(); + robot.clear(); addShooter(); - hoodAngle = 0.5; + shooter.setHoodAngle(0); } public void periodic(){ - shooter.shoot(-gamepad1.left_stick_y); - - hoodAngle += -gamepad1.right_stick_y * 2.0; - shooter.setHoodAngle(hoodAngle); + sm.update(); } } From 7be359dd62250daf04f58e1cba38a39499c1a0ad Mon Sep 17 00:00:00 2001 From: InutilePleb Date: Sun, 5 Oct 2025 11:49:02 -0400 Subject: [PATCH 034/123] made transfer and intake implement subsystem --- .../ftc/teamcode/blucru/common/subsytems/Intake/Intake.java | 3 ++- .../teamcode/blucru/common/subsytems/transfer/Transfer.java | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java index dfc73a86..c31d88c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java @@ -3,13 +3,14 @@ import static org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake.Intake.State.IN; import static org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake.Intake.State.OUT; +import com.arcrobotics.ftclib.command.Subsystem; import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; -public class Intake implements BluSubsystem { +public class Intake implements BluSubsystem, Subsystem { private BluMotor motor; private static final double JAM_CURRENT_THRESHOLD = 2500; // milliamps, adjust as needed public enum State{ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java index cf3e7ca4..45423d16 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java @@ -1,12 +1,13 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +import com.arcrobotics.ftclib.command.Subsystem; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; import org.firstinspires.ftc.robotcore.external.Telemetry; -public class Transfer implements BluSubsystem { +public class Transfer implements BluSubsystem, Subsystem { TransferServo[] transferServos; public enum State { From 8249e75b8367a73a9ce02898c36d2d6c3ed17844 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 5 Oct 2025 13:35:57 -0400 Subject: [PATCH 035/123] private vars for shooter and it uses rampDownShooter --- .../teamcode/blucru/common/subsytems/shooter/Shooter.java | 6 +++--- .../subsytems/shooterCommands/TurnOnShooterCommand.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index 0fd6e70a..bf5e8f63 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -10,8 +10,8 @@ public class Shooter implements BluSubsystem, Subsystem { - BluMotorWithEncoder shooter; - BluServo hood; + private BluMotorWithEncoder shooter; + private BluServo hood; public Shooter(){ shooter = new BluMotorWithEncoder("shooter"); @@ -39,7 +39,7 @@ public void shoot(double power){ } public void rampDownShooter(){ - shooter.setPower(0.5); + shooter.setPower(0); } public void setHoodAngle(double angle){ hood.setPos(Globals.convertAngleToServoPos(255, angle)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java index 7f3f58c9..61d16d38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java @@ -8,7 +8,7 @@ public class TurnOnShooterCommand extends InstantCommand { public TurnOnShooterCommand(double power) { super(() -> { - Robot.getInstance().shooter.shoot(1); + Robot.getInstance().shooter.rampDownShooter(); }); addRequirements(Robot.getInstance().shooter); From e9e81ca533d4d330ba515bf6c9e2c1cb1d43b0aa Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 5 Oct 2025 13:40:11 -0400 Subject: [PATCH 036/123] made stuff private --- .../subsytems/drivetrain/localization/Pinpoint.java | 6 +++--- .../drivetrain/localization/PoseHistory.java | 4 ++-- .../drivetrain/sixWheelDrive/SixWheelDrive.java | 13 ++++++++++--- .../drivetrain/sixWheelDrive/SixWheelDriveBase.java | 7 +------ .../purePursuit/PurePursuitComputer.java | 2 +- .../sixWheelDrive/purePursuit/SixWheelPID.java | 8 ++++---- .../blucru/opmodes/test/PIDPathBuilderTest.java | 2 +- 7 files changed, 22 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java index 3ded5afe..529d6cf2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java @@ -11,10 +11,10 @@ public class Pinpoint implements RobotLocalizer{ //TODO TUNE PER ROBOT public static double parallelYOffset = -144.675, perpXOffset = -70; - GoBildaPinpointDriver pinpoint; - double headingOffset; + private GoBildaPinpointDriver pinpoint; + private double headingOffset; - Pose2D pinpointPose; + private Pose2D pinpointPose; public Pinpoint(String name){ this(Globals.hwMap.get(GoBildaPinpointDriver.class, name)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseHistory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseHistory.java index edaca648..eb266302 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseHistory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/PoseHistory.java @@ -6,9 +6,9 @@ public class PoseHistory { - static double STORAGE_NANOSECONDS = Math.pow(10,9); + private static double STORAGE_NANOSECONDS = Math.pow(10,9); - LinkedList poseList; + private LinkedList poseList; public PoseHistory(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index 5fcab684..120d48be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -4,16 +4,23 @@ import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.PurePursuitComputer; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.SixWheelPID; import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; public class SixWheelDrive extends SixWheelDriveBase implements Subsystem { - double drivePower; - Point2d[] path; + private double drivePower; + private Point2d[] path; + private PurePursuitComputer computer; + private final double LOOK_AHEAD_DIST = 10; + private SixWheelPID pid; public SixWheelDrive(){ super(); drivePower = 1; path = null; + computer = new PurePursuitComputer(); + pid = new SixWheelPID(); } @Override @@ -30,7 +37,7 @@ public void write(){ case IDLE: break; case PID: - double[] powers = computer.computeRotAndXY(path,localizer.getPose(), localizer.getVel(), lookAheadDist, pid); + double[] powers = computer.computeRotAndXY(path,localizer.getPose(), localizer.getVel(), LOOK_AHEAD_DIST, pid); drive(powers[0], powers[1]); case TELE_DRIVE: break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java index c9a795db..b6c4cd09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java @@ -16,12 +16,9 @@ public class SixWheelDriveBase implements BluSubsystem{ - BluMotor[] dtMotors; + private BluMotor[] dtMotors; RobotLocalizer localizer; - PurePursuitComputer computer; - double lookAheadDist = 10; - SixWheelPID pid; public enum State{ IDLE, PID, @@ -42,8 +39,6 @@ private SixWheelDriveBase(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ br.setDirection(DcMotorSimple.Direction.REVERSE); localizer = new Pinpoint("pinpoint"); dtState = State.IDLE; - computer = new PurePursuitComputer(); - pid = new SixWheelPID(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/PurePursuitComputer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/PurePursuitComputer.java index eccd53d6..bd2c36eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/PurePursuitComputer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/PurePursuitComputer.java @@ -9,7 +9,7 @@ * Guide from https://wiki.purduesigbots.com/software/control-algorithms/basic-pure-pursuit * */ public class PurePursuitComputer { - int lastFoundIndex; + private int lastFoundIndex; public PurePursuitComputer(){ lastFoundIndex = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/SixWheelPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/SixWheelPID.java index 6d0bdaf4..a7f54f1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/SixWheelPID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/purePursuit/SixWheelPID.java @@ -8,10 +8,10 @@ public class SixWheelPID { - PDController xy; - PDController r; - double kXY = 0, dXY = 0; - double kR = 0, dR = 0; + private PDController xy; + private PDController r; + private double kXY = 0, dXY = 0; + private double kR = 0, dR = 0; public SixWheelPID(){ xy = new PDController(kXY, dXY); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PIDPathBuilderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PIDPathBuilderTest.java index 9052f187..5d05413b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PIDPathBuilderTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PIDPathBuilderTest.java @@ -35,7 +35,7 @@ public void initialize(){ .state(States.RUNNING_PATH) .transition(() -> currentPath.isDone(), States.IDLE, () -> { drivetrain.switchToIdle(); - currentPath.end(); + currentPath.endMecanum(); }) .loop(() -> currentPath.run()) .build(); From 1e58daa80738b96e98a4983c4869b61fb56e6a97 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 5 Oct 2025 13:41:29 -0400 Subject: [PATCH 037/123] fixed lastFoundIndex issue --- .../common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index 120d48be..fc58d3e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -76,6 +76,7 @@ public void setDrivePower(double power){ } public void followPath(Point2d[] path){ this.path = path; + computer.resetLastFoundIndex(); dtState = State.PID; } From 6266c350ef72f87284c6e88dc940a1d7785013bc Mon Sep 17 00:00:00 2001 From: InutilePleb Date: Mon, 6 Oct 2025 20:00:05 -0400 Subject: [PATCH 038/123] revisions on intake and transfer classes: made state private, made setters and getters for the states. --- .../blucru/common/subsytems/Intake/Intake.java | 18 +++++++++++++++++- .../common/subsytems/transfer/Transfer.java | 16 ++++++++++------ 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java index c31d88c5..0fe1e12c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java @@ -18,7 +18,23 @@ public enum State{ OUT, IDlE } - public State state; + private State state; + + public void setIn() { + state = State.IN; + } + + public void setOut() { + state = State.OUT; + } + + public void setIdle() { + state = State.IDlE; + } + + public State getState() { + return state; + } public Intake(String motorName) { motor = new BluMotor(motorName); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java index 45423d16..8750800f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java @@ -14,19 +14,23 @@ public enum State { DOWN, UP } - public State state; + private State state; public Transfer(HardwareMap hardwareMap) { transferServos = new TransferServo[]{new LeftTransferServo(), new MiddleTransferServo(), new RightTransferServo()}; } - public void down(){ - setAngle(-10); //TODO: find correct angle, -10 degrees is just a random number + public void setDown() { state = State.DOWN; + setAngle(-10); //TODO: find correct angle, -10 degrees is just a random number } - public void up(){ - setAngle(30); //TODO: find correct angle, 30 degrees is just a random number + public void setUp() { state = State.UP; + setAngle(30); //TODO: find correct angle, 30 degrees is just a random number + } + + public State getState() { + return state; } @Override @@ -61,7 +65,7 @@ public void telemetry(Telemetry telemetry) { @Override public void reset() { - down(); + setDown(); } public void setAngle(double degrees){ for(TransferServo servo:transferServos){ From 31d56bd06d2509ef846a21230d7a6e05a979a658 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sun, 12 Oct 2025 17:16:14 -0400 Subject: [PATCH 039/123] shooter velocity pid and commands working --- .../common/subsytems/shooter/Shooter.java | 44 ++++++++++++++++++- .../subsytems/shooter/ShooterVelocityPID.java | 37 ++++++++++++++++ .../ShootWithVelocityCommand.java | 13 ++++++ .../shooterCommands/TurnOnShooterCommand.java | 2 +- .../blucru/opmodes/test/ShooterPIDTuning.java | 44 +++++++++++++++++++ .../blucru/opmodes/test/ShooterTest.java | 27 +++++++++++- 6 files changed, 163 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/ShootWithVelocityCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index bf5e8f63..6e64b477 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -1,21 +1,36 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.command.Subsystem; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; - +@Config public class Shooter implements BluSubsystem, Subsystem { + public static double p = 0, d = 0; + public static double limit = 0.00005; + private BluMotorWithEncoder shooter; private BluServo hood; + enum State{ + IDLE, + VELOCITY + } + private State state; + double targetVel = 0; + ShooterVelocityPID pid; public Shooter(){ shooter = new BluMotorWithEncoder("shooter"); hood = new BluServo("hood"); + state = State.IDLE; + pid = new ShooterVelocityPID(p,d); } @Override @@ -30,6 +45,17 @@ public void read() { @Override public void write() { + switch (state){ + case IDLE: + targetVel = 0; + break; + case VELOCITY: + if (Math.abs(shooter.getVel()- targetVel) > limit){ + shooter.setPower(shooter.getPower() + pid.calculateDeltaPower(shooter.getVel(), targetVel)); + Globals.telemetry.addData("delta", pid.calculateDeltaPower(shooter.getVel(), targetVel)); + } + } + shooter.write(); hood.write(); } @@ -38,6 +64,18 @@ public void shoot(double power){ shooter.setPower(power); } + public void shootWithVelocity(double vel){ + targetVel = vel; + state = State.VELOCITY; + } + + public double getVel(){ + return shooter.getVel(); + } + public double getPower(){ + return shooter.getPower(); + } + public void rampDownShooter(){ shooter.setPower(0); } @@ -64,4 +102,8 @@ public void reset() { shooter.reset(); shooter.read(); } + + public void updatePID(){ + pid.setPD(p,d); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java new file mode 100644 index 00000000..c593512a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; + +import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; + +public class ShooterVelocityPID{ + PDController controller; + double currAccel; + public ShooterVelocityPID(PDController controller){ + this.controller = controller; + currAccel = 0; + } + + public ShooterVelocityPID(double p, double d){ + this(new PDController(p,d)); + } + + public double calculateDeltaPower(double currVel, double targetVel){ + + //this should keep currAccel always updated + currAccel = controller.calculate(currVel, targetVel, currAccel ,0); + + return currAccel; + } + + public void setP(double p){ + controller.setP(p); + } + + public void setD(double d){ + controller.setD(d); + } + + public void setPD(double p, double d){ + controller.setPD(p,d); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/ShootWithVelocityCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/ShootWithVelocityCommand.java new file mode 100644 index 00000000..5170557f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/ShootWithVelocityCommand.java @@ -0,0 +1,13 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class ShootWithVelocityCommand extends InstantCommand { + public ShootWithVelocityCommand(double vel){ + super (() -> Robot.getInstance().shooter.shootWithVelocity(vel)); + + addRequirements(Robot.getInstance().shooter); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java index 61d16d38..4ce884a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java @@ -8,7 +8,7 @@ public class TurnOnShooterCommand extends InstantCommand { public TurnOnShooterCommand(double power) { super(() -> { - Robot.getInstance().shooter.rampDownShooter(); + Robot.getInstance().shooter.shoot(power); }); addRequirements(Robot.getInstance().shooter); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java new file mode 100644 index 00000000..660a59be --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.Shooter; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.ShootWithVelocityCommand; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +@TeleOp +public class ShooterPIDTuning extends BluLinearOpMode { + + public void initialize(){ + robot.clear(); + addShooter(); + Globals.multiTelemetry = new MultipleTelemetry(telemetry + , FtcDashboard.getInstance().getTelemetry()); + } + + public void periodic(){ + if (gamepad1.a){ + new ShootWithVelocityCommand(2500).schedule(); + } + + if (gamepad1.b){ + new ShootWithVelocityCommand(0).schedule(); + } + + if (gamepad1.x){ + shooter.updatePID(); + telemetry.addLine("updated"); + telemetry.addData("New PID Constants", Shooter.p + ", " + Shooter.d); + } + } + + public void telemetry(){ + Globals.multiTelemetry.addData("shooter vel", shooter.getVel()); + telemetry.addData("Shooter power", shooter.getPower()); + telemetry.addData("g1x", gamepad1.x); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java index 4c5b1c96..62973e7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java @@ -9,6 +9,9 @@ import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.TurnOffShooterCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.TurnOnShooterCommand; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +import java.net.IDN; + @TeleOp(group = "test") public class ShooterTest extends BluLinearOpMode { enum State{ @@ -23,16 +26,24 @@ public void initialize(){ .loop( () -> { new SetHoodAngleCommand(gamepad1.left_stick_y * 2 + Robot.getInstance().shooter.getHoodAngle()).schedule(); + telemetry.addLine("here"); } ) - .transition(() -> gamepad1.left_bumper, State.SHOOTING, () -> { + .transition(() -> driver1.pressedLeftBumper(), State.SHOOTING, () -> { new TurnOnShooterCommand(1).schedule(); }) - .transition(() -> gamepad1.right_bumper, State.IDLE, () -> { + .state(State.SHOOTING) + .loop(() ->{ + telemetry.addLine("here1"); + }) + .transition(() -> driver1.pressedLeftBumper(), State.IDLE, () -> { new TurnOffShooterCommand().schedule(); + telemetry.addLine("here2"); }) .build(); + sm.setState(State.IDLE); + sm.start(); robot.clear(); addShooter(); shooter.setHoodAngle(0); @@ -40,6 +51,18 @@ public void initialize(){ public void periodic(){ sm.update(); + if (gamepad1.a){ + shooter.setHoodAngle(0); + } + if (gamepad1.b){ + new TurnOnShooterCommand(1).schedule(); + } + } + + public void telemetry(){ + telemetry.addData("Shooter angle", shooter.getHoodAngle()); + telemetry.addData("G1LSTY", gamepad1.left_stick_y); + telemetry.addData("State", sm.getState()); } } From a7992c5c546ad8c3974f7db1fc8970096666571b Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 17 Oct 2025 23:01:40 -0400 Subject: [PATCH 040/123] set some values --- .../teamcode/blucru/common/subsytems/shooter/Shooter.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index 6e64b477..ac666813 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -13,8 +13,8 @@ @Config public class Shooter implements BluSubsystem, Subsystem { - public static double p = 0, d = 0; - public static double limit = 0.00005; + public static double p = 0.2, d = 0.1; + public static double limit = 20; private BluMotorWithEncoder shooter; private BluServo hood; @@ -50,7 +50,7 @@ public void write() { targetVel = 0; break; case VELOCITY: - if (Math.abs(shooter.getVel()- targetVel) > limit){ + if (Math.abs(shooter.getVel()- targetVel) >= limit){ shooter.setPower(shooter.getPower() + pid.calculateDeltaPower(shooter.getVel(), targetVel)); Globals.telemetry.addData("delta", pid.calculateDeltaPower(shooter.getVel(), targetVel)); } From a7e84d34a0d160daa65ab4e207b184c541c59993 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 17 Oct 2025 23:03:32 -0400 Subject: [PATCH 041/123] init turret code --- .../common/hardware/servo/BluCRServo.java | 2 +- .../blucru/common/subsytems/Robot.java | 5 +- .../common/subsytems/turret/Turret.java | 84 +++++++++++++------ .../common/subsytems/turret/TurretServos.java | 54 ++++++++++++ .../TurnTurretToPosCommand.java | 17 ++++ .../blucru/opmodes/test/TurretTest.java | 2 +- 6 files changed, 136 insertions(+), 28 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/TurretServos.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluCRServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluCRServo.java index ee302bc7..351107cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluCRServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluCRServo.java @@ -25,7 +25,7 @@ private BluCRServo(CRServo servo, String name, Direction direction){ this.lastPower = 0; } - public void setPower(){ + public void setPower(double power){ this.power = Range.clip(power, -1, 1); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index df3ec4cf..3cb75f75 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -5,10 +5,13 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluEncoder; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluCRServo; import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluPIDServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.Turret; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; import java.util.ArrayList; @@ -115,7 +118,7 @@ public Drivetrain addDrivetrain(){ } public Turret addTurret(){ - turret = new Turret(new BluPIDServo("servo1", new PDController(0,0)), new BluPIDServo("servo2", new PDController(0,0))); + turret = new Turret(new BluCRServo("servoLeft"), new BluCRServo("servoRight"), new BluEncoder(Globals.flMotorName), new PDController(0,0)); subsystems.add(turret); return turret; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java index 73fabf7b..b0ef8d95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java @@ -1,59 +1,93 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret; +import com.arcrobotics.ftclib.command.Subsystem; + import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluEncoder; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluCRServo; import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluPIDServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; +import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; -public class Turret implements BluSubsystem { - BluPIDServo servo1; - BluPIDServo servo2; - public Turret(BluPIDServo servo1, BluPIDServo servo2){ - this.servo1 = servo1; - this.servo2 = servo2; - } - +public class Turret implements BluSubsystem, Subsystem { + private TurretServos servos; + private BluEncoder encoder; + private PDController controller; + private double position; + private final double TICKS_PER_REV = 8192; + Pose2d goalPose; private enum State{ - POWER_MODE, - PID_MODE, - GOAL_LOCK_MODE; + IDLE, + PID + + } + private State state; + public Turret(BluCRServo servoLeft, BluCRServo servoRight, BluEncoder encoder, PDController controller){ + servos = new TurretServos(servoLeft, servoRight); + this.encoder = encoder; + this.controller = controller; + state = State.IDLE; } + + @Override public void init() { - servo1.init(); - servo2.init(); + servos.init(); + encoder.init(); } @Override public void read() { - servo1.read(); - servo2.read(); + servos.read(); + encoder.read(); } @Override public void write() { - servo1.write(); - servo2.write(); + + switch(state){ + case IDLE: + break; + case PID: + servos.setPower(controller.calculate(encoder.getCurrentPos(), position, servos.getPower(), 0)); + break; + } + + servos.write(); + encoder.write(); + } + + public void setAngle(double angle){ + this.position = angle / 360 * TICKS_PER_REV; + state = State.PID; } - public void moveToPos(double pos){ - servo1.setPosition(pos); - servo2.setPosition(1-pos); + public void setPower(double power){ + servos.setPower(power); + state = State.IDLE; } - public void moveToAngle(double angle){ - double servoPos = Globals.convertServoPosToAngle(255,angle); - moveToPos(servoPos); + public void setFieldCentricPosition(double position, double robotHeading){ + setAngle(position - robotHeading); } + public void turnToPointTowardsGoal(Pose2d robotPose){ + double targetAngle = goalPose.vec().subtractNotInPlace(robotPose.vec()).getHeading(); + + setFieldCentricPosition(targetAngle, robotPose.getH()); + } + + @Override public void telemetry(Telemetry telemetry) { - servo1.telemetry(); - servo2.telemetry(); + servos.telemetry(); + encoder.telemetry(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/TurretServos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/TurretServos.java new file mode 100644 index 00000000..7e8b4bf6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/TurretServos.java @@ -0,0 +1,54 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret; + +import org.firstinspires.ftc.teamcode.blucru.common.hardware.BluHardwareDevice; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluCRServo; +import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; + +public class TurretServos implements BluHardwareDevice { + private BluCRServo servoLeft; + private BluCRServo servoRight; + + /** + * + * class assumes that positions need to be inverted(ie if one servo needs to go to + * + * */ + public TurretServos(BluCRServo servoLeft, BluCRServo servoRight){ + this.servoLeft = servoLeft; + this.servoRight = servoRight; + } + + @Override + public void init() { + servoLeft.init(); + servoRight.init(); + } + + @Override + public void read() { + servoLeft.read(); + servoRight.read(); + } + + @Override + public void write() { + servoLeft.write(); + servoRight.write(); + } + + public void setPower(double power){ + servoLeft.setPower(power); + servoRight.setPower(-power); + } + + public double getPower(){ + return servoLeft.getPower(); + } + + @Override + public void telemetry() { + servoLeft.telemetry(); + servoRight.telemetry(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java new file mode 100644 index 00000000..dc6bf062 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.turretCommands; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class TurnTurretToPosCommand extends InstantCommand { + + public TurnTurretToPosCommand(double pos){ + super (() -> { + Robot.getInstance().turret.setAngle(pos); + }); + + addRequirements(Robot.getInstance().turret); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java index 242f42d2..65f8bbd9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretTest.java @@ -12,7 +12,7 @@ public void initialize(){ public void periodic(){ //positive power is left - turret.moveWithPower(-gamepad1.left_stick_y); + turret.setPower(-gamepad1.left_stick_y); } public void telemetry(){ From aec803674f20efd8232fbb4131e5dd2776041d30 Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 18 Oct 2025 15:48:15 -0400 Subject: [PATCH 042/123] not working intake and elevator --- .../blucru/common/subsytems/Robot.java | 12 +++++ .../common/subsytems/elevator/Elevator.java | 51 +++++++++++++++++++ .../elevator/ElevatorDownCommand.java | 5 ++ .../subsytems/{Intake => intake}/Intake.java | 35 +++++++------ .../blucru/opmodes/BluLinearOpMode.java | 7 +++ .../blucru/opmodes/test/intakeTest.java | 24 +++++++++ 6 files changed, 118 insertions(+), 16 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{Intake => intake}/Intake.java (68%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 8b01c2e8..511f1d00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -5,7 +5,10 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.Elevator; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.Intake; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.Transfer; import java.util.ArrayList; import java.util.List; @@ -19,6 +22,9 @@ public class Robot { //list of subsystems static ArrayList subsystems; public Drivetrain drivetrain; + public Intake intake; + public Elevator elevator; + public Transfer transfer; private static Robot instance; HardwareMap hwMap; List hubs; @@ -109,6 +115,12 @@ public Drivetrain addDrivetrain(){ return drivetrain; } + public Intake addIntake(){ + intake = new Intake("intake"); + subsystems.add(intake); + return intake; + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java new file mode 100644 index 00000000..48ca721a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator; + +import com.arcrobotics.ftclib.command.Subsystem; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; + +public class Elevator implements BluSubsystem, Subsystem { + private BluServo elevatorServo; + private static final double DOWN_POSITION = 0.0;//TODO: find positions + private static final double UP_POSITION = 1.0; + + public Elevator(HardwareMap hardwareMap){ + elevatorServo = new BluServo("elevator"); + } + + public void setUp(){ + elevatorServo.setPos(UP_POSITION); + } + + public void setDown(){ + elevatorServo.setPos(DOWN_POSITION); + } + + @Override + public void init() { + elevatorServo.init(); + } + + @Override + public void read() { + elevatorServo.read(); + } + + @Override + public void write() { + elevatorServo.write(); + } + + @Override + public void telemetry(Telemetry telemetry) { + elevatorServo.telemetry(); + } + + @Override + public void reset() { + setDown(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java new file mode 100644 index 00000000..b4ee9d66 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java @@ -0,0 +1,5 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator; + +public class ElevatorDownCommand { + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java similarity index 68% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index 0fe1e12c..f4879d61 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -1,10 +1,6 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake; - -import static org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake.Intake.State.IN; -import static org.firstinspires.ftc.teamcode.blucru.common.subsytems.Intake.Intake.State.OUT; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake; import com.arcrobotics.ftclib.command.Subsystem; -import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; @@ -12,6 +8,7 @@ public class Intake implements BluSubsystem, Subsystem { private BluMotor motor; + public boolean jammed; private static final double JAM_CURRENT_THRESHOLD = 2500; // milliamps, adjust as needed public enum State{ IN, @@ -39,6 +36,7 @@ public State getState() { public Intake(String motorName) { motor = new BluMotor(motorName); state = State.IDlE; + jammed = false; } @Override @@ -50,23 +48,28 @@ public void init() { public void read() { motor.read(); if (state == State.IN && motor.getCurrent() > JAM_CURRENT_THRESHOLD) { - state = State.OUT; // Jam detected, spit out the ball + jammed = true; // Jam detected, spit out the ball } } @Override public void write() { - switch(state){ - case IN: - motor.setPower(1); - break; - case OUT: - motor.setPower(-1); - break; - case IDlE: - motor.setPower(0); - break; + if (jammed){ + motor.setPower(-1); + } else { + switch(state){ + case IN: + motor.setPower(1); + break; + case OUT: + motor.setPower(-1); + break; + case IDlE: + motor.setPower(0); + break; + } } + motor.write(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index 60ac7e0d..3392c37b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -8,7 +8,10 @@ import org.firstinspires.ftc.teamcode.blucru.common.hardware.SinglePressGamepad; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.Elevator; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.Intake; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.mecanumDrivetrain.Drivetrain; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.Transfer; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public abstract class BluLinearOpMode extends LinearOpMode { @@ -17,6 +20,9 @@ public abstract class BluLinearOpMode extends LinearOpMode { protected boolean reportTelemetry = true; public Drivetrain drivetrain; + public Intake intake; + public Elevator elevator; + public Transfer transfer; //add all of the subsystems that need to be added to the robot here @@ -164,6 +170,7 @@ public void end(){ } public void addDrivetrain(){drivetrain = robot.addDrivetrain();} + public void addIntake(){intake = robot.addIntake();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java new file mode 100644 index 00000000..09d48ea1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +@TeleOp(group = "test") +public class intakeTest extends BluLinearOpMode { + + public void initialize(){ + robot.clear(); + addIntake(); + } + + public void periodic(){ + if(gamepad1.left_trigger > 0.2){ + intake.setIn(); + } + if(gamepad1.right_trigger > 0.2){ + intake.setOut(); + } + } + +} From af11609cd650d5dba1231cbad3fc07b8d12fa41f Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Mon, 20 Oct 2025 10:03:41 -0400 Subject: [PATCH 043/123] turret pid accounts for wrapping --- .../blucru/common/subsytems/turret/Turret.java | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java index b0ef8d95..4fbf9750 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java @@ -55,7 +55,7 @@ public void write() { case IDLE: break; case PID: - servos.setPower(controller.calculate(encoder.getCurrentPos(), position, servos.getPower(), 0)); + servos.setPower(controller.calculate(getRotateError(encoder.getCurrentPos(), position), -servos.getPower())); break; } @@ -83,6 +83,19 @@ public void turnToPointTowardsGoal(Pose2d robotPose){ setFieldCentricPosition(targetAngle, robotPose.getH()); } + public double getRotateError(double currAngle, double targetAngle){ + + double delta = Globals.normalize(targetAngle - currAngle); + + if (delta > 180) { + delta -= 180; + } else if (delta < -180){ + delta += 180; + } + + return delta; + } + @Override public void telemetry(Telemetry telemetry) { From 7fbef5c27ad2d594cb18b937124b2531bbdb9f67 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Mon, 20 Oct 2025 10:05:43 -0400 Subject: [PATCH 044/123] added f to pid controller --- .../blucru/common/subsytems/shooter/Shooter.java | 4 ++-- .../common/subsytems/shooter/ShooterVelocityPID.java | 10 ++++++---- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index ac666813..3cbfef7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -13,7 +13,7 @@ @Config public class Shooter implements BluSubsystem, Subsystem { - public static double p = 0.2, d = 0.1; + public static double p = 0.2, d = 0.1, f = 0.01; public static double limit = 20; private BluMotorWithEncoder shooter; @@ -30,7 +30,7 @@ public Shooter(){ shooter = new BluMotorWithEncoder("shooter"); hood = new BluServo("hood"); state = State.IDLE; - pid = new ShooterVelocityPID(p,d); + pid = new ShooterVelocityPID(p,d,f); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java index c593512a..638a1809 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java @@ -5,19 +5,21 @@ public class ShooterVelocityPID{ PDController controller; double currAccel; - public ShooterVelocityPID(PDController controller){ + double f; + public ShooterVelocityPID(PDController controller, double f){ this.controller = controller; + this.f = f; currAccel = 0; } - public ShooterVelocityPID(double p, double d){ - this(new PDController(p,d)); + public ShooterVelocityPID(double p, double d, double f){ + this(new PDController(p,d),f); } public double calculateDeltaPower(double currVel, double targetVel){ //this should keep currAccel always updated - currAccel = controller.calculate(currVel, targetVel, currAccel ,0); + currAccel = controller.calculate(currVel, targetVel, currAccel ,0) + f; return currAccel; } From 607b551974d81cc06129961aa727bf7cc206ad71 Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 22 Oct 2025 19:42:51 -0400 Subject: [PATCH 045/123] added commands for elevator, transfer, and intake --- .../elevator/ElevatorDownCommand.java | 11 +++- .../subsytems/elevator/ElevatorUpCommand.java | 14 +++++ .../common/subsytems/intake/Intake.java | 21 ++++--- .../subsytems/intake/IntakeSpitCommand.java | 14 +++++ .../subsytems/intake/IntakeStartCommand.java | 14 +++++ .../subsytems/intake/IntakeStopCommand.java | 14 +++++ .../transfer/AllTransferDownCommand.java | 19 ++++++ .../transfer/AllTransferUpCommand.java | 20 +++++++ .../transfer/LeftTransferDownCommand.java | 14 +++++ .../transfer/LeftTransferUpCommand.java | 14 +++++ .../transfer/MiddleTransferDownCommand.java | 14 +++++ .../transfer/MiddleTransferUpCommand.java | 14 +++++ .../transfer/RightTransferDownCommand.java | 14 +++++ .../transfer/RightTransferUpCommand.java | 14 +++++ .../common/subsytems/transfer/Transfer.java | 58 +++++++++++++++---- .../blucru/opmodes/BluLinearOpMode.java | 3 +- .../blucru/opmodes/test/intakeTest.java | 5 +- 17 files changed, 255 insertions(+), 22 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorUpCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeSpitCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStartCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStopCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferDownCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferUpCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferDownCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferUpCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferDownCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferUpCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferDownCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferUpCommand.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java index b4ee9d66..4c6298d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorDownCommand.java @@ -1,5 +1,14 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator; -public class ElevatorDownCommand { +import com.arcrobotics.ftclib.command.InstantCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class ElevatorDownCommand extends InstantCommand { + public ElevatorDownCommand(){ + super(()->{ + Robot.getInstance().elevator.setDown(); + }); + addRequirements(Robot.getInstance().elevator); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorUpCommand.java new file mode 100644 index 00000000..f50fe504 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorUpCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class ElevatorUpCommand extends InstantCommand { + public ElevatorUpCommand(){ + super(()->{ + Robot.getInstance().elevator.setUp(); + }); + addRequirements(Robot.getInstance().elevator); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index f4879d61..141b6e95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -1,15 +1,17 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.command.Subsystem; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; - +@Config public class Intake implements BluSubsystem, Subsystem { private BluMotor motor; public boolean jammed; - private static final double JAM_CURRENT_THRESHOLD = 2500; // milliamps, adjust as needed + public static double JAM_CURRENT_THRESHOLD = 9800; // milliamps, adjust as needed public enum State{ IN, OUT, @@ -25,6 +27,10 @@ public void setOut() { state = State.OUT; } + public void stop(){ + state = State.IDlE; + } + public void setIdle() { state = State.IDlE; } @@ -47,22 +53,20 @@ public void init() { @Override public void read() { motor.read(); - if (state == State.IN && motor.getCurrent() > JAM_CURRENT_THRESHOLD) { - jammed = true; // Jam detected, spit out the ball - } + jammed = (state == State.IN && motor.getCurrent() > JAM_CURRENT_THRESHOLD); // Jam detected, spit out the ball } @Override public void write() { if (jammed){ - motor.setPower(-1); + motor.setPower(1); } else { switch(state){ case IN: - motor.setPower(1); + motor.setPower(-1); break; case OUT: - motor.setPower(-1); + motor.setPower(1); break; case IDlE: motor.setPower(0); @@ -76,6 +80,7 @@ public void write() { @Override public void telemetry(Telemetry telemetry) { motor.telemetry(); + telemetry.addData("Current", motor.getCurrent()); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeSpitCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeSpitCommand.java new file mode 100644 index 00000000..15e45d87 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeSpitCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class IntakeSpitCommand extends InstantCommand { + public IntakeSpitCommand(){ + super(()->{ + Robot.getInstance().intake.setOut(); + }); + addRequirements(Robot.getInstance().intake); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStartCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStartCommand.java new file mode 100644 index 00000000..2c112475 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStartCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class IntakeStartCommand extends InstantCommand { + public IntakeStartCommand(){ + super(()->{ + Robot.getInstance().intake.setIn(); + }); + addRequirements(Robot.getInstance().intake); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStopCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStopCommand.java new file mode 100644 index 00000000..ec95a40c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/IntakeStopCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class IntakeStopCommand extends InstantCommand { + public IntakeStopCommand(){ + super(()->{ + Robot.getInstance().intake.stop(); + }); + addRequirements(Robot.getInstance().intake); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferDownCommand.java new file mode 100644 index 00000000..06de1909 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferDownCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.ParallelCommandGroup; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class AllTransferDownCommand extends InstantCommand { + public AllTransferDownCommand(){ + super(()->{ + new ParallelCommandGroup( + new MiddleTransferDownCommand(), + new LeftTransferDownCommand(), + new RightTransferDownCommand() + ).schedule(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferUpCommand.java new file mode 100644 index 00000000..8683c26a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferUpCommand.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.ParallelCommandGroup; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class AllTransferUpCommand extends InstantCommand { + public AllTransferUpCommand(){ + super(()->{ + new ParallelCommandGroup( + new MiddleTransferUpCommand(), + new LeftTransferUpCommand(), + new RightTransferUpCommand() + ).schedule(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferDownCommand.java new file mode 100644 index 00000000..1d0f86f2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferDownCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class LeftTransferDownCommand extends InstantCommand { + public LeftTransferDownCommand(){ + super(()->{ + Robot.getInstance().transfer.leftSetDown(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferUpCommand.java new file mode 100644 index 00000000..a3335ab4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferUpCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class LeftTransferUpCommand extends InstantCommand { + public LeftTransferUpCommand(){ + super(()->{ + Robot.getInstance().transfer.leftSetUp(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferDownCommand.java new file mode 100644 index 00000000..26c5e989 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferDownCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class MiddleTransferDownCommand extends InstantCommand { + public MiddleTransferDownCommand(){ + super(()->{ + Robot.getInstance().transfer.middleSetDown(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferUpCommand.java new file mode 100644 index 00000000..b1a7d30e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferUpCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class MiddleTransferUpCommand extends InstantCommand { + public MiddleTransferUpCommand(){ + super(()->{ + Robot.getInstance().transfer.middleSetUp(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferDownCommand.java new file mode 100644 index 00000000..8d5ddd7f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferDownCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class RightTransferDownCommand extends InstantCommand { + public RightTransferDownCommand(){ + super(()->{ + Robot.getInstance().transfer.rightSetDown(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferUpCommand.java new file mode 100644 index 00000000..e99f597c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferUpCommand.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class RightTransferUpCommand extends InstantCommand { + public RightTransferUpCommand(){ + super(()->{ + Robot.getInstance().transfer.rightSetUp(); + }); + addRequirements(Robot.getInstance().transfer); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java index 8750800f..f79becbf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java @@ -10,29 +10,60 @@ public class Transfer implements BluSubsystem, Subsystem { TransferServo[] transferServos; + private static final double DOWN_POSITION = 0.0;//TODO: find positions + private static final double UP_POSITION = 1.0; public enum State { DOWN, UP } - private State state; + private State leftState; + private State middleState; + private State rightState; public Transfer(HardwareMap hardwareMap) { transferServos = new TransferServo[]{new LeftTransferServo(), new MiddleTransferServo(), new RightTransferServo()}; } - public void setDown() { - state = State.DOWN; - setAngle(-10); //TODO: find correct angle, -10 degrees is just a random number + public void leftSetDown() { + leftState = State.DOWN; + setAngle(DOWN_POSITION); } - public void setUp() { - state = State.UP; - setAngle(30); //TODO: find correct angle, 30 degrees is just a random number + public void leftSetUp() { + leftState = State.UP; + setAngle(UP_POSITION); + } + public void middleSetDown() { + middleState = State.DOWN; + setAngle(DOWN_POSITION); + } + + public void middleSetUp() { + middleState = State.UP; + setAngle(UP_POSITION); + } + public void rightSetDown() { + rightState = State.DOWN; + setAngle(DOWN_POSITION); + } + + public void rightSetUp() { + rightState = State.UP; + setAngle(UP_POSITION); } - public State getState() { - return state; + public State getLeftState() { + return leftState; } + public State getMiddleState() { + return middleState; + } + + public State getRightState() { + return rightState; + } + + @Override public void init() { for(TransferServo servo : transferServos){ @@ -57,7 +88,10 @@ public void write() { @Override public void telemetry(Telemetry telemetry) { - telemetry.addData("Transfer state: ", state); + telemetry.addData("Left transfer state: ", leftState); + telemetry.addData("Middle transfer state: ", middleState); + telemetry.addData("Right transfer state: ", rightState); + for(TransferServo servo : transferServos){ servo.telemetry(); } @@ -65,7 +99,9 @@ public void telemetry(Telemetry telemetry) { @Override public void reset() { - setDown(); + leftSetDown(); + middleSetDown(); + rightSetDown(); } public void setAngle(double degrees){ for(TransferServo servo:transferServos){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index 3392c37b..2d67e3d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -170,7 +170,8 @@ public void end(){ } public void addDrivetrain(){drivetrain = robot.addDrivetrain();} - public void addIntake(){intake = robot.addIntake();} + public void addIntake(){ + intake = robot.addIntake();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java index 09d48ea1..2a0de977 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java @@ -16,9 +16,12 @@ public void periodic(){ if(gamepad1.left_trigger > 0.2){ intake.setIn(); } - if(gamepad1.right_trigger > 0.2){ + else if(gamepad1.right_trigger > 0.2){ intake.setOut(); } + else{ + intake.stop(); + } } } From 0a82c5f79d5060629575b1a3bb6de63a78b9b00e Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Wed, 22 Oct 2025 20:00:21 -0400 Subject: [PATCH 046/123] ok --- .../subsytems/turret/turretCommands/TurnTurretToPosCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java index dc6bf062..68a1c014 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/TurnTurretToPosCommand.java @@ -10,7 +10,7 @@ public TurnTurretToPosCommand(double pos){ super (() -> { Robot.getInstance().turret.setAngle(pos); }); - + addRequirements(Robot.getInstance().turret); } From 5cd6c2ac539886da9861f211244cd7e30fee5d04 Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 25 Oct 2025 15:22:46 -0400 Subject: [PATCH 047/123] made a package to hold all the transfer commands --- .../{ => transferCommands}/AllTransferDownCommand.java | 2 +- .../transfer/{ => transferCommands}/AllTransferUpCommand.java | 3 +-- .../{ => transferCommands}/LeftTransferDownCommand.java | 2 +- .../transfer/{ => transferCommands}/LeftTransferUpCommand.java | 2 +- .../{ => transferCommands}/MiddleTransferDownCommand.java | 2 +- .../{ => transferCommands}/MiddleTransferUpCommand.java | 2 +- .../{ => transferCommands}/RightTransferDownCommand.java | 2 +- .../{ => transferCommands}/RightTransferUpCommand.java | 2 +- 8 files changed, 8 insertions(+), 9 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/AllTransferDownCommand.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/AllTransferUpCommand.java (90%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/LeftTransferDownCommand.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/LeftTransferUpCommand.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/MiddleTransferDownCommand.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/MiddleTransferUpCommand.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/RightTransferDownCommand.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/{ => transferCommands}/RightTransferUpCommand.java (94%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferDownCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java index 06de1909..ff062c62 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; import com.arcrobotics.ftclib.command.ParallelCommandGroup; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferUpCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java index 8683c26a..d1bc84fd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/AllTransferUpCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java @@ -1,8 +1,7 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; import com.arcrobotics.ftclib.command.ParallelCommandGroup; -import com.arcrobotics.ftclib.command.SequentialCommandGroup; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/LeftTransferDownCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferDownCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/LeftTransferDownCommand.java index 1d0f86f2..a29ee851 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/LeftTransferDownCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/LeftTransferUpCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferUpCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/LeftTransferUpCommand.java index a3335ab4..6e2efa5a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferUpCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/LeftTransferUpCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/MiddleTransferDownCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferDownCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/MiddleTransferDownCommand.java index 26c5e989..5286b04a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/MiddleTransferDownCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/MiddleTransferUpCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferUpCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/MiddleTransferUpCommand.java index b1a7d30e..913721e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferUpCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/MiddleTransferUpCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/RightTransferDownCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferDownCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/RightTransferDownCommand.java index 8d5ddd7f..0858dfdb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/RightTransferDownCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/RightTransferUpCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferUpCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/RightTransferUpCommand.java index e99f597c..393e66d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferUpCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/RightTransferUpCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands; import com.arcrobotics.ftclib.command.InstantCommand; From 8db81d40cdb7bd7996784776a1e8f1fdb46070f1 Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 25 Oct 2025 15:23:12 -0400 Subject: [PATCH 048/123] added math to adjust jam threshold for current voltage --- .../ftc/teamcode/blucru/common/subsytems/intake/Intake.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index 141b6e95..715b447f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -7,11 +7,14 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + @Config public class Intake implements BluSubsystem, Subsystem { private BluMotor motor; public boolean jammed; public static double JAM_CURRENT_THRESHOLD = 9800; // milliamps, adjust as needed + public static double NOMINAL_VOLTAGE = 12.0 public enum State{ IN, OUT, @@ -53,6 +56,9 @@ public void init() { @Override public void read() { motor.read(); + double currentVoltage = Robot.getInstance().getVoltage(); + double adjustedThreshold = JAM_CURRENT_THRESHOLD * (currentVoltage / NOMINAL_VOLTAGE); + jammed = (state == State.IN && motor.getCurrent() > JAM_CURRENT_THRESHOLD); // Jam detected, spit out the ball } From 3ddddd34fe1be53300fd13ca06a734a3339cd55a Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 25 Oct 2025 15:23:34 -0400 Subject: [PATCH 049/123] forgot semicolon --- .../ftc/teamcode/blucru/common/subsytems/intake/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index 715b447f..fd3e6629 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -14,7 +14,7 @@ public class Intake implements BluSubsystem, Subsystem { private BluMotor motor; public boolean jammed; public static double JAM_CURRENT_THRESHOLD = 9800; // milliamps, adjust as needed - public static double NOMINAL_VOLTAGE = 12.0 + public static double NOMINAL_VOLTAGE = 12.0; public enum State{ IN, OUT, From a6ce5e4e50c6992fe4b71795cf3907ee3999acd8 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 1 Nov 2025 14:23:26 -0400 Subject: [PATCH 050/123] revamped pid --- .../common/subsytems/shooter/Shooter.java | 13 +++---- .../subsytems/shooter/ShooterVelocityPID.java | 35 ++++++++++++------- .../blucru/common/util/PDController.java | 7 ++++ .../blucru/opmodes/test/ShooterPIDTuning.java | 17 ++++----- 4 files changed, 43 insertions(+), 29 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index 3cbfef7f..145f0747 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -13,7 +13,7 @@ @Config public class Shooter implements BluSubsystem, Subsystem { - public static double p = 0.2, d = 0.1, f = 0.01; + public static double p = 0.2,i = 0.001, d = 0.1, f = 0.006; public static double limit = 20; private BluMotorWithEncoder shooter; @@ -30,7 +30,7 @@ public Shooter(){ shooter = new BluMotorWithEncoder("shooter"); hood = new BluServo("hood"); state = State.IDLE; - pid = new ShooterVelocityPID(p,d,f); + pid = new ShooterVelocityPID(p,i,d,f); } @Override @@ -50,10 +50,8 @@ public void write() { targetVel = 0; break; case VELOCITY: - if (Math.abs(shooter.getVel()- targetVel) >= limit){ - shooter.setPower(shooter.getPower() + pid.calculateDeltaPower(shooter.getVel(), targetVel)); - Globals.telemetry.addData("delta", pid.calculateDeltaPower(shooter.getVel(), targetVel)); - } + shooter.setPower(pid.calculateDeltaPower(shooter.getVel(), targetVel)); + Globals.telemetry.addData("delta", pid.calculateDeltaPower(shooter.getVel(), targetVel)); } shooter.write(); @@ -94,7 +92,6 @@ public double getHoodAngle(){ @Override public void telemetry(Telemetry telemetry) { telemetry.addData("Shooter power", shooter.getPower()); - telemetry.addData("Shooter Pos", shooter.getCurrentPos()); } @Override @@ -104,6 +101,6 @@ public void reset() { } public void updatePID(){ - pid.setPD(p,d); + pid.setPIDF(p,i,d,f); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java index 638a1809..c8deacd3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/ShooterVelocityPID.java @@ -1,27 +1,25 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; +import com.arcrobotics.ftclib.controller.PIDController; + import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; public class ShooterVelocityPID{ - PDController controller; - double currAccel; + double p; + double i; + double d; double f; - public ShooterVelocityPID(PDController controller, double f){ - this.controller = controller; - this.f = f; - currAccel = 0; - } + PIDController controller; - public ShooterVelocityPID(double p, double d, double f){ - this(new PDController(p,d),f); + public ShooterVelocityPID(double p, double i, double d, double f){ + controller = new PIDController(p,i,d); + this.f=f; } public double calculateDeltaPower(double currVel, double targetVel){ - //this should keep currAccel always updated - currAccel = controller.calculate(currVel, targetVel, currAccel ,0) + f; + return controller.calculate(currVel, targetVel) + f*targetVel; - return currAccel; } public void setP(double p){ @@ -33,7 +31,18 @@ public void setD(double d){ } public void setPD(double p, double d){ - controller.setPD(p,d); + controller.setP(p); + controller.setD(d); + } + + public void setPDF(double p, double d, double f){ + setPD(p,d); + this.f = f; + } + + public void setPIDF(double p, double i, double d, double f){ + setPDF(p,d,f); + controller.setI(i); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/PDController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/PDController.java index 72451693..bdc0d41e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/PDController.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/PDController.java @@ -54,12 +54,19 @@ public double calculate(Vector2d pv, MotionProfile profile){ public void setPD(double p, double d){ super.setPID(p, 0, d); + this.p=p; + this.d=d; + k = new Vector2d(this.p, this.d); } public void setP(double p){ super.setP(p); + this.p=p; + k = new Vector2d(this.p, d); } public void setD(double d){ super.setD(d); + this.d = d; + k = new Vector2d(p, this.d); } public void telemetry(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java index 660a59be..5694d240 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.blucru.opmodes.test; import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -10,22 +11,22 @@ import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; @TeleOp +@Config public class ShooterPIDTuning extends BluLinearOpMode { + public static double vel = 0; + public void initialize(){ robot.clear(); addShooter(); Globals.multiTelemetry = new MultipleTelemetry(telemetry , FtcDashboard.getInstance().getTelemetry()); + vel = 0; } public void periodic(){ - if (gamepad1.a){ - new ShootWithVelocityCommand(2500).schedule(); - } - - if (gamepad1.b){ - new ShootWithVelocityCommand(0).schedule(); + if (gamepad1.a) { + shooter.shootWithVelocity(vel); } if (gamepad1.x){ @@ -37,8 +38,8 @@ public void periodic(){ public void telemetry(){ Globals.multiTelemetry.addData("shooter vel", shooter.getVel()); - telemetry.addData("Shooter power", shooter.getPower()); - telemetry.addData("g1x", gamepad1.x); + Globals.multiTelemetry.addData("target vel", vel); + Globals.multiTelemetry.update(); } } From f759e966559c383eb663f3cd3ab8546ce5096a40 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 15 Nov 2025 15:16:03 -0500 Subject: [PATCH 051/123] added some commands and did turret lock on --- .../shooterCommands/SetHoodAngleCommand.java | 2 +- .../shooterCommands/ShootWithVelocityCommand.java | 2 +- .../shooterCommands/TurnOffShooterCommand.java | 2 +- .../shooterCommands/TurnOnShooterCommand.java | 2 +- .../blucru/common/subsytems/turret/Turret.java | 14 ++++++++------ .../common/util/LimelightObeliskTagDetector.java | 3 ++- .../blucru/opmodes/test/ShooterPIDTuning.java | 1 - .../teamcode/blucru/opmodes/test/ShooterTest.java | 8 +++----- 8 files changed, 17 insertions(+), 17 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => shooter}/shooterCommands/SetHoodAngleCommand.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => shooter}/shooterCommands/ShootWithVelocityCommand.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => shooter}/shooterCommands/TurnOffShooterCommand.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/{ => shooter}/shooterCommands/TurnOnShooterCommand.java (94%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/SetHoodAngleCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/SetHoodAngleCommand.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/SetHoodAngleCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/SetHoodAngleCommand.java index 7e0c4d1c..7f9007be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/SetHoodAngleCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/SetHoodAngleCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/ShootWithVelocityCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/ShootWithVelocityCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/ShootWithVelocityCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/ShootWithVelocityCommand.java index 5170557f..076c43e1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/ShootWithVelocityCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/ShootWithVelocityCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOffShooterCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/TurnOffShooterCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOffShooterCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/TurnOffShooterCommand.java index 905743ae..57fabe7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOffShooterCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/TurnOffShooterCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/TurnOnShooterCommand.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/TurnOnShooterCommand.java index 4ce884a5..738aa216 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooterCommands/TurnOnShooterCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/shooterCommands/TurnOnShooterCommand.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands; +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands; import com.arcrobotics.ftclib.command.InstantCommand; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java index 4fbf9750..3d7a1f07 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/Turret.java @@ -9,6 +9,8 @@ import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluCRServo; import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluPIDServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; +import org.firstinspires.ftc.teamcode.blucru.common.util.Alliance; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.PDController; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; @@ -22,8 +24,8 @@ public class Turret implements BluSubsystem, Subsystem { Pose2d goalPose; private enum State{ IDLE, - PID - + PID, + LOCK_ON_GOAL } private State state; @@ -57,6 +59,8 @@ public void write() { case PID: servos.setPower(controller.calculate(getRotateError(encoder.getCurrentPos(), position), -servos.getPower())); break; + case LOCK_ON_GOAL: + setFieldCentricPosition((Globals.alliance == Alliance.BLUE) ? 144 : 36, Math.toDegrees(Robot.getInstance().sixWheelDrivetrain.getPos().getH())); } servos.write(); @@ -77,10 +81,8 @@ public void setFieldCentricPosition(double position, double robotHeading){ setAngle(position - robotHeading); } - public void turnToPointTowardsGoal(Pose2d robotPose){ - double targetAngle = goalPose.vec().subtractNotInPlace(robotPose.vec()).getHeading(); - - setFieldCentricPosition(targetAngle, robotPose.getH()); + public void lockOnGoal(){ + state = State.LOCK_ON_GOAL; } public double getRotateError(double currAngle, double targetAngle){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java index 118c240b..45714d7a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/LimelightObeliskTagDetector.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.blucru.common.util; +import com.arcrobotics.ftclib.command.Subsystem; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; @@ -12,7 +13,7 @@ import java.util.ArrayList; import java.util.List; -public class LimelightObeliskTagDetector implements BluSubsystem { +public class LimelightObeliskTagDetector implements BluSubsystem, Subsystem { Limelight3A limelight; String[] pattern; int greenIndex; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java index 5694d240..9af4d7ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterPIDTuning.java @@ -6,7 +6,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.Shooter; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.ShootWithVelocityCommand; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java index 62973e7b..18d89f70 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ShooterTest.java @@ -5,13 +5,11 @@ import com.sfdev.assembly.state.StateMachineBuilder; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.SetHoodAngleCommand; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.TurnOffShooterCommand; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooterCommands.TurnOnShooterCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands.SetHoodAngleCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands.TurnOffShooterCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands.TurnOnShooterCommand; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; -import java.net.IDN; - @TeleOp(group = "test") public class ShooterTest extends BluLinearOpMode { enum State{ From ec387b748247cede39d178dce21261af8d637148 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 15 Nov 2025 15:16:35 -0500 Subject: [PATCH 052/123] commands --- .../blucru/common/commands/IntakeCommand.java | 20 ++++++++++++++ .../common/commands/ShootBallsCommand.java | 22 ++++++++++++++++ .../common/commands/TransferCommand.java | 26 +++++++++++++++++++ .../UpdateRobotPositionFromLLCommand.java | 17 ++++++++++++ .../turretCommands/LockOnGoalCommand.java | 17 ++++++++++++ 5 files changed, 102 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/IntakeCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/ShootBallsCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/UpdateRobotPositionFromLLCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/LockOnGoalCommand.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/IntakeCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/IntakeCommand.java new file mode 100644 index 00000000..7c54b7a5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/IntakeCommand.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.blucru.common.commands; + +import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorDownCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStartCommand; + +public class IntakeCommand extends InstantCommand { + + public IntakeCommand(){ + super (() -> { + new SequentialCommandGroup( + new IntakeStartCommand(), + new ElevatorDownCommand() + ).schedule(); + }); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/ShootBallsCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/ShootBallsCommand.java new file mode 100644 index 00000000..034d3388 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/ShootBallsCommand.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.blucru.common.commands; + +import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; +import com.arcrobotics.ftclib.command.WaitCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands.AllTransferDownCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands.AllTransferUpCommand; + +public class ShootBallsCommand extends InstantCommand { + + public ShootBallsCommand(){ + super(() ->{ + new SequentialCommandGroup( + new AllTransferUpCommand(), + new WaitCommand(400), + new AllTransferDownCommand() + ).schedule();} + ); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java new file mode 100644 index 00000000..31cd9f0a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.blucru.common.commands; + +import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; +import com.arcrobotics.ftclib.command.WaitCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorUpCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStopCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands.AllTransferDownCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.turretCommands.LockOnGoalCommand; + +public class TransferCommand extends InstantCommand { + + public TransferCommand(){ + super(() -> { + new SequentialCommandGroup( + new ElevatorUpCommand(), + new IntakeStopCommand(), + new AllTransferDownCommand(), + new WaitCommand(900), + new LockOnGoalCommand() + ).schedule(); + }); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/UpdateRobotPositionFromLLCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/UpdateRobotPositionFromLLCommand.java new file mode 100644 index 00000000..b5d19c45 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/UpdateRobotPositionFromLLCommand.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode.blucru.common.commands; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class UpdateRobotPositionFromLLCommand extends InstantCommand { + + public UpdateRobotPositionFromLLCommand(){ + super (() -> { + Robot.getInstance().drivetrain.setCurrentPose(Robot.getInstance().llTagDetector.getLLBotPose()); + }); + + addRequirements(Robot.getInstance().drivetrain, Robot.getInstance().llTagDetector); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/LockOnGoalCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/LockOnGoalCommand.java new file mode 100644 index 00000000..f37bf74b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/turret/turretCommands/LockOnGoalCommand.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.turretCommands; + +import com.arcrobotics.ftclib.command.InstantCommand; + +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; + +public class LockOnGoalCommand extends InstantCommand { + + public LockOnGoalCommand(){ + super (() -> { + Robot.getInstance().turret.lockOnGoal(); + }); + + addRequirements(Robot.getInstance().turret); + } + +} From aac6d2a9a6415f7d74177a82b80c12b1c539ccdc Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Wed, 26 Nov 2025 14:45:17 -0500 Subject: [PATCH 053/123] tele done and driving works --- .../common/commands/TransferCommand.java | 4 +- .../common/hardware/SinglePressGamepad.java | 8 +- .../common/hardware/motor/BluMotor.java | 21 +++++- .../hardware/motor/BluMotorWithEncoder.java | 2 + .../common/hardware/servo/BluServo.java | 15 +++- .../blucru/common/subsytems/Robot.java | 7 +- .../drivetrain/localization/Pinpoint.java | 2 +- .../sixWheelDrive/SixWheelDrive.java | 18 ++++- .../sixWheelDrive/SixWheelDriveBase.java | 13 ++-- .../common/subsytems/shooter/Shooter.java | 16 +++- .../teamcode/blucru/common/util/Globals.java | 8 +- .../teamcode/blucru/common/util/Pose2d.java | 4 + .../blucru/opmodes/BluLinearOpMode.java | 3 + .../ftc/teamcode/blucru/opmodes/TeleOp.java | 75 +++++++++++++++++++ .../blucru/opmodes/test/HoodTest.java | 39 ++++++++++ .../blucru/opmodes/test/LocalizerTest.java | 27 +++++++ .../opmodes/test/MotorDirectionFinder.java | 62 +++++++++++++++ .../opmodes/test/SixWheelManualDrive.java | 20 +++++ 18 files changed, 316 insertions(+), 28 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/TeleOp.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LocalizerTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/MotorDirectionFinder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java index 31cd9f0a..ce7075c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java @@ -6,6 +6,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorUpCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStopCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands.ShootWithVelocityCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands.AllTransferDownCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.turretCommands.LockOnGoalCommand; @@ -18,7 +19,8 @@ public TransferCommand(){ new IntakeStopCommand(), new AllTransferDownCommand(), new WaitCommand(900), - new LockOnGoalCommand() + new LockOnGoalCommand(), + new ShootWithVelocityCommand(2500) ).schedule(); }); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/SinglePressGamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/SinglePressGamepad.java index 396c7aca..861a81c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/SinglePressGamepad.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/SinglePressGamepad.java @@ -128,8 +128,8 @@ public void update(){ if (gamepad.dpad_left){ //pressed last loop - thisFaceButtons[2] = !lastFaceButtons[2]; - lastFaceButtons[2] = true; + thisDpad[2] = !lastDpad[2]; + lastDpad[2] = true; } else { thisDpad[2] = false; lastDpad[2] = false; @@ -137,8 +137,8 @@ public void update(){ if (gamepad.dpad_right){ //pressed last loop - thisFaceButtons[3] = !lastFaceButtons[3]; - lastFaceButtons[3] = true; + thisFaceButtons[3] = !lastDpad[3]; + lastDpad[3] = true; } else { thisDpad[3] = false; lastDpad[3] = false; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java index 1de2fdde..a81328a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotor.java @@ -8,12 +8,13 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.firstinspires.ftc.teamcode.R; import org.firstinspires.ftc.teamcode.blucru.common.hardware.BluHardwareDevice; import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; public class BluMotor extends DcMotorImplEx implements BluHardwareDevice { String name; - double power=0, lastPower=0; + double power=0, lastPower=0, multiplier = 1; double current; public BluMotor(String name){ this(name, Direction.FORWARD); @@ -25,11 +26,23 @@ public BluMotor(String name, Direction direction, ZeroPowerBehavior zpb){ this(Globals.hwMap.get(DcMotor.class, name), name, direction, zpb); } private BluMotor(DcMotor motor, String name, Direction direction, ZeroPowerBehavior zpb){ - super(motor.getController(), motor.getPortNumber(), direction); + super(motor.getController(), motor.getPortNumber(), Direction.FORWARD); this.name = name; + if (direction == Direction.REVERSE){ + multiplier = -1; + } super.setZeroPowerBehavior(zpb); } + + public void setDirection(Direction direction) { + if (direction == Direction.FORWARD) { + multiplier = 1; + } else { + multiplier = -1; + } + } + public void setPower(double power){ this.power = Range.clip(power,-1,1); } @@ -48,8 +61,8 @@ public void read() { public void write() { if (Math.abs(power - lastPower) > 0.005){ //power has changed - lastPower = power; - super.setPower(Globals.getCorrectPower(power)); + lastPower = power * multiplier; + super.setPower(Globals.getCorrectPower(power * multiplier)); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java index 94b350e4..5a19d43e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/motor/BluMotorWithEncoder.java @@ -29,6 +29,8 @@ private BluMotorWithEncoder(DcMotor motor, String name, Direction direction, Zer super.setZeroPowerBehavior(zpb); } + + public void reset(){ super.setMode(RunMode.STOP_AND_RESET_ENCODER); super.setMode(RunMode.RUN_WITHOUT_ENCODER); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluServo.java index 8938f6de..4f3d5f02 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/servo/BluServo.java @@ -12,6 +12,7 @@ public class BluServo extends ServoImpl implements BluHardwareDevice { String name; ServoController controller; double pos=0, lastPos=0; + int direction; boolean enabled; public BluServo(String name){ this(name, Direction.FORWARD); @@ -20,18 +21,29 @@ public BluServo(String name, Direction direction){ this(Globals.hwMap.get(ServoImpl.class, name), name, direction); } private BluServo(ServoImpl servo, String name, Direction direction){ - super(servo.getController(), servo.getPortNumber(), servo.getDirection()); + super(servo.getController(), servo.getPortNumber(), direction); super.setDirection(direction); this.name = name; this.controller = servo.getController(); this.enabled = true; + if (direction == Direction.FORWARD){ + this.direction = 1; + } else{ + this.direction = 0; + } } public void setPos(double pos){ this.pos = Range.clip(pos,0,1); } + public double getServoPos(){ + return super.getPosition(); + } public double getPos(){ return pos; } + public int getDir(){ + return direction; + } /** @@ -61,6 +73,7 @@ public void write() { if (Math.abs(pos - lastPos) > 0.002){ lastPos = pos; super.setPosition(pos); + } } public String getName(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index c4ccc98e..584e39cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -122,6 +122,11 @@ public void telemetry(Telemetry telemetry){ subsystem.telemetry(telemetry); } } + public void idleRobot(){ + for (BluSubsystem subsystem: subsystems){ + subsystem.reset(); + } + } public double getAmountOfSubsystems(){ return subsystems.size(); @@ -136,7 +141,7 @@ public Drivetrain addDrivetrain(){ return mecanumDrivetrain; } - public SixWheelDriveBase addSixWheelDrivetrain(){ + public SixWheelDrive addSixWheelDrivetrain(){ sixWheelDrivetrain = new SixWheelDrive(); subsystems.add(sixWheelDrivetrain); return sixWheelDrivetrain; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java index 529d6cf2..553a4071 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Pinpoint.java @@ -10,7 +10,7 @@ public class Pinpoint implements RobotLocalizer{ //TODO TUNE PER ROBOT - public static double parallelYOffset = -144.675, perpXOffset = -70; + public static double parallelYOffset = 138.5, perpXOffset = -94.05879; private GoBildaPinpointDriver pinpoint; private double headingOffset; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index fc58d3e0..05fef1c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.command.Subsystem; import com.qualcomm.robotcore.hardware.Gamepad; @@ -8,13 +9,16 @@ import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.SixWheelPID; import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; - +@Config public class SixWheelDrive extends SixWheelDriveBase implements Subsystem { private double drivePower; private Point2d[] path; private PurePursuitComputer computer; private final double LOOK_AHEAD_DIST = 10; private SixWheelPID pid; + public static double a = 0.6; + public static double b = 0.01; + public static double c = 0.39; public SixWheelDrive(){ super(); drivePower = 1; @@ -47,8 +51,8 @@ public void write(){ } public void teleDrive(Gamepad g1, double tol){ - double x = -g1.left_stick_y; - double r = g1.left_stick_x; + double x = cubicScaling(-g1.left_stick_y); + double r = cubicScaling(g1.right_stick_x); if (Math.abs(x) <= tol){ x = 0; @@ -63,6 +67,7 @@ public void teleDrive(Gamepad g1, double tol){ } else { //either stopped driving or idle alr dtState = State.IDLE; + drive(0,0); } } else { dtState = State.TELE_DRIVE; @@ -74,6 +79,9 @@ public void teleDrive(Gamepad g1, double tol){ public void setDrivePower(double power){ this.drivePower = power; } + public double getDrivePower(){ + return drivePower; + } public void followPath(Point2d[] path){ this.path = path; computer.resetLastFoundIndex(); @@ -97,4 +105,8 @@ public void setHeading(double heading){ public void telemetry(Telemetry telemetry) { super.telemetry(telemetry); } + + public double cubicScaling(double value){ + return a * value * value * value + b * value * value + c * value; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java index b6c4cd09..d6f7642d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java @@ -14,6 +14,8 @@ import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; +import java.util.Arrays; + public class SixWheelDriveBase implements BluSubsystem{ private BluMotor[] dtMotors; @@ -29,7 +31,7 @@ public enum State{ public SixWheelDriveBase(){ this(new BluMotor(Globals.flMotorName), - new BluMotor(Globals.flMotorName), + new BluMotor(Globals.frMotorName), new BluMotor(Globals.blMotorName), new BluMotor(Globals.brMotorName)); } @@ -65,10 +67,11 @@ public void write() { public void drive(double x, double r){ double[] powers = SixWheelKinematics.getPowers(x,r); - for(int i = 0; i<4; i+=2){ - dtMotors[i].setPower(powers[0]); - dtMotors[i+1].setPower(powers[1]); - } + dtMotors[0].setPower(powers[0]); + dtMotors[2].setPower(powers[0]); + dtMotors[1].setPower(powers[1]); + dtMotors[3].setPower(powers[1]); + Globals.telemetry.addData("Powers", Arrays.toString(powers)); } public Pose2d getPos(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index 145f0747..c030af7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -4,6 +4,7 @@ import com.arcrobotics.ftclib.command.Subsystem; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; @@ -15,9 +16,14 @@ public class Shooter implements BluSubsystem, Subsystem { public static double p = 0.2,i = 0.001, d = 0.1, f = 0.006; public static double limit = 20; + public static final double ZERO_ANGLE = 26; + public static final double TOP_ANGLE = 50; + public static final double SERVO_ROT_TO_HOOD_ROT = 260/28; + public static final double SERVO_ANGLE_DELTA = TOP_ANGLE - ZERO_ANGLE; + public static final double SERVO_POS = SERVO_ROT_TO_HOOD_ROT * SERVO_ANGLE_DELTA / 255.0; private BluMotorWithEncoder shooter; - private BluServo hood; + public BluServo hood; enum State{ IDLE, @@ -28,10 +34,13 @@ enum State{ ShooterVelocityPID pid; public Shooter(){ shooter = new BluMotorWithEncoder("shooter"); - hood = new BluServo("hood"); + hood = new BluServo("hood", Servo.Direction.FORWARD); state = State.IDLE; pid = new ShooterVelocityPID(p,i,d,f); } + public double shooterAngleToPos(double angle){ + return (SERVO_POS)/SERVO_ANGLE_DELTA * angle - (ZERO_ANGLE * SERVO_POS)/SERVO_ANGLE_DELTA ; + } @Override public void init() { @@ -61,7 +70,6 @@ public void write() { public void shoot(double power){ shooter.setPower(power); } - public void shootWithVelocity(double vel){ targetVel = vel; state = State.VELOCITY; @@ -78,7 +86,7 @@ public void rampDownShooter(){ shooter.setPower(0); } public void setHoodAngle(double angle){ - hood.setPos(Globals.convertAngleToServoPos(255, angle)); + setHoodServoPos(shooterAngleToPos(angle)); } public void setHoodServoPos(double pos){ hood.setPos(pos); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java index 798c8654..c5a6371d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Globals.java @@ -18,10 +18,10 @@ public class Globals { public static double voltage = 13.0; public static Alliance alliance = RED; public static String pinpointName = "pinpoint"; - public static String flMotorName = "fl"; - public static String frMotorName = "fr"; - public static String blMotorName = "bl"; - public static String brMotorName = "br"; + public static String flMotorName = "FL"; + public static String frMotorName = "FR"; + public static String blMotorName = "BL"; + public static String brMotorName = "BR"; public static Pose2d startPose = new Pose2d(0,0,Math.PI/2); public static ElapsedTime matchTime; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java index 5db88ace..d46b3b4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/Pose2d.java @@ -51,5 +51,9 @@ public double getDistTo(Pose2d pose2d){ return Math.sqrt(Math.pow(pose2d.getX(),2) + Math.pow(pose2d.getY(),2)); } + public String toString(){ + return "(" + x + ", " + y + ", " + h + ")"; + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index 37479087..bd3cb07d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -8,6 +8,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.hardware.SinglePressGamepad; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.SixWheelDrive; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.Elevator; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.Intake; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.mecanumDrivetrain.Drivetrain; @@ -25,6 +26,7 @@ public abstract class BluLinearOpMode extends LinearOpMode { protected boolean reportTelemetry = true; public Drivetrain drivetrain; + public SixWheelDrive sixWheel; public Shooter shooter; public Intake intake; public Elevator elevator; @@ -187,6 +189,7 @@ public void addIntake(){ public void addTurret(){turret = robot.addTurret();} public void addObeliskTagDetector(){obeliskTagDetector = robot.addObeliskTagDetector();} public void addLLTagDetector(){llTagDetector = robot.addLLTagDetector();} + public void addSixWheel(){sixWheel = robot.addSixWheelDrivetrain();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/TeleOp.java new file mode 100644 index 00000000..9940d8fb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/TeleOp.java @@ -0,0 +1,75 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes; + +import com.arcrobotics.ftclib.command.SequentialCommandGroup; +import com.arcrobotics.ftclib.command.WaitCommand; +import com.sfdev.assembly.state.StateMachine; +import com.sfdev.assembly.state.StateMachineBuilder; + +import org.firstinspires.ftc.teamcode.blucru.common.commands.IntakeCommand; +import org.firstinspires.ftc.teamcode.blucru.common.commands.ShootBallsCommand; +import org.firstinspires.ftc.teamcode.blucru.common.commands.TransferCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStartCommand; + +public class TeleOp extends BluLinearOpMode{ + + StateMachine sm; + + public enum State{ + IDLE, + INTAKING, + DRIVING_TO_SHOOT + } + + @Override + public void initialize(){ + robot.clear(); + addSixWheel(); + addIntake(); + addShooter(); + addLLTagDetector(); + addTurret(); + sm = new StateMachineBuilder() + .state(State.IDLE) + .transition(() -> driver1.pressedRightBumper(), State.INTAKING, () ->{ + new IntakeCommand().schedule(); + }) + .state(State.INTAKING) + .transition(() -> driver1.pressedOptions(), State.IDLE, () -> { + robot.idleRobot(); + }) + .transition(() -> driver2.pressedA(), State.DRIVING_TO_SHOOT, () -> { + new TransferCommand().schedule(); + }) + .state(State.DRIVING_TO_SHOOT) + .transition(() -> driver1.pressedOptions(), State.IDLE, () -> { + robot.idleRobot(); + }) + .transition(() -> driver1.pressedRightBumper(), State.INTAKING, () -> { + new SequentialCommandGroup( + new ShootBallsCommand(), + new WaitCommand(100), + new IntakeCommand() + ).schedule(); + }) + .build(); + + sm.setState(State.IDLE); + sm.start(); + } + + public void periodic(){ + sm.update(); + sixWheel.drive(-gamepad1.left_stick_y, gamepad1.left_stick_x); + if (driver2.pressedB()){ + sixWheel.setPosition(llTagDetector.getLLBotPose()); + } + if (driver1.pressedLeftBumper()){ + if (sixWheel.getDrivePower() == 0.5){ + sixWheel.setDrivePower(1); + } else { + sixWheel.setDrivePower(0.5); + } + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java new file mode 100644 index 00000000..890f22b7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; +@TeleOp +public class HoodTest extends BluLinearOpMode { + double pos; + + @Override + public void initialize() { + robot.clear(); + addShooter(); + } + + @Override + public void periodic() { + if (driver1.pressedX()){ + shooter.setHoodAngle(26); + pos = 26; + } + if (gamepad1.x){ + shooter.setHoodAngle(50); + pos = 50; + } + if (gamepad1.a){ + shooter.setHoodAngle(38); + pos = 38; + } + } + + @Override + public void telemetry(){ + telemetry.addData("Servo dir", shooter.hood.getDir()); + telemetry.addData("Servo pos", shooter.hood.getServoPos()); + telemetry.addData("Thought pos", shooter.hood.getPos()); + telemetry.addData("Angle", pos); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LocalizerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LocalizerTest.java new file mode 100644 index 00000000..cadf870f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/LocalizerTest.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +public class LocalizerTest extends BluLinearOpMode { + + @Override + public void initialize() { + robot.clear(); + addSixWheel(); + } + + public void onStart(){ + sixWheel.setPosition(new Pose2d(0,0,0)); + } + + public void periodic(){ + if (gamepad1.a){ + sixWheel.setPosition(new Pose2d(0,0,0)); + } + } + + public void telemetry(){ + telemetry.addData("Pos", sixWheel.getPos().toString()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/MotorDirectionFinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/MotorDirectionFinder.java new file mode 100644 index 00000000..dadeff85 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/MotorDirectionFinder.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; + +@TeleOp() +public class MotorDirectionFinder extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + Globals.hwMap = hardwareMap; + BluMotorWithEncoder FL = new BluMotorWithEncoder("FL"); + BluMotorWithEncoder FR = new BluMotorWithEncoder("FR"); + BluMotorWithEncoder BL = new BluMotorWithEncoder("BL"); + BluMotorWithEncoder BR = new BluMotorWithEncoder("BR"); + + waitForStart(); + + while (opModeIsActive()){ + FL.read(); + FR.read(); + BL.read(); + BR.read(); + if (gamepad1.a){ + telemetry.addLine("here"); + FL.setPower(1); + } else { + FL.setPower(0); + } + + if (gamepad1.y){ + FR.setPower(-1); + telemetry.addLine("here"); + } else { + FR.setPower(0); + } + + if (gamepad1.left_bumper){ + BL.setPower(1); + telemetry.addLine("here"); + } else { + BL.setPower(0); + } + + if (gamepad1.right_bumper){ + BR.setPower(-1); + telemetry.addLine("here"); + } else { + BR.setPower(0); + } + FL.write(); + FR.write(); + BL.write(); + BR.write(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java new file mode 100644 index 00000000..5cf414db --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +@TeleOp +public class SixWheelManualDrive extends BluLinearOpMode { + @Override + public void initialize() { + robot.clear(); + addSixWheel(); + } + + @Override + public void periodic() { + sixWheel.setDrivePower(0.5); + sixWheel.teleDrive(gamepad1,0.2); + } +} From aa77e6a5ee11c1eefe58bf35a76ba1c98d4435f8 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Wed, 26 Nov 2025 18:13:14 -0500 Subject: [PATCH 054/123] pure pursuit opmode setup, elevator and intake work --- .../blucru/common/subsytems/Robot.java | 2 +- .../common/subsytems/elevator/Elevator.java | 4 +- .../common/subsytems/intake/Intake.java | 45 ++++++++++++------- .../blucru/opmodes/test/BluServoTest.java | 9 +++- .../blucru/opmodes/test/PurePusuitTest.java | 24 ++++++++++ .../opmodes/test/TestPurePursuitPath.java | 12 +++++ 6 files changed, 76 insertions(+), 20 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PurePusuitTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TestPurePursuitPath.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 584e39cf..996fbb7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -153,7 +153,7 @@ public Shooter addShooter() { return shooter; } public Intake addIntake(){ - intake = new Intake("intake"); + intake = new Intake("intakeLeft", "intakeRight"); subsystems.add(intake); return intake; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java index 48ca721a..5e6f2b61 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java @@ -9,8 +9,8 @@ public class Elevator implements BluSubsystem, Subsystem { private BluServo elevatorServo; - private static final double DOWN_POSITION = 0.0;//TODO: find positions - private static final double UP_POSITION = 1.0; + private static final double DOWN_POSITION = 0.57;//TODO: find positions + private static final double UP_POSITION = 0.75; public Elevator(HardwareMap hardwareMap){ elevatorServo = new BluServo("elevator"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index fd3e6629..35d2d038 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -6,12 +6,14 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; @Config public class Intake implements BluSubsystem, Subsystem { - private BluMotor motor; + private BluMotorWithEncoder leftMotor; + private BluMotorWithEncoder rightMotor; public boolean jammed; public static double JAM_CURRENT_THRESHOLD = 9800; // milliamps, adjust as needed public static double NOMINAL_VOLTAGE = 12.0; @@ -42,56 +44,67 @@ public State getState() { return state; } - public Intake(String motorName) { - motor = new BluMotor(motorName); + public Intake(String leftMotorName, String rightMotorName) { + leftMotor = new BluMotorWithEncoder(leftMotorName); + rightMotor = new BluMotorWithEncoder(rightMotorName); + rightMotor.setDirection(DcMotorSimple.Direction.REVERSE); state = State.IDlE; jammed = false; } @Override public void init() { - motor.init(); + leftMotor.init(); + rightMotor.init(); } @Override public void read() { - motor.read(); + leftMotor.read(); + rightMotor.read(); double currentVoltage = Robot.getInstance().getVoltage(); - double adjustedThreshold = JAM_CURRENT_THRESHOLD * (currentVoltage / NOMINAL_VOLTAGE); - jammed = (state == State.IN && motor.getCurrent() > JAM_CURRENT_THRESHOLD); // Jam detected, spit out the ball + jammed = (state == State.IN && leftMotor.getVelocity() > 100); // Jam detected, spit out the ball } @Override public void write() { if (jammed){ - motor.setPower(1); + leftMotor.setPower(-1); + rightMotor.setPower(-1); } else { switch(state){ case IN: - motor.setPower(-1); + leftMotor.setPower(1); + rightMotor.setPower(1); break; case OUT: - motor.setPower(1); + leftMotor.setPower(-1); + rightMotor.setPower(-1); break; case IDlE: - motor.setPower(0); + leftMotor.setPower(0); + rightMotor.setPower(0); break; } } - motor.write(); + leftMotor.write(); + rightMotor.write(); } @Override public void telemetry(Telemetry telemetry) { - motor.telemetry(); - telemetry.addData("Current", motor.getCurrent()); + leftMotor.telemetry(); + rightMotor.telemetry(); + telemetry.addData("Current", leftMotor.getCurrent()); } @Override public void reset() { - motor.setPower(0); - motor.write(); + leftMotor.setPower(0); + rightMotor.setPower(0); + leftMotor.write(); + rightMotor.write(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/BluServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/BluServoTest.java index ddbbe82e..08ff105b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/BluServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/BluServoTest.java @@ -9,7 +9,7 @@ @Config public class BluServoTest extends BluLinearOpMode { BluServo servo; - public static String name; + public static String name = "elevator"; public static double pos; public void initialize(){ servo = new BluServo(name); @@ -22,6 +22,8 @@ public void initializePeriodic(){ } public void periodic(){ + servo.read(); + if (driver1.pressedA()){ servo.setPos(pos); } @@ -33,5 +35,10 @@ public void periodic(){ servo.enable(); } } + + servo.write(); + telemetry.addData("Servo pos", servo.getPos()); + + telemetry.addData("Servo name", name); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PurePusuitTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PurePusuitTest.java new file mode 100644 index 00000000..2183d3f6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/PurePusuitTest.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import org.firstinspires.ftc.teamcode.blucru.common.pathing.Path; +import org.firstinspires.ftc.teamcode.blucru.common.pathing.PurePursuitSegment; +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +public class PurePusuitTest extends BluLinearOpMode { + Path currentPath; + @Override + public void initialize(){ + robot.clear(); + addSixWheel(); + } + + public void periodic(){ + if (driver1.pressedA()){ + currentPath = new TestPurePursuitPath().build().start(); + } + if (currentPath != null){ + currentPath.run(); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TestPurePursuitPath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TestPurePursuitPath.java new file mode 100644 index 00000000..88e19ec1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TestPurePursuitPath.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import org.firstinspires.ftc.teamcode.blucru.common.pathing.SixWheelPIDPathBuilder; +import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; + +public class TestPurePursuitPath extends SixWheelPIDPathBuilder { + Point2d[] pointsOnPath = {new Point2d(0, 30), new Point2d(30,30)}; + public TestPurePursuitPath(){ + super(); + this.addMappedPurePursuitPath(pointsOnPath, 5000); + } +} From a096a41b865f0a1a769a98d40f584cccf12bc7b3 Mon Sep 17 00:00:00 2001 From: Michael Date: Fri, 28 Nov 2025 13:41:46 -0500 Subject: [PATCH 055/123] tyshi --- .../blucru/common/subsytems/Robot.java | 5 +++ .../subsytems/transfer/LeftTransferServo.java | 11 +++--- .../transfer/MiddleTransferServo.java | 12 +++---- .../transfer/RightTransferServo.java | 12 +++---- .../common/subsytems/transfer/Transfer.java | 26 ++++++++------ .../subsytems/transfer/TransferServo.java | 10 +++--- .../AllTransferDownCommand.java | 7 ++-- .../AllTransferUpCommand.java | 7 ++-- .../blucru/opmodes/BluLinearOpMode.java | 3 ++ .../blucru/opmodes/test/kickerTest.java | 34 +++++++++++++++++++ 10 files changed, 84 insertions(+), 43 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/kickerTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 996fbb7d..3e1be39c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -152,6 +152,11 @@ public Shooter addShooter() { subsystems.add(shooter); return shooter; } + public Transfer addTransfer(){ + transfer = new Transfer(hwMap); + subsystems.add(transfer); + return transfer; + } public Intake addIntake(){ intake = new Intake("intakeLeft", "intakeRight"); subsystems.add(intake); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java index b0becd43..6b5992ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/LeftTransferServo.java @@ -2,17 +2,16 @@ public class LeftTransferServo extends TransferServo{ public LeftTransferServo(){ - super("transfer left"); + super("kickerLeft"); } @Override - // TODO: find tick delta for 90 degrees REMEMBER THIS IS +/- ticks of 90 degrees - ticks of 0 degrees / 90 degrees - double getTicksPerDeg() { - return 0; + double getVerticalPos() { + return 0.3; } @Override - double getVerticalPos() { - return 0; + double getBottomPos() { + return 0.78; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java index 8f52c8c7..82edf5f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/MiddleTransferServo.java @@ -2,17 +2,15 @@ public class MiddleTransferServo extends TransferServo{ public MiddleTransferServo(){ - super("transfer middle"); + super("kickerMiddle"); } - @Override - // TODO: find tick delta for 90 degrees REMEMBER THIS IS +/- ticks of 90 degrees - ticks of 0 degrees / 90 degrees - double getTicksPerDeg() { - return 0; + double getVerticalPos() { + return 0.8; } @Override - double getVerticalPos() { - return 0; + double getBottomPos() { + return 0.3; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java index c74a83e2..2c2c5a43 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/RightTransferServo.java @@ -2,17 +2,17 @@ public class RightTransferServo extends TransferServo{ public RightTransferServo(){ - super("transfer right"); + super("kickerRight"); } + @Override - // TODO: find tick delta for 90 degrees REMEMBER THIS IS +/- ticks of 90 degrees - ticks of 0 degrees / 90 degrees - double getTicksPerDeg() { - return 0; + double getVerticalPos() { + return 0.8; } @Override - double getVerticalPos() { - return 0; + double getBottomPos() { + return 0.3; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java index f79becbf..5661d96c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java @@ -25,30 +25,30 @@ public Transfer(HardwareMap hardwareMap) { public void leftSetDown() { leftState = State.DOWN; - setAngle(DOWN_POSITION); + transferServos[0].setBottom(); } public void leftSetUp() { leftState = State.UP; - setAngle(UP_POSITION); + transferServos[0].setVertical(); } public void middleSetDown() { middleState = State.DOWN; - setAngle(DOWN_POSITION); + transferServos[1].setBottom(); } public void middleSetUp() { middleState = State.UP; - setAngle(UP_POSITION); + transferServos[1].setVertical(); } public void rightSetDown() { rightState = State.DOWN; - setAngle(DOWN_POSITION); + transferServos[2].setBottom(); } public void rightSetUp() { rightState = State.UP; - setAngle(UP_POSITION); + transferServos[2].setVertical(); } public State getLeftState() { @@ -103,9 +103,15 @@ public void reset() { middleSetDown(); rightSetDown(); } - public void setAngle(double degrees){ - for(TransferServo servo:transferServos){ - servo.setAngle(degrees); - } + public void setAllDown(){ + leftSetDown(); + middleSetDown(); + rightSetDown(); + } + public void setAllUp(){ + leftSetUp(); + middleSetUp(); + rightSetUp(); } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java index 07c2cd06..8f516010 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/TransferServo.java @@ -6,10 +6,12 @@ public abstract class TransferServo extends BluServo { public TransferServo(String name) { super(name); } - void setAngle(double degrees){ - double pos = (degrees - 90.0)*getTicksPerDeg() + getVerticalPos(); - setPosition(pos); + void setBottom(){ + setPos(getBottomPos()); + } + void setVertical(){ + setPos(getVerticalPos()); } - abstract double getTicksPerDeg(); abstract double getVerticalPos(); + abstract double getBottomPos(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java index ff062c62..729deab2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java @@ -2,17 +2,14 @@ import com.arcrobotics.ftclib.command.InstantCommand; import com.arcrobotics.ftclib.command.ParallelCommandGroup; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; public class AllTransferDownCommand extends InstantCommand { public AllTransferDownCommand(){ super(()->{ - new ParallelCommandGroup( - new MiddleTransferDownCommand(), - new LeftTransferDownCommand(), - new RightTransferDownCommand() - ).schedule(); + Robot.getInstance().transfer.setAllDown(); }); addRequirements(Robot.getInstance().transfer); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java index d1bc84fd..80e415cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java @@ -2,17 +2,14 @@ import com.arcrobotics.ftclib.command.InstantCommand; import com.arcrobotics.ftclib.command.ParallelCommandGroup; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.Robot; public class AllTransferUpCommand extends InstantCommand { public AllTransferUpCommand(){ super(()->{ - new ParallelCommandGroup( - new MiddleTransferUpCommand(), - new LeftTransferUpCommand(), - new RightTransferUpCommand() - ).schedule(); + Robot.getInstance().transfer.setAllUp(); }); addRequirements(Robot.getInstance().transfer); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index bd3cb07d..ca64680c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -186,6 +186,9 @@ public void end(){ public void addIntake(){ intake = robot.addIntake(); } + public void addTransfer(){ + transfer = robot.addTransfer(); + } public void addTurret(){turret = robot.addTurret();} public void addObeliskTagDetector(){obeliskTagDetector = robot.addObeliskTagDetector();} public void addLLTagDetector(){llTagDetector = robot.addLLTagDetector();} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/kickerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/kickerTest.java new file mode 100644 index 00000000..3efa59c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/kickerTest.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; + +@TeleOp(group = "test") +public class kickerTest extends BluLinearOpMode { + + public void initialize(){ + robot.clear(); + addTransfer(); + } + + public void periodic(){ + if(gamepad1.dpad_left){ + transfer.leftSetUp(); + } + if(gamepad1.dpad_down){ + transfer.middleSetUp(); + } + + if(gamepad1.dpad_right){ + transfer.rightSetUp(); + } + if(gamepad1.left_bumper){ + transfer.setAllUp(); + } + if(gamepad1.right_bumper){ + transfer.setAllDown(); + } + } + +} From 805b9f8c6e259f271952495b3a08009e7309296a Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 28 Nov 2025 13:53:01 -0500 Subject: [PATCH 056/123] fixing a stash conflict --- .../transfer/transferCommands/AllTransferDownCommand.java | 1 + .../transfer/transferCommands/AllTransferUpCommand.java | 1 + 2 files changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java index 729deab2..980fec9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java @@ -11,6 +11,7 @@ public AllTransferDownCommand(){ super(()->{ Robot.getInstance().transfer.setAllDown(); }); + addRequirements(Robot.getInstance().transfer); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java index 80e415cd..cb30b03e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferUpCommand.java @@ -11,6 +11,7 @@ public AllTransferUpCommand(){ super(()->{ Robot.getInstance().transfer.setAllUp(); }); + addRequirements(Robot.getInstance().transfer); } } From 97ee2178452b5921c5585be017086755f75a91a9 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 28 Nov 2025 13:59:03 -0500 Subject: [PATCH 057/123] transfer fixes, brushland stuff, fixed cubic scaling --- .../hardware/BluBrushlandLabsColorSensor.java | 49 ++++ .../blucru/common/subsytems/Robot.java | 7 +- .../sixWheelDrive/SixWheelDrive.java | 5 +- .../common/subsytems/elevator/Elevator.java | 9 +- .../ElevatorTransferWithJiggleCommand.java | 70 ++++++ .../common/subsytems/intake/Intake.java | 2 +- .../AllTransferDownCommand.java | 2 +- .../blucru/opmodes/BluLinearOpMode.java | 1 + .../test/ConfigureColorRangefinder.java | 227 ++++++++++++++++++ .../blucru/opmodes/test/intakeTest.java | 24 +- 10 files changed, 379 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/BluBrushlandLabsColorSensor.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorTransferWithJiggleCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ConfigureColorRangefinder.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/BluBrushlandLabsColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/BluBrushlandLabsColorSensor.java new file mode 100644 index 00000000..2994a7c9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/hardware/BluBrushlandLabsColorSensor.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.blucru.common.hardware; + +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.DigitalChannel; + +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; + +public class BluBrushlandLabsColorSensor implements BluHardwareDevice{ + + private DigitalChannel pin0, pin1; + private boolean state1; + private boolean state2; + + public BluBrushlandLabsColorSensor(String pin0, String pin1){ + this.pin0 = Globals.hwMap.get(DigitalChannel.class, pin0); + this.pin1 = Globals.hwMap.get(DigitalChannel.class, pin1); + } + + @Override + public void init() { + + } + + @Override + public void read() { + state1 = pin0.getState(); + state2 = pin1.getState(); + } + + @Override + public void write() { + } + + public boolean ballDetected(){ + return state1 || state2; + } + + @Override + public void telemetry() { + } + + public boolean getPin0State(){ + return state1; + } + + public boolean getPin1State(){ + return state2; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java index 3e1be39c..e7beefe6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/Robot.java @@ -181,6 +181,9 @@ public LimelightObeliskTagDetector addLLTagDetector(){ } - - + public Elevator addElevator() { + elevator = new Elevator(); + subsystems.add(elevator); + return elevator; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index 05fef1c4..74609c9c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -16,9 +16,6 @@ public class SixWheelDrive extends SixWheelDriveBase implements Subsystem { private PurePursuitComputer computer; private final double LOOK_AHEAD_DIST = 10; private SixWheelPID pid; - public static double a = 0.6; - public static double b = 0.01; - public static double c = 0.39; public SixWheelDrive(){ super(); drivePower = 1; @@ -107,6 +104,6 @@ public void telemetry(Telemetry telemetry) { } public double cubicScaling(double value){ - return a * value * value * value + b * value * value + c * value; + return value * value * value; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java index 5e6f2b61..3313af6e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java @@ -4,15 +4,17 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.blucru.common.hardware.BluBrushlandLabsColorSensor; import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; public class Elevator implements BluSubsystem, Subsystem { private BluServo elevatorServo; + private BluBrushlandLabsColorSensor leftSensor, middleSensor, rightSensor; private static final double DOWN_POSITION = 0.57;//TODO: find positions - private static final double UP_POSITION = 0.75; + private static final double UP_POSITION = 0.8; - public Elevator(HardwareMap hardwareMap){ + public Elevator(){ elevatorServo = new BluServo("elevator"); } @@ -23,6 +25,9 @@ public void setUp(){ public void setDown(){ elevatorServo.setPos(DOWN_POSITION); } + public boolean ballInElevatorSlot(){ + return leftSensor.ballDetected() || middleSensor.ballDetected() || rightSensor.ballDetected(); + } @Override public void init() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorTransferWithJiggleCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorTransferWithJiggleCommand.java new file mode 100644 index 00000000..6198d76f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/ElevatorTransferWithJiggleCommand.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator; + +import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; +import com.arcrobotics.ftclib.command.WaitCommand; + +public class ElevatorTransferWithJiggleCommand extends InstantCommand { + + public ElevatorTransferWithJiggleCommand(){ + super( + () ->{ + new SequentialCommandGroup( + new ElevatorUpCommand(), + new WaitCommand(500), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(500), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100), + new ElevatorDownCommand(), + new WaitCommand(100), + new ElevatorUpCommand(), + new WaitCommand(100) + ).schedule(); + } + ); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index 35d2d038..06bc2fb1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -47,7 +47,7 @@ public State getState() { public Intake(String leftMotorName, String rightMotorName) { leftMotor = new BluMotorWithEncoder(leftMotorName); rightMotor = new BluMotorWithEncoder(rightMotorName); - rightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + leftMotor.setDirection(DcMotorSimple.Direction.REVERSE); state = State.IDlE; jammed = false; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java index 980fec9e..43f2eee8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/transferCommands/AllTransferDownCommand.java @@ -11,7 +11,7 @@ public AllTransferDownCommand(){ super(()->{ Robot.getInstance().transfer.setAllDown(); }); - + addRequirements(Robot.getInstance().transfer); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java index ca64680c..ee67eb7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/BluLinearOpMode.java @@ -193,6 +193,7 @@ public void addTransfer(){ public void addObeliskTagDetector(){obeliskTagDetector = robot.addObeliskTagDetector();} public void addLLTagDetector(){llTagDetector = robot.addLLTagDetector();} public void addSixWheel(){sixWheel = robot.addSixWheelDrivetrain();} + public void addElevator(){elevator = robot.addElevator();} public void enableDash(){ telemetry = new MultipleTelemetry(FtcDashboard.getInstance().getTelemetry(), telemetry); Globals.telemetry = telemetry; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ConfigureColorRangefinder.java new file mode 100644 index 00000000..f4e1362b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/ConfigureColorRangefinder.java @@ -0,0 +1,227 @@ +package org.firstinspires.ftc.teamcode.blucru.opmodes.test; + +import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; + +/** + * + * Obtained from Brushland Labs Website + * + * */ +@TeleOp +public class ConfigureColorRangefinder extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "Color")); + waitForStart(); + /* Using this example configuration, you can detect both artifact colors based on which pin is reading true: + pin0 --> purple + pin1 --> green */ + crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 160 / 360.0 * 255, 190 / 360.0 * 255); // purple + crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 50); // 50mm or closer requirement + crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 110 / 360.0 * 255, 140 / 360.0 * 255); // green + crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 50); // 50mm or closer requirement + } +} + +/** + * Helper class for configuring the Brushland Labs Color Rangefinder. + * Online documentation: ... + */ +class ColorRangefinder { + private final I2cDeviceSynchSimple i2c; + + public ColorRangefinder(RevColorSensorV3 emulator) { + this.i2c = emulator.getDeviceClient(); + this.i2c.enableWriteCoalescing(true); + } + + /** + * Configure Pin 0 to be in digital mode, and add a threshold. + * Multiple thresholds can be added to the same pin by calling this function repeatedly. + * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm). + */ + public void setPin0Digital(DigitalMode digitalMode, double lowerBound, double higherBound) { + setDigital(PinNum.PIN0, digitalMode, lowerBound, higherBound); + } + + /** + * Configure Pin 1 to be in digital mode, and add a threshold. + * Multiple thresholds can be added to the same pin by calling this function repeatedly. + * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm). + */ + public void setPin1Digital(DigitalMode digitalMode, double lowerBound, double higherBound) { + setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound); + } + + /** + * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger. + * This is most useful when we want to know if an object is both close and the correct color. + */ + public void setPin0DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) { + setPin0Digital(digitalMode, mmRequirement, mmRequirement); + } + + /** + * Sets the maximum distance (in millimeters) within which an object must be located for Pin 1's thresholds to trigger. + * This is most useful when we want to know if an object is both close and the correct color. + */ + public void setPin1DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) { + setPin1Digital(digitalMode, mmRequirement, mmRequirement); + } + + /** + * Invert the hue value before thresholding it, meaning that the colors become their opposite. + * This is useful if we want to threshold red; instead of having two thresholds we would invert + * the color and look for blue. + */ + public void setPin0InvertHue() { + setPin0DigitalMaxDistance(DigitalMode.HSV, 200); + } + + /** + * Invert the hue value before thresholding it, meaning that the colors become their opposite. + * This is useful if we want to threshold red; instead of having two thresholds we would invert + * the color and look for blue. + */ + public void setPin1InvertHue() { + setPin1DigitalMaxDistance(DigitalMode.HSV, 200); + } + + /** + * The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog. + * For the full range of that channel, leave the denominator as 65535 for colors or 100 for distance. + * Smaller values will clip off higher ranges of the data in exchange for higher resolution within a lower range. + */ + public void setPin0Analog(AnalogMode analogMode, int denominator) { + byte denom0 = (byte) (denominator & 0xFF); + byte denom1 = (byte) ((denominator & 0xFF00) >> 8); + i2c.write(PinNum.PIN0.modeAddress, new byte[]{analogMode.value, denom0, denom1}); + } + + /** + * Configure Pin 0 as analog output of one of the six data channels. + * To read analog, make sure the physical switch on the sensor is flipped away from the + * connector side. + */ + public void setPin0Analog(AnalogMode analogMode) { + setPin0Analog(analogMode, analogMode == AnalogMode.DISTANCE ? 100 : 0xFFFF); + } + + public float[] getCalibration() { + java.nio.ByteBuffer bytes = + java.nio.ByteBuffer.wrap(i2c.read(CALIB_A_VAL_0, 16)).order(java.nio.ByteOrder.LITTLE_ENDIAN); + return new float[]{bytes.getFloat(), bytes.getFloat(), bytes.getFloat(), bytes.getFloat()}; + } + + /** + * Save a brightness value of the LED to the sensor. + * + * @param value brightness between 0-255 + */ + public void setLedBrightness(int value) { + i2c.write8(LED_BRIGHTNESS, value); + } + + /** + * Change the I2C address at which the sensor will be found. The address can be reset to the + * default of 0x52 by holding the reset button. + * + * @param value new I2C address from 1 to 127 + */ + public void setI2cAddress(int value) { + i2c.write8(I2C_ADDRESS_REG, value << 1); + } + + /** + * Read distance via I2C + * @return distance in millimeters + */ + public double readDistance() { + java.nio.ByteBuffer bytes = + java.nio.ByteBuffer.wrap(i2c.read(PS_DISTANCE_0, 4)).order(java.nio.ByteOrder.LITTLE_ENDIAN); + return bytes.getFloat(); + } + + private void setDigital( + PinNum pinNum, + DigitalMode digitalMode, + double lowerBound, + double higherBound + ) { + int lo, hi; + if (lowerBound == higherBound) { + lo = (int) lowerBound; + hi = (int) higherBound; + } else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255 + lo = (int) Math.round(lowerBound / 255.0 * 65535); + hi = (int) Math.round(higherBound / 255.0 * 65535); + } else { // distance in mm + float[] calib = getCalibration(); + if (lowerBound < .5) hi = 2048; + else hi = rawFromDistance(calib[0], calib[1], calib[2], calib[3], lowerBound); + lo = rawFromDistance(calib[0], calib[1], calib[2], calib[3], higherBound); + } + + byte lo0 = (byte) (lo & 0xFF); + byte lo1 = (byte) ((lo & 0xFF00) >> 8); + byte hi0 = (byte) (hi & 0xFF); + byte hi1 = (byte) ((hi & 0xFF00) >> 8); + i2c.write(pinNum.modeAddress, new byte[]{digitalMode.value, lo0, lo1, hi0, hi1}); + try { + Thread.sleep(25); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } + + private double root(double n, double v) { + double val = Math.pow(v, 1.0 / Math.abs(n)); + if (n < 0) val = 1.0 / val; + return val; + } + + private int rawFromDistance(float a, float b, float c, float x0, double mm) { + return (int) (root(b, (mm - c) / a) + x0); + } + + private enum PinNum { + PIN0(0x28), PIN1(0x2D); + + private final byte modeAddress; + + PinNum(int modeAddress) { + this.modeAddress = (byte) modeAddress; + } + } + + // other writeable registers + private static final byte CALIB_A_VAL_0 = 0x32; + private static final byte PS_DISTANCE_0 = 0x42; + private static final byte LED_BRIGHTNESS = 0x46; + private static final byte I2C_ADDRESS_REG = 0x47; + + public static int invertHue(int hue360) { + return ((hue360 - 180) % 360); + } + + public enum DigitalMode { + RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6); + public final byte value; + + DigitalMode(int value) { + this.value = (byte) value; + } + } + + public enum AnalogMode { + RED(13), BLUE(14), GREEN(15), ALPHA(16), HSV(17), DISTANCE(18); + public final byte value; + + AnalogMode(int value) { + this.value = (byte) value; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java index 2a0de977..61319805 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java @@ -1,7 +1,12 @@ package org.firstinspires.ftc.teamcode.blucru.opmodes.test; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorDownCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStartCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStopCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorTransferWithJiggleCommand; import org.firstinspires.ftc.teamcode.blucru.opmodes.BluLinearOpMode; @TeleOp(group = "test") @@ -10,17 +15,22 @@ public class intakeTest extends BluLinearOpMode { public void initialize(){ robot.clear(); addIntake(); + addElevator(); } public void periodic(){ - if(gamepad1.left_trigger > 0.2){ - intake.setIn(); + if (driver1.pressedA()){ + new SequentialCommandGroup( + new IntakeStartCommand(), + new ElevatorDownCommand() + ).schedule(); } - else if(gamepad1.right_trigger > 0.2){ - intake.setOut(); - } - else{ - intake.stop(); + + if (driver1.pressedY()){ + new SequentialCommandGroup( + new IntakeStopCommand(), + new ElevatorTransferWithJiggleCommand() + ).schedule(); } } From 1028fe815e1c8198d64087331e53e9d854605595 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 28 Nov 2025 20:41:43 -0500 Subject: [PATCH 058/123] fixed intakeTest, fixed elevator positions, shooter now has 2 motors --- .../common/subsytems/elevator/Elevator.java | 4 +- .../common/subsytems/intake/Intake.java | 2 +- .../common/subsytems/shooter/Shooter.java | 48 +++++++++++-------- .../blucru/opmodes/test/HoodTest.java | 6 +-- .../blucru/opmodes/test/intakeTest.java | 10 +++- 5 files changed, 42 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java index 3313af6e..f83dd14d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java @@ -11,8 +11,8 @@ public class Elevator implements BluSubsystem, Subsystem { private BluServo elevatorServo; private BluBrushlandLabsColorSensor leftSensor, middleSensor, rightSensor; - private static final double DOWN_POSITION = 0.57;//TODO: find positions - private static final double UP_POSITION = 0.8; + private static final double DOWN_POSITION = 0;//TODO: find positions + private static final double UP_POSITION = 0.3; public Elevator(){ elevatorServo = new BluServo("elevator"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index 06bc2fb1..35d2d038 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -47,7 +47,7 @@ public State getState() { public Intake(String leftMotorName, String rightMotorName) { leftMotor = new BluMotorWithEncoder(leftMotorName); rightMotor = new BluMotorWithEncoder(rightMotorName); - leftMotor.setDirection(DcMotorSimple.Direction.REVERSE); + rightMotor.setDirection(DcMotorSimple.Direction.REVERSE); state = State.IDlE; jammed = false; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index c030af7f..952131db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -2,7 +2,6 @@ import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.command.Subsystem; -import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; @@ -22,8 +21,9 @@ public class Shooter implements BluSubsystem, Subsystem { public static final double SERVO_ANGLE_DELTA = TOP_ANGLE - ZERO_ANGLE; public static final double SERVO_POS = SERVO_ROT_TO_HOOD_ROT * SERVO_ANGLE_DELTA / 255.0; - private BluMotorWithEncoder shooter; - public BluServo hood; + private BluMotorWithEncoder shooter1; + private BluMotorWithEncoder shooter2; + public BluServo hoodLeft; enum State{ IDLE, @@ -33,8 +33,9 @@ enum State{ double targetVel = 0; ShooterVelocityPID pid; public Shooter(){ - shooter = new BluMotorWithEncoder("shooter"); - hood = new BluServo("hood", Servo.Direction.FORWARD); + shooter1 = new BluMotorWithEncoder("shooter1", DcMotorSimple.Direction.REVERSE); + shooter2 = new BluMotorWithEncoder("shooter2", DcMotorSimple.Direction.REVERSE); + hoodLeft = new BluServo("hoodLeft", Servo.Direction.FORWARD); state = State.IDLE; pid = new ShooterVelocityPID(p,i,d,f); } @@ -44,12 +45,14 @@ public double shooterAngleToPos(double angle){ @Override public void init() { - shooter.init(); + shooter1.init(); + shooter2.init(); } @Override public void read() { - shooter.read(); + shooter1.read(); + shooter2.read(); } @Override @@ -59,16 +62,18 @@ public void write() { targetVel = 0; break; case VELOCITY: - shooter.setPower(pid.calculateDeltaPower(shooter.getVel(), targetVel)); - Globals.telemetry.addData("delta", pid.calculateDeltaPower(shooter.getVel(), targetVel)); + shooter1.setPower(pid.calculateDeltaPower(shooter1.getVel(), targetVel)); + shooter2.setPower(shooter1.getPower()); + Globals.telemetry.addData("delta", pid.calculateDeltaPower(shooter1.getVel(), targetVel)); } - shooter.write(); - hood.write(); + shooter1.write(); + shooter2.write(); + hoodLeft.write(); } public void shoot(double power){ - shooter.setPower(power); + shooter1.setPower(power); } public void shootWithVelocity(double vel){ targetVel = vel; @@ -76,36 +81,39 @@ public void shootWithVelocity(double vel){ } public double getVel(){ - return shooter.getVel(); + return shooter1.getVel(); } public double getPower(){ - return shooter.getPower(); + return shooter1.getPower(); } public void rampDownShooter(){ - shooter.setPower(0); + shooter1.setPower(0); + shooter2.setPower(0); } public void setHoodAngle(double angle){ setHoodServoPos(shooterAngleToPos(angle)); } public void setHoodServoPos(double pos){ - hood.setPos(pos); + hoodLeft.setPos(pos); } public double getHoodAngle(){ - return Globals.convertServoPosToAngle(255,hood.getPos()); + return Globals.convertServoPosToAngle(255, hoodLeft.getPos()); } @Override public void telemetry(Telemetry telemetry) { - telemetry.addData("Shooter power", shooter.getPower()); + telemetry.addData("Shooter power", shooter1.getPower()); } @Override public void reset() { - shooter.reset(); - shooter.read(); + shooter1.reset(); + shooter2.reset(); + shooter2.read(); + shooter1.read(); } public void updatePID(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java index 890f22b7..b2bd3613 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java @@ -31,9 +31,9 @@ public void periodic() { @Override public void telemetry(){ - telemetry.addData("Servo dir", shooter.hood.getDir()); - telemetry.addData("Servo pos", shooter.hood.getServoPos()); - telemetry.addData("Thought pos", shooter.hood.getPos()); + telemetry.addData("Servo dir", shooter.hoodLeft.getDir()); + telemetry.addData("Servo pos", shooter.hoodLeft.getServoPos()); + telemetry.addData("Thought pos", shooter.hoodLeft.getPos()); telemetry.addData("Angle", pos); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java index 61319805..80913575 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/intakeTest.java @@ -1,9 +1,11 @@ package org.firstinspires.ftc.teamcode.blucru.opmodes.test; import com.arcrobotics.ftclib.command.SequentialCommandGroup; +import com.arcrobotics.ftclib.command.WaitCommand; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorDownCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorUpCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStartCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStopCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorTransferWithJiggleCommand; @@ -16,6 +18,10 @@ public void initialize(){ robot.clear(); addIntake(); addElevator(); + elevator.setUp(); + elevator.write(); + elevator.setDown(); + elevator.write(); } public void periodic(){ @@ -28,8 +34,8 @@ public void periodic(){ if (driver1.pressedY()){ new SequentialCommandGroup( - new IntakeStopCommand(), - new ElevatorTransferWithJiggleCommand() + new IntakeStopCommand(), + new ElevatorUpCommand() ).schedule(); } } From fe92e4178b55af498c52f21b2b2df04daebd9925 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Fri, 28 Nov 2025 22:03:03 -0500 Subject: [PATCH 059/123] fixed shooter for 3 hoods --- .../blucru/common/subsytems/shooter/Hood.java | 44 +++++++++++++++++++ .../common/subsytems/shooter/HoodLeft.java | 16 +++++++ .../common/subsytems/shooter/HoodMiddle.java | 16 +++++++ .../common/subsytems/shooter/HoodRight.java | 16 +++++++ .../common/subsytems/shooter/Shooter.java | 24 ++++++---- .../blucru/opmodes/test/HoodTest.java | 3 -- 6 files changed, 107 insertions(+), 12 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Hood.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Hood.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Hood.java new file mode 100644 index 00000000..c5f692ba --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Hood.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; + +import org.firstinspires.ftc.teamcode.blucru.common.hardware.servo.BluServo; + +public abstract class Hood { + + static final double ZERO_ANGLE = 26; + static final double TOP_ANGLE = 50; + abstract double BOTTOM_ANGLE_POS(); + abstract double TOP_ANGLE_POS(); + double ANGLE_DELTA = TOP_ANGLE - ZERO_ANGLE; + double SERVO_POS_DELTA = TOP_ANGLE_POS() - BOTTOM_ANGLE_POS(); + double SLOPE = SERVO_POS_DELTA/ANGLE_DELTA; + private BluServo servo; + public Hood(String servoName){ + servo = new BluServo(servoName); + } + + public void read(){ + servo.read(); + } + + public void write(){ + servo.write(); + } + + public double shooterAngleToPos(double angle){ + return SLOPE * angle - ZERO_ANGLE * SLOPE + BOTTOM_ANGLE_POS(); + } + + public void setShooterAngle(double angle){ + servo.setPos(shooterAngleToPos(angle)); + } + + public double getHoodAngle(){ + return shooterAngleToPos(servo.getPos()); + } + + public void reset(){ + + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java new file mode 100644 index 00000000..8cb97a58 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; + +public class HoodLeft extends Hood{ + + public double BOTTOM_ANGLE_POS(){ + return 0; + } + public double TOP_ANGLE_POS(){ + return 1; + } + + public HoodLeft(){ + super("hoodLeft"); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java new file mode 100644 index 00000000..e74a6815 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; + +public class HoodMiddle extends Hood{ + + public double BOTTOM_ANGLE_POS(){ + return 0; + } + public double TOP_ANGLE_POS(){ + return 1; + } + + public HoodMiddle(){ + super("hoodMiddle"); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java new file mode 100644 index 00000000..e80b42dc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter; + +public class HoodRight extends Hood{ + + public double BOTTOM_ANGLE_POS(){ + return 1; + } + public double TOP_ANGLE_POS(){ + return 0; + } + + public HoodRight(){ + super("hoodRight"); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index 952131db..36ba841d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -23,7 +23,9 @@ public class Shooter implements BluSubsystem, Subsystem { private BluMotorWithEncoder shooter1; private BluMotorWithEncoder shooter2; - public BluServo hoodLeft; + public HoodLeft hoodLeft; + public HoodMiddle hoodMiddle; + public HoodRight hoodRight; enum State{ IDLE, @@ -35,7 +37,9 @@ enum State{ public Shooter(){ shooter1 = new BluMotorWithEncoder("shooter1", DcMotorSimple.Direction.REVERSE); shooter2 = new BluMotorWithEncoder("shooter2", DcMotorSimple.Direction.REVERSE); - hoodLeft = new BluServo("hoodLeft", Servo.Direction.FORWARD); + hoodLeft = new HoodLeft(); + hoodMiddle = new HoodMiddle(); + hoodRight = new HoodRight(); state = State.IDLE; pid = new ShooterVelocityPID(p,i,d,f); } @@ -70,6 +74,8 @@ public void write() { shooter1.write(); shooter2.write(); hoodLeft.write(); + hoodMiddle.write(); + hoodRight.write(); } public void shoot(double power){ @@ -92,14 +98,13 @@ public void rampDownShooter(){ shooter2.setPower(0); } public void setHoodAngle(double angle){ - setHoodServoPos(shooterAngleToPos(angle)); - } - public void setHoodServoPos(double pos){ - hoodLeft.setPos(pos); + hoodLeft.setShooterAngle(angle); + hoodMiddle.setShooterAngle(angle); + hoodRight.setShooterAngle(angle); } public double getHoodAngle(){ - return Globals.convertServoPosToAngle(255, hoodLeft.getPos()); + return hoodLeft.getHoodAngle(); } @@ -112,8 +117,9 @@ public void telemetry(Telemetry telemetry) { public void reset() { shooter1.reset(); shooter2.reset(); - shooter2.read(); - shooter1.read(); + hoodLeft.reset(); + hoodMiddle.reset(); + hoodRight.reset(); } public void updatePID(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java index b2bd3613..2d92e75c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/HoodTest.java @@ -31,9 +31,6 @@ public void periodic() { @Override public void telemetry(){ - telemetry.addData("Servo dir", shooter.hoodLeft.getDir()); - telemetry.addData("Servo pos", shooter.hoodLeft.getServoPos()); - telemetry.addData("Thought pos", shooter.hoodLeft.getPos()); telemetry.addData("Angle", pos); } } From 58f244b191a1219f633ea3eb7a56519bda1f0021 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 29 Nov 2025 10:58:37 -0500 Subject: [PATCH 060/123] general tele cycle without actually shooting --- .../blucru/common/commands/TransferCommand.java | 10 +++++++--- .../common/subsytems/elevator/Elevator.java | 2 ++ .../common/subsytems/transfer/Transfer.java | 2 ++ .../blucru/opmodes/{TeleOp.java => Tele.java} | 16 ++++++++++------ 4 files changed, 21 insertions(+), 9 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/{TeleOp.java => Tele.java} (86%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java index ce7075c1..ac61013d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java @@ -4,6 +4,7 @@ import com.arcrobotics.ftclib.command.SequentialCommandGroup; import com.arcrobotics.ftclib.command.WaitCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorDownCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorUpCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStopCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands.ShootWithVelocityCommand; @@ -15,12 +16,15 @@ public class TransferCommand extends InstantCommand { public TransferCommand(){ super(() -> { new SequentialCommandGroup( + new AllTransferDownCommand(), + new WaitCommand(200), new ElevatorUpCommand(), new IntakeStopCommand(), - new AllTransferDownCommand(), - new WaitCommand(900), + new WaitCommand(500), + new ElevatorDownCommand() + /**new WaitCommand(900), new LockOnGoalCommand(), - new ShootWithVelocityCommand(2500) + new ShootWithVelocityCommand(2500)*/ ).schedule(); }); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java index f83dd14d..7b1256f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/elevator/Elevator.java @@ -16,6 +16,8 @@ public class Elevator implements BluSubsystem, Subsystem { public Elevator(){ elevatorServo = new BluServo("elevator"); + setDown(); + write(); } public void setUp(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java index 5661d96c..ca001ed5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/transfer/Transfer.java @@ -21,6 +21,8 @@ public enum State { private State rightState; public Transfer(HardwareMap hardwareMap) { transferServos = new TransferServo[]{new LeftTransferServo(), new MiddleTransferServo(), new RightTransferServo()}; + setAllDown(); + write(); } public void leftSetDown() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java similarity index 86% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/TeleOp.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java index 9940d8fb..4cdda0af 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java @@ -2,15 +2,17 @@ import com.arcrobotics.ftclib.command.SequentialCommandGroup; import com.arcrobotics.ftclib.command.WaitCommand; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.sfdev.assembly.state.StateMachine; import com.sfdev.assembly.state.StateMachineBuilder; import org.firstinspires.ftc.teamcode.blucru.common.commands.IntakeCommand; import org.firstinspires.ftc.teamcode.blucru.common.commands.ShootBallsCommand; import org.firstinspires.ftc.teamcode.blucru.common.commands.TransferCommand; -import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStartCommand; -public class TeleOp extends BluLinearOpMode{ +@TeleOp (group = "a") + +public class Tele extends BluLinearOpMode{ StateMachine sm; @@ -25,9 +27,11 @@ public void initialize(){ robot.clear(); addSixWheel(); addIntake(); + addElevator(); + addTransfer(); addShooter(); - addLLTagDetector(); - addTurret(); + //addLLTagDetector(); + //addTurret(); sm = new StateMachineBuilder() .state(State.IDLE) .transition(() -> driver1.pressedRightBumper(), State.INTAKING, () ->{ @@ -37,7 +41,7 @@ public void initialize(){ .transition(() -> driver1.pressedOptions(), State.IDLE, () -> { robot.idleRobot(); }) - .transition(() -> driver2.pressedA(), State.DRIVING_TO_SHOOT, () -> { + .transition(() -> driver1.pressedRightBumper(), State.DRIVING_TO_SHOOT, () -> { new TransferCommand().schedule(); }) .state(State.DRIVING_TO_SHOOT) @@ -47,7 +51,7 @@ public void initialize(){ .transition(() -> driver1.pressedRightBumper(), State.INTAKING, () -> { new SequentialCommandGroup( new ShootBallsCommand(), - new WaitCommand(100), + new WaitCommand(400), new IntakeCommand() ).schedule(); }) From f522403658db5ae7f1f01e121ece6a6a2bec8cf1 Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 29 Nov 2025 15:19:17 -0500 Subject: [PATCH 061/123] hood angle changes --- .../teamcode/blucru/common/subsytems/shooter/HoodLeft.java | 4 ++-- .../teamcode/blucru/common/subsytems/shooter/HoodMiddle.java | 4 ++-- .../teamcode/blucru/common/subsytems/shooter/HoodRight.java | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java index 8cb97a58..3f1ed7cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodLeft.java @@ -3,10 +3,10 @@ public class HoodLeft extends Hood{ public double BOTTOM_ANGLE_POS(){ - return 0; + return 0.78; } public double TOP_ANGLE_POS(){ - return 1; + return 0.15; } public HoodLeft(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java index e74a6815..a5b56628 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodMiddle.java @@ -3,10 +3,10 @@ public class HoodMiddle extends Hood{ public double BOTTOM_ANGLE_POS(){ - return 0; + return 0.8; } public double TOP_ANGLE_POS(){ - return 1; + return 0.18; } public HoodMiddle(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java index e80b42dc..8f6b999b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/HoodRight.java @@ -3,10 +3,10 @@ public class HoodRight extends Hood{ public double BOTTOM_ANGLE_POS(){ - return 1; + return 0.18; } public double TOP_ANGLE_POS(){ - return 0; + return 0.8; } public HoodRight(){ From a8a265a8023404841dcb061b92dc2b46650f0817 Mon Sep 17 00:00:00 2001 From: Deven Balaji Date: Sat, 29 Nov 2025 15:20:37 -0500 Subject: [PATCH 062/123] michael commit on devens computer --- .../blucru/common/commands/TransferCommand.java | 9 +++++---- .../drivetrain/sixWheelDrive/SixWheelDrive.java | 3 ++- .../drivetrain/sixWheelDrive/SixWheelDriveBase.java | 11 +++++------ .../drivetrain/sixWheelDrive/SixWheelKinematics.java | 2 +- .../blucru/common/subsytems/intake/Intake.java | 6 +++--- .../blucru/common/subsytems/shooter/Shooter.java | 2 +- .../ftc/teamcode/blucru/opmodes/Tele.java | 10 ++++++---- .../blucru/opmodes/test/SixWheelManualDrive.java | 2 +- 8 files changed, 24 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java index ac61013d..73fdd292 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/commands/TransferCommand.java @@ -7,6 +7,7 @@ import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorDownCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.elevator.ElevatorUpCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.intake.IntakeStopCommand; +import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands.SetHoodAngleCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.shooter.shooterCommands.ShootWithVelocityCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.transfer.transferCommands.AllTransferDownCommand; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.turret.turretCommands.LockOnGoalCommand; @@ -21,10 +22,10 @@ public TransferCommand(){ new ElevatorUpCommand(), new IntakeStopCommand(), new WaitCommand(500), - new ElevatorDownCommand() - /**new WaitCommand(900), - new LockOnGoalCommand(), - new ShootWithVelocityCommand(2500)*/ + new ElevatorDownCommand(), + new WaitCommand(900), + new ShootWithVelocityCommand(1500), + new SetHoodAngleCommand(30) ).schedule(); }); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java index 74609c9c..db76a768 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDrive.java @@ -7,6 +7,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.PurePursuitComputer; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive.purePursuit.SixWheelPID; +import org.firstinspires.ftc.teamcode.blucru.common.util.Globals; import org.firstinspires.ftc.teamcode.blucru.common.util.Point2d; import org.firstinspires.ftc.teamcode.blucru.common.util.Pose2d; @Config @@ -104,6 +105,6 @@ public void telemetry(Telemetry telemetry) { } public double cubicScaling(double value){ - return value * value * value; + return 64 / 27.0 * value * value * value; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java index d6f7642d..6ebabbb8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelDriveBase.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.blucru.common.subsytems.drivetrain.sixWheelDrive; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; @@ -30,15 +31,13 @@ public enum State{ State dtState; public SixWheelDriveBase(){ - this(new BluMotor(Globals.flMotorName), - new BluMotor(Globals.frMotorName), - new BluMotor(Globals.blMotorName), - new BluMotor(Globals.brMotorName)); + this(new BluMotor(Globals.flMotorName, DcMotorSimple.Direction.REVERSE, DcMotor.ZeroPowerBehavior.BRAKE), + new BluMotor(Globals.frMotorName, DcMotorSimple.Direction.FORWARD, DcMotor.ZeroPowerBehavior.BRAKE), + new BluMotor(Globals.blMotorName, DcMotorSimple.Direction.REVERSE, DcMotor.ZeroPowerBehavior.BRAKE), + new BluMotor(Globals.brMotorName, DcMotorSimple.Direction.FORWARD, DcMotor.ZeroPowerBehavior.BRAKE)); } private SixWheelDriveBase(BluMotor fl, BluMotor fr, BluMotor bl, BluMotor br){ dtMotors = new BluMotor[]{fl, fr, bl, br}; - fr.setDirection(DcMotorSimple.Direction.REVERSE); - br.setDirection(DcMotorSimple.Direction.REVERSE); localizer = new Pinpoint("pinpoint"); dtState = State.IDLE; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelKinematics.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelKinematics.java index e42e7a4d..d0e3a118 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelKinematics.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/sixWheelDrive/SixWheelKinematics.java @@ -8,7 +8,7 @@ public static double[] getPowers(double x, double rot){ double max = Math.max(Math.abs(left), Math.abs(right)); - if (max>0){ + if (max>1){ left /= max; right /= max; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java index 35d2d038..004a6b4f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/intake/Intake.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.R; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotor; import org.firstinspires.ftc.teamcode.blucru.common.hardware.motor.BluMotorWithEncoder; import org.firstinspires.ftc.teamcode.blucru.common.subsytems.BluSubsystem; @@ -45,9 +46,8 @@ public State getState() { } public Intake(String leftMotorName, String rightMotorName) { - leftMotor = new BluMotorWithEncoder(leftMotorName); - rightMotor = new BluMotorWithEncoder(rightMotorName); - rightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + leftMotor = new BluMotorWithEncoder(leftMotorName, DcMotorSimple.Direction.FORWARD); + rightMotor = new BluMotorWithEncoder(rightMotorName, DcMotorSimple.Direction.REVERSE); state = State.IDlE; jammed = false; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java index 36ba841d..877b9d3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/shooter/Shooter.java @@ -13,7 +13,7 @@ @Config public class Shooter implements BluSubsystem, Subsystem { - public static double p = 0.2,i = 0.001, d = 0.1, f = 0.006; + public static double p = 0.01,i = 0, d = 0, f = 0.00058; public static double limit = 20; public static final double ZERO_ANGLE = 26; public static final double TOP_ANGLE = 50; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java index 4cdda0af..2920a0b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java @@ -34,21 +34,22 @@ public void initialize(){ //addTurret(); sm = new StateMachineBuilder() .state(State.IDLE) - .transition(() -> driver1.pressedRightBumper(), State.INTAKING, () ->{ + .transition(() -> driver1.pressedLeftTrigger(), State.INTAKING, () ->{ + telemetry.addLine("here"); new IntakeCommand().schedule(); }) .state(State.INTAKING) .transition(() -> driver1.pressedOptions(), State.IDLE, () -> { robot.idleRobot(); }) - .transition(() -> driver1.pressedRightBumper(), State.DRIVING_TO_SHOOT, () -> { + .transition(() -> driver1.pressedLeftBumper(), State.DRIVING_TO_SHOOT, () -> { new TransferCommand().schedule(); }) .state(State.DRIVING_TO_SHOOT) .transition(() -> driver1.pressedOptions(), State.IDLE, () -> { robot.idleRobot(); }) - .transition(() -> driver1.pressedRightBumper(), State.INTAKING, () -> { + .transition(() -> driver1.pressedRightTrigger(), State.INTAKING, () -> { new SequentialCommandGroup( new ShootBallsCommand(), new WaitCommand(400), @@ -63,7 +64,7 @@ public void initialize(){ public void periodic(){ sm.update(); - sixWheel.drive(-gamepad1.left_stick_y, gamepad1.left_stick_x); + sixWheel.teleDrive(gamepad1, 0.0001); if (driver2.pressedB()){ sixWheel.setPosition(llTagDetector.getLLBotPose()); } @@ -74,6 +75,7 @@ public void periodic(){ sixWheel.setDrivePower(0.5); } } + telemetry.addData("State", sm.getState()); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java index 5cf414db..d3521e91 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/SixWheelManualDrive.java @@ -15,6 +15,6 @@ public void initialize() { @Override public void periodic() { sixWheel.setDrivePower(0.5); - sixWheel.teleDrive(gamepad1,0.2); + sixWheel.teleDrive(gamepad1,0.001); } } From d3051842ddf5e3cdcc387156023ec7e077deae5e Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 3 Dec 2025 18:20:14 -0500 Subject: [PATCH 063/123] import roadrunner --- build.dependencies.gradle | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 1284b2ee..fd460e69 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -24,5 +24,9 @@ dependencies { implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.acmerobotics.dashboard:dashboard:0.4.17' implementation 'com.github.StateFactory-Dev:StateFactory:0.3.9' + implementation "com.acmerobotics.roadrunner:ftc:0.1.25" + implementation "com.acmerobotics.roadrunner:core:1.0.1" + implementation "com.acmerobotics.roadrunner:actions:1.0.1" + implementation "com.acmerobotics.dashboard:dashboard:0.5.1" } From ae9e7674c747abc57e29228577616838a17158b3 Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 3 Dec 2025 18:37:44 -0500 Subject: [PATCH 064/123] add roadrunner --- .../ftc/teamcode/roadrunner/Drawing.java | 22 + .../ftc/teamcode/roadrunner/Localizer.java | 25 + .../ftc/teamcode/roadrunner/MecanumDrive.java | 506 +++++++++++++++++ .../teamcode/roadrunner/OTOSLocalizer.java | 68 +++ .../roadrunner/PinpointLocalizer.java | 73 +++ .../ftc/teamcode/roadrunner/TankDrive.java | 509 ++++++++++++++++++ .../roadrunner/ThreeDeadWheelLocalizer.java | 113 ++++ .../roadrunner/TwoDeadWheelLocalizer.java | 143 +++++ .../messages/DriveCommandMessage.java | 24 + .../messages/MecanumCommandMessage.java | 19 + .../MecanumLocalizerInputsMessage.java | 30 ++ .../roadrunner/messages/PoseMessage.java | 18 + .../messages/TankCommandMessage.java | 15 + .../messages/TankLocalizerInputsMessage.java | 17 + .../messages/ThreeDeadWheelInputsMessage.java | 17 + .../messages/TwoDeadWheelInputsMessage.java | 35 ++ .../roadrunner/tuning/LocalizationTest.java | 78 +++ .../tuning/ManualFeedbackTuner.java | 63 +++ .../roadrunner/tuning/SplineTest.java | 39 ++ .../roadrunner/tuning/TuningOpModes.java | 321 +++++++++++ 20 files changed, 2135 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/OTOSLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/PinpointLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TankDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/TwoDeadWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/ManualFeedbackTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/SplineTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java new file mode 100644 index 00000000..02dce20b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.roadrunner; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + +public final class Drawing { + private Drawing() {} + + + public static void drawRobot(Canvas c, Pose2d t) { + final double ROBOT_RADIUS = 9; + + c.setStrokeWidth(1); + c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); + + Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); + Vector2d p1 = t.position.plus(halfv); + Vector2d p2 = p1.plus(halfv); + c.strokeLine(p1.x, p1.y, p2.x, p2.y); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java new file mode 100644 index 00000000..7a19827d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.roadrunner; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; + +/** + * Interface for localization methods. + */ +public interface Localizer { + void setPose(Pose2d pose); + + /** + * Returns the current pose estimate. + * NOTE: Does not update the pose estimate; + * you must call update() to update the pose estimate. + * @return the Localizer's current pose + */ + Pose2d getPose(); + + /** + * Updates the Localizer's pose estimate. + * @return the Localizer's current velocity estimate + */ + PoseVelocity2d update(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java new file mode 100644 index 00000000..8037c788 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java @@ -0,0 +1,506 @@ +package org.firstinspires.ftc.teamcode.roadrunner; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.AccelConstraint; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Actions; +import com.acmerobotics.roadrunner.AngularVelConstraint; +import com.acmerobotics.roadrunner.DualNum; +import com.acmerobotics.roadrunner.HolonomicController; +import com.acmerobotics.roadrunner.MecanumKinematics; +import com.acmerobotics.roadrunner.MinVelConstraint; +import com.acmerobotics.roadrunner.MotorFeedforward; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Pose2dDual; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.PoseVelocity2dDual; +import com.acmerobotics.roadrunner.ProfileAccelConstraint; +import com.acmerobotics.roadrunner.ProfileParams; +import com.acmerobotics.roadrunner.Rotation2d; +import com.acmerobotics.roadrunner.Time; +import com.acmerobotics.roadrunner.TimeTrajectory; +import com.acmerobotics.roadrunner.TimeTurn; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TrajectoryBuilderParams; +import com.acmerobotics.roadrunner.TurnConstraints; +import com.acmerobotics.roadrunner.Twist2d; +import com.acmerobotics.roadrunner.Twist2dDual; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.VelConstraint; +import com.acmerobotics.roadrunner.ftc.DownsampledWriter; +import com.acmerobotics.roadrunner.ftc.Encoder; +import com.acmerobotics.roadrunner.ftc.FlightRecorder; +import com.acmerobotics.roadrunner.ftc.LazyHardwareMapImu; +import com.acmerobotics.roadrunner.ftc.LazyImu; +import com.acmerobotics.roadrunner.ftc.LynxFirmware; +import com.acmerobotics.roadrunner.ftc.OverflowEncoder; +import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; +import com.acmerobotics.roadrunner.ftc.RawEncoder; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.Drawing; +import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage; +import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage; +import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage; +import org.firstinspires.ftc.teamcode.messages.PoseMessage; + +import java.util.Arrays; +import java.util.LinkedList; +import java.util.List; + +@Config +public final class MecanumDrive { + public static class Params { + // IMU orientation + // TODO: fill in these values based on + // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting + public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = + RevHubOrientationOnRobot.LogoFacingDirection.UP; + public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + // drive model parameters + public double inPerTick = 1; + public double lateralInPerTick = inPerTick; + public double trackWidthTicks = 0; + + // feedforward parameters (in tick units) + public double kS = 0; + public double kV = 0; + public double kA = 0; + + // path profile parameters (in inches) + public double maxWheelVel = 50; + public double minProfileAccel = -30; + public double maxProfileAccel = 50; + + // turn profile parameters (in radians) + public double maxAngVel = Math.PI; // shared with path + public double maxAngAccel = Math.PI; + + // path controller gains + public double axialGain = 0.0; + public double lateralGain = 0.0; + public double headingGain = 0.0; // shared with turn + + public double axialVelGain = 0.0; + public double lateralVelGain = 0.0; + public double headingVelGain = 0.0; // shared with turn + } + + public static Params PARAMS = new Params(); + + public final MecanumKinematics kinematics = new MecanumKinematics( + PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); + + public final TurnConstraints defaultTurnConstraints = new TurnConstraints( + PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); + public final VelConstraint defaultVelConstraint = + new MinVelConstraint(Arrays.asList( + kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), + new AngularVelConstraint(PARAMS.maxAngVel) + )); + public final AccelConstraint defaultAccelConstraint = + new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); + + public final DcMotorEx leftFront, leftBack, rightBack, rightFront; + + public final VoltageSensor voltageSensor; + + public final LazyImu lazyImu; + + public final Localizer localizer; + private final LinkedList poseHistory = new LinkedList<>(); + + private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); + private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); + private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); + private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000); + + public class DriveLocalizer implements Localizer { + public final Encoder leftFront, leftBack, rightBack, rightFront; + public final IMU imu; + + private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; + private Rotation2d lastHeading; + private boolean initialized; + private Pose2d pose; + + public DriveLocalizer(Pose2d pose) { + leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); + leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); + rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); + rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); + + imu = lazyImu.get(); + + // TODO: reverse encoders if needed + // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + + this.pose = pose; + } + + @Override + public void setPose(Pose2d pose) { + this.pose = pose; + } + + @Override + public Pose2d getPose() { + return pose; + } + + @Override + public PoseVelocity2d update() { + PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity(); + PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity(); + PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity(); + PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); + + YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); + + FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage( + leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles)); + + Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS)); + + if (!initialized) { + initialized = true; + + lastLeftFrontPos = leftFrontPosVel.position; + lastLeftBackPos = leftBackPosVel.position; + lastRightBackPos = rightBackPosVel.position; + lastRightFrontPos = rightFrontPosVel.position; + + lastHeading = heading; + + return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0); + } + + double headingDelta = heading.minus(lastHeading); + Twist2dDual