From 616119dce3d7ef895f0157204d12cd9bd9d372c3 Mon Sep 17 00:00:00 2001 From: Titan Robotics Club Date: Fri, 18 Oct 2024 01:12:31 -0700 Subject: [PATCH] Updated ftclib, trclib. Added Gobilda LED indicator support. --- TeamCode/src/main/java/ftclib | 2 +- .../src/main/java/teamcode/FtcTeleOp.java | 4 +- TeamCode/src/main/java/teamcode/Robot.java | 8 +- .../src/main/java/teamcode/RobotParams.java | 8 +- .../java/teamcode/subsystems/BlinkinLEDs.java | 106 ------------- .../teamcode/subsystems/LEDIndicator.java | 144 ++++++++++++++++++ .../src/main/java/teamcode/vision/Vision.java | 22 +-- TeamCode/src/main/java/trclib | 2 +- 8 files changed, 169 insertions(+), 127 deletions(-) delete mode 100644 TeamCode/src/main/java/teamcode/subsystems/BlinkinLEDs.java create mode 100644 TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib index 2e59a5f..e00a6a5 160000 --- a/TeamCode/src/main/java/ftclib +++ b/TeamCode/src/main/java/ftclib @@ -1 +1 @@ -Subproject commit 2e59a5fd11aa99911a8bfac5f9d6ffe663f699ce +Subproject commit e00a6a5bfdb3b4d9399960e732a60357aa2cdbd3 diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index d74fd2f..160ba5c 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -225,9 +225,9 @@ public class FtcTeleOp extends FtcOpMode robot.globalTracer.traceInfo(moduleName, "driveOrientation=" + orientation); robot.robotDrive.driveBase.setDriveOrientation( orientation, orientation == TrcDriveBase.DriveOrientation.FIELD); - if (robot.blinkin != null) + if (robot.ledIndicator != null) { - robot.blinkin.setDriveOrientation(orientation); + robot.ledIndicator.setDriveOrientation(orientation); } } } //setDriveOrientation diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 96d55a1..2e70260 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -29,7 +29,7 @@ import ftclib.driverio.FtcDashboard; import ftclib.driverio.FtcMatchInfo; import ftclib.robotcore.FtcOpMode; import ftclib.sensor.FtcRobotBattery; -import teamcode.subsystems.BlinkinLEDs; +import teamcode.subsystems.LEDIndicator; import teamcode.subsystems.RobotBase; import teamcode.vision.Vision; import trclib.motor.TrcMotor; @@ -59,7 +59,7 @@ public class Robot // Vision subsystems. public Vision vision; // Sensors and indicators. - public BlinkinLEDs blinkin; + public LEDIndicator ledIndicator; public FtcRobotBattery battery; // Subsystems. @@ -95,9 +95,9 @@ public class Robot if (RobotParams.Preferences.robotType != RobotParams.RobotType.VisionOnly) { // Create and initialize sensors and indicators. - if (robotInfo.blinkinName != null) + if (robotInfo.indicatorName != null) { - blinkin = new BlinkinLEDs(robotInfo.blinkinName); + ledIndicator = new LEDIndicator(robotInfo.indicatorName); } if (RobotParams.Preferences.useBatteryMonitor) diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 9bbe5d2..20111f8 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -143,6 +143,8 @@ public class RobotParams // Status Update: Status Update may affect robot loop time, don't do it when in competition. public static final boolean doStatusUpdate = !inCompetition; public static final boolean showSubsystems = true; + public static final boolean useBlinkinLED = true; + public static final boolean useGobildaLED = false; // Vision public static final boolean useVision = false; public static final boolean useWebCam = false; // false to use Android phone camera. @@ -362,7 +364,7 @@ public class RobotParams webCam2 = new BackCamParams(); limelight = new LimelightParams(); // Miscellaneous - blinkinName = "blinkin"; + indicatorName = "blinkin"; } //DifferentialParams } //class DifferentialParams @@ -457,7 +459,7 @@ public class RobotParams webCam2 = new BackCamParams(); limelight = new LimelightParams(); // Miscellaneous - blinkinName = "blinkin"; + indicatorName = "blinkin"; } //MecanumParams } //class MecanumParams @@ -549,7 +551,7 @@ public class RobotParams webCam2 = new BackCamParams(); limelight = new LimelightParams(); // Miscellaneous - blinkinName = "blinkin"; + indicatorName = "blinkin"; // Steer Encoders steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"}; steerEncoderInverted = new boolean[] {false, false, false, false}; diff --git a/TeamCode/src/main/java/teamcode/subsystems/BlinkinLEDs.java b/TeamCode/src/main/java/teamcode/subsystems/BlinkinLEDs.java deleted file mode 100644 index 545fc89..0000000 --- a/TeamCode/src/main/java/teamcode/subsystems/BlinkinLEDs.java +++ /dev/null @@ -1,106 +0,0 @@ -/* - * Copyright (c) 2023 Titan Robotics Club (http://www.titanrobotics.com) - * - * 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 teamcode.subsystems; - -import ftclib.driverio.FtcRevBlinkin; -import trclib.drivebase.TrcDriveBase; -import trclib.driverio.TrcRevBlinkin; - -/** - * This class encapsulates the REV Blinkin LED controller to provide a priority indicator showing the status of the - * robot. - */ -public class BlinkinLEDs extends FtcRevBlinkin -{ - // LED pattern names. - public static final String APRIL_TAG = "AprilTag"; - public static final String RED_BLOB = "RedBlob"; - public static final String BLUE_BLOB = "BlueBlob"; - public static final String DRIVE_ORIENTATION_FIELD = "FieldMode"; - public static final String DRIVE_ORIENTATION_ROBOT = "RobotMode"; - public static final String DRIVE_ORIENTATION_INVERTED = "InvertedMode"; - public static final String OFF_PATTERN = "Off"; - - /** - * Constructor: Create an instance of the object. - * - * @param instanceName specifies the instance name. This is also the REV Blinkin's hardware name. - */ - public BlinkinLEDs(String instanceName) - { - super(instanceName); - // LED Patterns are sorted in decreasing priority order. - final TrcRevBlinkin.Pattern[] ledPatternPriorities = { - // Highest priority. - new TrcRevBlinkin.Pattern(APRIL_TAG, RevLedPattern.SolidAqua), - new TrcRevBlinkin.Pattern(RED_BLOB, RevLedPattern.SolidRed), - new TrcRevBlinkin.Pattern(BLUE_BLOB, RevLedPattern.SolidBlue), - new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_FIELD, RevLedPattern.SolidViolet), - new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_ROBOT, RevLedPattern.SolidWhite), - new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_INVERTED, RevLedPattern.SolidGray), - new TrcRevBlinkin.Pattern(OFF_PATTERN, RevLedPattern.SolidBlack) - // Lowest priority. - }; - setPatternPriorities(ledPatternPriorities); - } //BlinkinLEDs - - /** - * This method sets the pattern ON for a period of time and turns off automatically afterwards. - * - * @param patternName specifies the name of the LED pattern to turn on. - */ - public void setDetectedPattern(String patternName) - { - setPatternState(patternName, true, 0.5); - } //setDetectedPattern - - /** - * This method sets the LED to indicate the drive orientation mode of the robot. - * - * @param orientation specifies the drive orientation mode. - */ - public void setDriveOrientation(TrcDriveBase.DriveOrientation orientation) - { - switch (orientation) - { - case INVERTED: - setPatternState(DRIVE_ORIENTATION_INVERTED, true); - setPatternState(DRIVE_ORIENTATION_ROBOT, false); - setPatternState(DRIVE_ORIENTATION_FIELD, false); - break; - - case ROBOT: - setPatternState(DRIVE_ORIENTATION_INVERTED, false); - setPatternState(DRIVE_ORIENTATION_ROBOT, true); - setPatternState(DRIVE_ORIENTATION_FIELD, false); - break; - - case FIELD: - setPatternState(DRIVE_ORIENTATION_INVERTED, false); - setPatternState(DRIVE_ORIENTATION_ROBOT, false); - setPatternState(DRIVE_ORIENTATION_FIELD, true); - break; - } - } //setDriveOrientation - -} //class BlinkinLEDs diff --git a/TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java b/TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java new file mode 100644 index 0000000..4839bfe --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java @@ -0,0 +1,144 @@ +/* + * Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com) + * + * 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 teamcode.subsystems; + +import ftclib.driverio.FtcGobildaIndicatorLight; +import ftclib.driverio.FtcRevBlinkin; +import teamcode.RobotParams; +import trclib.drivebase.TrcDriveBase; +import trclib.driverio.TrcGobildaIndicatorLight; +import trclib.driverio.TrcPriorityIndicator; +import trclib.driverio.TrcRevBlinkin; + +/** + * This class encapsulates the LED controller to provide a priority indicator showing the status of the robot. + */ +public class LEDIndicator +{ + // LED pattern names. + public static final String RED_BLOB = "RedBlob"; + public static final String BLUE_BLOB = "BlueBlob"; + public static final String NO_BLOB = "NoBlob"; + public static final String APRIL_TAG = "AprilTag"; + public static final String DRIVE_ORIENTATION_FIELD = "FieldMode"; + public static final String DRIVE_ORIENTATION_ROBOT = "RobotMode"; + public static final String DRIVE_ORIENTATION_INVERTED = "InvertedMode"; + public static final String OFF_PATTERN = "Off"; + + private final TrcPriorityIndicator indicator; + + /** + * Constructor: Create an instance of the object. + * + * @param indicatorName specifies the indicator hardware name. + */ + public LEDIndicator(String indicatorName) + { + if (RobotParams.Preferences.useGobildaLED) + { + // LED Patterns are sorted in decreasing priority order. + final TrcGobildaIndicatorLight.Pattern[] ledPatternPriorities = { + // Highest priority. + new TrcGobildaIndicatorLight.Pattern(RED_BLOB, TrcGobildaIndicatorLight.Color.Red), + new TrcGobildaIndicatorLight.Pattern(BLUE_BLOB, TrcGobildaIndicatorLight.Color.Blue), + new TrcGobildaIndicatorLight.Pattern(NO_BLOB, TrcGobildaIndicatorLight.Color.Azure), + new TrcGobildaIndicatorLight.Pattern(APRIL_TAG, TrcGobildaIndicatorLight.Color.Green), + new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_FIELD, TrcGobildaIndicatorLight.Color.Violet), + new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_ROBOT, TrcGobildaIndicatorLight.Color.White), + new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_INVERTED, TrcGobildaIndicatorLight.Color.Orange), + new TrcGobildaIndicatorLight.Pattern(OFF_PATTERN, TrcGobildaIndicatorLight.Color.Off) + // Lowest priority. + }; + indicator = new FtcGobildaIndicatorLight(indicatorName); + ((FtcGobildaIndicatorLight) indicator).setPatternPriorities(ledPatternPriorities); + } + else if (RobotParams.Preferences.useBlinkinLED) + { + // LED Patterns are sorted in decreasing priority order. + final TrcRevBlinkin.Pattern[] ledPatternPriorities = { + // Highest priority. + new TrcRevBlinkin.Pattern(RED_BLOB, TrcRevBlinkin.RevLedPattern.SolidRed), + new TrcRevBlinkin.Pattern(BLUE_BLOB, TrcRevBlinkin.RevLedPattern.SolidBlue), + new TrcRevBlinkin.Pattern(NO_BLOB, TrcRevBlinkin.RevLedPattern.SolidAqua), + new TrcRevBlinkin.Pattern(APRIL_TAG, TrcRevBlinkin.RevLedPattern.SolidGreen), + new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_FIELD, TrcRevBlinkin.RevLedPattern.SolidViolet), + new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_ROBOT, TrcRevBlinkin.RevLedPattern.SolidWhite), + new TrcRevBlinkin.Pattern(DRIVE_ORIENTATION_INVERTED, TrcRevBlinkin.RevLedPattern.SolidOrange), + new TrcRevBlinkin.Pattern(OFF_PATTERN, TrcRevBlinkin.RevLedPattern.SolidBlack) + // Lowest priority. + }; + indicator = new FtcRevBlinkin(indicatorName); + ((FtcRevBlinkin) indicator).setPatternPriorities(ledPatternPriorities); + } + else + { + indicator = null; + } + } //LEDIndicator + + /** + * This method sets the pattern ON for a period of time and turns off automatically afterwards. + * + * @param patternName specifies the name of the LED pattern to turn on. + */ + public void setDetectedPattern(String patternName) + { + if (indicator != null) + { + indicator.setPatternState(patternName, true, 0.5); + } + } //setDetectedPattern + + /** + * This method sets the LED to indicate the drive orientation mode of the robot. + * + * @param orientation specifies the drive orientation mode. + */ + public void setDriveOrientation(TrcDriveBase.DriveOrientation orientation) + { + if (indicator != null) + { + switch (orientation) + { + case INVERTED: + indicator.setPatternState(DRIVE_ORIENTATION_INVERTED, true); + indicator.setPatternState(DRIVE_ORIENTATION_ROBOT, false); + indicator.setPatternState(DRIVE_ORIENTATION_FIELD, false); + break; + + case ROBOT: + indicator.setPatternState(DRIVE_ORIENTATION_INVERTED, false); + indicator.setPatternState(DRIVE_ORIENTATION_ROBOT, true); + indicator.setPatternState(DRIVE_ORIENTATION_FIELD, false); + break; + + case FIELD: + indicator.setPatternState(DRIVE_ORIENTATION_INVERTED, false); + indicator.setPatternState(DRIVE_ORIENTATION_ROBOT, false); + indicator.setPatternState(DRIVE_ORIENTATION_FIELD, true); + break; + } + } + } //setDriveOrientation + +} //class LEDIndicator diff --git a/TeamCode/src/main/java/teamcode/vision/Vision.java b/TeamCode/src/main/java/teamcode/vision/Vision.java index 16125ab..f5d4672 100644 --- a/TeamCode/src/main/java/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/teamcode/vision/Vision.java @@ -22,6 +22,8 @@ package teamcode.vision; +import com.qualcomm.robotcore.hardware.LED; + import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -45,7 +47,7 @@ import ftclib.vision.FtcVisionAprilTag; import ftclib.vision.FtcVisionEocvColorBlob; import teamcode.Robot; import teamcode.RobotParams; -import teamcode.subsystems.BlinkinLEDs; +import teamcode.subsystems.LEDIndicator; import trclib.pathdrive.TrcPose2D; import trclib.robotcore.TrcDbgTrace; import trclib.vision.TrcOpenCvColorBlobPipeline; @@ -441,9 +443,9 @@ public class Vision } } - if (objectName != null && robot.blinkin != null) + if (objectName != null && robot.ledIndicator != null) { - robot.blinkin.setDetectedPattern(objectName); + robot.ledIndicator.setDetectedPattern(objectName); } if (lineNum != -1) @@ -542,15 +544,15 @@ public class Vision aprilTagInfo.detectedObj.getRotatedRectVertices()); } - if (aprilTagInfo != null && robot.blinkin != null) + if (aprilTagInfo != null && robot.ledIndicator != null) { - robot.blinkin.setDetectedPattern(BlinkinLEDs.APRIL_TAG); + robot.ledIndicator.setDetectedPattern(LEDIndicator.APRIL_TAG); } if (lineNum != -1) { robot.dashboard.displayPrintf( - lineNum, "%s: %s", BlinkinLEDs.APRIL_TAG, aprilTagInfo != null? aprilTagInfo : "Not found."); + lineNum, "%s: %s", LEDIndicator.APRIL_TAG, aprilTagInfo != null? aprilTagInfo : "Not found."); } return aprilTagInfo; @@ -681,13 +683,13 @@ public class Vision case RedBlob: colorBlobInfo = redBlobVision != null? redBlobVision.getBestDetectedTargetInfo( null, this::compareDistance, 0.0, 0.0): null; - colorBlobName = BlinkinLEDs.RED_BLOB; + colorBlobName = LEDIndicator.RED_BLOB; break; case BlueBlob: colorBlobInfo = blueBlobVision != null? blueBlobVision.getBestDetectedTargetInfo( null, this::compareDistance, 0.0, 0.0): null; - colorBlobName = BlinkinLEDs.BLUE_BLOB; + colorBlobName = LEDIndicator.BLUE_BLOB; break; case AnyColorBlob: @@ -726,9 +728,9 @@ public class Vision colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices()); } - if (colorBlobInfo != null && robot.blinkin != null) + if (colorBlobInfo != null && robot.ledIndicator != null) { - robot.blinkin.setDetectedPattern(colorBlobName); + robot.ledIndicator.setDetectedPattern(colorBlobName); } if (lineNum != -1) diff --git a/TeamCode/src/main/java/trclib b/TeamCode/src/main/java/trclib index b684c6f..5297676 160000 --- a/TeamCode/src/main/java/trclib +++ b/TeamCode/src/main/java/trclib @@ -1 +1 @@ -Subproject commit b684c6f714205c92bc0ae28d8913d5247494da1f +Subproject commit 5297676fcb83b6174ff31862d0dcaa026816d606