diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib index 0d08702..0bce031 160000 --- a/TeamCode/src/main/java/ftclib +++ b/TeamCode/src/main/java/ftclib @@ -1 +1 @@ -Subproject commit 0d087024161bffdd6935759f49afd0eca8202a03 +Subproject commit 0bce031baf81ec4c4afd7a4c8522b70fbccbe4be diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index 034529f..9c3201a 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -36,6 +36,7 @@ import ftclib.driverio.FtcGamepad; import ftclib.driverio.FtcMenu; import ftclib.driverio.FtcValueMenu; import ftclib.robotcore.FtcPidCoeffCache; +import ftclib.vision.FtcLimelightVision; import teamcode.vision.Vision; import trclib.command.CmdDriveMotorsTest; import trclib.command.CmdPidDrive; @@ -241,6 +242,12 @@ public class FtcTest extends FtcTeleOp robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision."); robot.vision.setColorBlobVisionEnabled(Vision.ColorBlobType.BlueBlob, true); } + + if (robot.vision.limelightVision != null) + { + robot.globalTracer.traceInfo(moduleName, "Enabling LimelightVision."); + robot.vision.setLimelightVisionEnabled(0, true); + } } break; @@ -1070,6 +1077,11 @@ public class FtcTest extends FtcTeleOp { robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, lineNum++); } + + if (robot.vision.limelightVision != null) + { + robot.vision.getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, lineNum++); + } } } //doVisionTest diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 91d9235..ca3104c 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -219,6 +219,12 @@ public class Robot vision.setColorBlobVisionEnabled(Vision.ColorBlobType.BlueBlob, false); } + if (vision.limelightVision != null) + { + globalTracer.traceInfo(moduleName, "Disabling LimelightVision."); + vision.setLimelightVisionEnabled(0, false); + } + vision.close(); } diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 75db3fe..bfabffd 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -144,7 +144,7 @@ public class RobotParams public static final boolean showSubsystems = true; // Vision public static final boolean useVision = false; - public static final boolean useWebCam = false; + public static final boolean useWebCam = false; // false to use Android phone camera. public static final boolean useBuiltinCamBack = false; // For Android Phone as Robot Controller. public static final boolean tuneColorBlobVision = false; public static final boolean useAprilTagVision = false; @@ -233,7 +233,7 @@ public class RobotParams { public LimelightParams() { - camName = "limelight3a"; + camName = "Limelight3a"; camImageWidth = 640; camImageHeight = 480; camXOffset = 0.0; // Inches to the right from robot center diff --git a/TeamCode/src/main/java/teamcode/vision/Vision.java b/TeamCode/src/main/java/teamcode/vision/Vision.java index 723b5c5..942b36b 100644 --- a/TeamCode/src/main/java/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/teamcode/vision/Vision.java @@ -611,6 +611,71 @@ public class Vision return colorBlobInfo; } //getDetectedColorBlob + /** + * This method enables/disables Limelight vision for the specified pipeline. + * + * @param pipelineIndex specifies the limelight pipeline index to be selected, ignore if disabled. + * @param enabled specifies true to enable, false to disable. + */ + public void setLimelightVisionEnabled(int pipelineIndex, boolean enabled) + { + if (limelightVision != null) + { + if (enabled) + { + limelightVision.setPipeline(pipelineIndex); + } + limelightVision.setVisionEnabled(enabled); + } + } //setLimelightVisionEnabled + + /** + * This method checks if Limelight vision is enabled. + * + * @return true if enabled, false if disabled. + */ + public boolean isLimelightVisionEnabled() + { + return limelightVision != null && limelightVision.isVisionEnabled(); + } //isLimelightVisionEnabled + + /** + * This method calls Limelight vision to detect the object. + * + * @param resultType specifies the result type to look for. + * @param label specifies the detected object label, can be null to match any label. + * @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing. + * @return detected Limelight object info. + */ + public TrcVisionTargetInfo getLimelightDetectedObject( + FtcLimelightVision.ResultType resultType, String label, int lineNum) + { + TrcVisionTargetInfo limelightInfo = null; + String objectName = null; + + if (limelightVision != null) + { + limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, null); + if (limelightInfo != null) + { + objectName = limelightInfo.detectedObj.label; + } + } + + if (objectName != null && robot.blinkin != null) + { + robot.blinkin.setDetectedPattern(objectName); + } + + if (lineNum != -1) + { + robot.dashboard.displayPrintf( + lineNum, "%s: %s", objectName, limelightInfo != null? limelightInfo: "Not found."); + } + + return limelightInfo; + } //getLimelightDetectedObject + /** * This method returns the target Z offset from ground. *