diff --git a/TeamCode/src/main/java/TrcCommonLib b/TeamCode/src/main/java/TrcCommonLib index 95fde8b7a4be..c3f784d50541 160000 --- a/TeamCode/src/main/java/TrcCommonLib +++ b/TeamCode/src/main/java/TrcCommonLib @@ -1 +1 @@ -Subproject commit 95fde8b7a4be2f32ec23b70b8d687ed5c0fef278 +Subproject commit c3f784d50541f137f2f2a47afe77637376c754ca diff --git a/TeamCode/src/main/java/TrcFtcLib b/TeamCode/src/main/java/TrcFtcLib index 5efde522ce34..85b63e1b7764 160000 --- a/TeamCode/src/main/java/TrcFtcLib +++ b/TeamCode/src/main/java/TrcFtcLib @@ -1 +1 @@ -Subproject commit 5efde522ce34d18a8ac80ae5574a68280de15ac6 +Subproject commit 85b63e1b7764d1e201c738494feba72433a3c832 diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index 4655628dec75..8db4507fe650 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -1,5 +1,5 @@ /* - * Copyright (c) 2022 Titan Robotics Club (http://www.titanrobotics.com) + * 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 @@ -24,6 +24,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import java.util.Arrays; import java.util.Locale; import TrcCommonLib.command.CmdDriveMotorsTest; @@ -34,6 +35,7 @@ import TrcCommonLib.trclib.TrcElapsedTimer; import TrcCommonLib.trclib.TrcGameController; import TrcCommonLib.trclib.TrcOpenCvColorBlobPipeline; +import TrcCommonLib.trclib.TrcOpenCvDetector; import TrcCommonLib.trclib.TrcPidController; import TrcCommonLib.trclib.TrcPose2D; import TrcCommonLib.trclib.TrcRobot; @@ -67,6 +69,7 @@ private enum Test SENSORS_TEST, SUBSYSTEMS_TEST, VISION_TEST, + TUNE_COLORBLOB_VISION, DRIVE_SPEED_TEST, DRIVE_MOTORS_TEST, X_TIMED_DRIVE, @@ -122,12 +125,19 @@ public String toString() private FtcChoiceMenu testMenu = null; private TrcRobot.RobotCommand testCommand = null; + // Drive Speed Test. private double maxDriveVelocity = 0.0; private double maxDriveAcceleration = 0.0; private double prevTime = 0.0; private double prevVelocity = 0.0; - + // Swerve Steering Calibration. private boolean steerCalibrating = false; + // Color Blob Vision Turning. + private static final double[] COLOR_THRESHOLD_LOW_RANGES = {0.0, 0.0, 0.0}; + private static final double[] COLOR_THRESHOLD_HIGH_RANGES = {255.0, 255.0, 255.0}; + private double[] colorThresholds = null; + private int colorThresholdIndex = 0; + private double colorThresholdMultiplier = 1.0; private boolean teleOpControlEnabled = true; // @@ -261,9 +271,7 @@ public void startMode(TrcRobot.RunMode prevMode, TrcRobot.RunMode nextMode) case VISION_TEST: if (robot.vision != null) { - // // Vision generally will impact performance, so we only enable it if it's needed. - // if (robot.vision.aprilTagVision != null) { robot.globalTracer.traceInfo(funcName, "Enabling AprilTagVision."); @@ -290,6 +298,18 @@ public void startMode(TrcRobot.RunMode prevMode, TrcRobot.RunMode nextMode) } break; + case TUNE_COLORBLOB_VISION: + if (robot.vision != null && robot.vision.rawColorBlobVision != null) + { + robot.globalTracer.traceInfo(funcName, "Enabling FtcRawEocvVision."); + robot.vision.setRawColorBlobVisionEnabled(true); + colorThresholds = robot.vision.getRawColorBlobThresholds(); + colorThresholdIndex = 0; + colorThresholdMultiplier = 1.0; + updateColorThresholds(); + } + break; + case PID_DRIVE: case TUNE_X_PID: case TUNE_Y_PID: @@ -483,6 +503,7 @@ public void periodic(double elapsedTime, boolean slowPeriodicLoop) break; case VISION_TEST: + case TUNE_COLORBLOB_VISION: doVisionTest(); break; @@ -531,7 +552,8 @@ public void periodic(double elapsedTime, boolean slowPeriodicLoop) @Override public void driverButtonEvent(TrcGameController gamepad, int button, boolean pressed) { - if (allowButtonControl() || testChoices.test == Test.VISION_TEST) + if (allowButtonControl() || testChoices.test == Test.VISION_TEST || + testChoices.test == Test.TUNE_COLORBLOB_VISION) { boolean processed = false; // @@ -562,15 +584,63 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre } processed = true; } + else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed) + { + // Commit color thresholds change. + robot.vision.setRawColorBlobThresholds(colorThresholds); + } + processed = true; + } break; case FtcGamepad.GAMEPAD_B: + if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed) + { + // Increment to next color threshold index. + colorThresholdIndex++; + if (colorThresholdIndex >= colorThresholds.length) + { + colorThresholdIndex = colorThresholds.length - 1; + } + } + processed = true; + } break; case FtcGamepad.GAMEPAD_X: + if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed) + { + // Decrement to previous color threshold index. + colorThresholdIndex--; + if (colorThresholdIndex < 0) + { + colorThresholdIndex = 0; + } + } + processed = true; + } break; case FtcGamepad.GAMEPAD_Y: + if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed) + { + // Set display to next intermediate Mat in the pipeline. + robot.vision.rawColorBlobVision.getPipeline().setNextVideoOutput(); + } + processed = true; + } break; case FtcGamepad.GAMEPAD_DPAD_UP: @@ -584,6 +654,19 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre teleOpControlEnabled = !pressed; processed = true; } + else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed && + colorThresholds[colorThresholdIndex] + colorThresholdMultiplier <= + COLOR_THRESHOLD_HIGH_RANGES[colorThresholdIndex/2]) + { + // Increment color threshold value. + colorThresholds[colorThresholdIndex] += colorThresholdMultiplier; + updateColorThresholds(); + } + processed = true; + } break; case FtcGamepad.GAMEPAD_DPAD_DOWN: @@ -597,6 +680,19 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre teleOpControlEnabled = !pressed; processed = true; } + else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed && + colorThresholds[colorThresholdIndex] - colorThresholdMultiplier >= + COLOR_THRESHOLD_LOW_RANGES[colorThresholdIndex/2]) + { + // Decrement color threshold value. + colorThresholds[colorThresholdIndex] -= colorThresholdMultiplier; + updateColorThresholds(); + } + processed = true; + } break; case FtcGamepad.GAMEPAD_DPAD_LEFT: @@ -610,6 +706,16 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre teleOpControlEnabled = !pressed; processed = true; } + else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed && colorThresholdMultiplier * 10.0 <= 100.0) + { + // Increment the significant multiplier. + colorThresholdMultiplier *= 10.0; + } + processed = true; + } break; case FtcGamepad.GAMEPAD_DPAD_RIGHT: @@ -623,6 +729,16 @@ public void driverButtonEvent(TrcGameController gamepad, int button, boolean pre teleOpControlEnabled = !pressed; processed = true; } + else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed && colorThresholdMultiplier / 10.0 >= 1.0) + { + // Decrement the significant multiplier. + colorThresholdMultiplier /= 10.0; + } + processed = true; + } break; } // @@ -689,6 +805,14 @@ public void operatorButtonEvent(TrcGameController gamepad, int button, boolean p } } //operatorButtonEvent + /** + * This method displays the current color thresholds on the dashboard. + */ + private void updateColorThresholds() + { + robot.dashboard.displayPrintf(10, "Thresholds: %s", Arrays.toString(colorThresholds)); + } //updateColorThresholds + /** * This method creates and displays the test menus and record the selected choices. */ @@ -734,6 +858,7 @@ private void doTestMenus() testMenu.addChoice("Sensors test", Test.SENSORS_TEST, true); testMenu.addChoice("Subsystems test", Test.SUBSYSTEMS_TEST, false); testMenu.addChoice("Vision test", Test.VISION_TEST, false); + testMenu.addChoice("Tune ColorBlob vision", Test.TUNE_COLORBLOB_VISION, false); testMenu.addChoice("Drive speed test", Test.DRIVE_SPEED_TEST, false); testMenu.addChoice("Drive motors test", Test.DRIVE_MOTORS_TEST, false); testMenu.addChoice("X Timed drive", Test.X_TIMED_DRIVE, false, driveTimeMenu); @@ -951,6 +1076,14 @@ private void doVisionTest() { int lineNum = 9; + if (robot.vision.rawColorBlobVision != null) + { + TrcVisionTargetInfo> colorBlobInfo = + robot.vision.rawColorBlobVision.getBestDetectedTargetInfo(null, null, 0.0, 0.0); + robot.dashboard.displayPrintf( + lineNum++, "ColorBlob: %s", colorBlobInfo != null? colorBlobInfo: "Not found."); + } + if (robot.vision.aprilTagVision != null) { TrcVisionTargetInfo aprilTagInfo = diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 41389390410f..b82664ce058f 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -84,7 +84,8 @@ public Robot(TrcRobot.RunMode runMode) // // Initialize vision subsystems. // - if (RobotParams.Preferences.useAprilTagVision || + if (RobotParams.Preferences.tuneColorBlobVision || + RobotParams.Preferences.useAprilTagVision || RobotParams.Preferences.useColorBlobVision || RobotParams.Preferences.useTensorFlowVision) { @@ -223,6 +224,12 @@ public void stopMode(TrcRobot.RunMode runMode) // if (vision != null) { + if (vision.rawColorBlobVision != null) + { + globalTracer.traceInfo(funcName, "Disabling RawColorBlobVision."); + vision.setRawColorBlobVisionEnabled(false); + } + if (vision.aprilTagVision != null) { globalTracer.traceInfo(funcName, "Disabling AprilTagVision."); diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 2fbe2ed76424..79f77deee1c4 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -26,6 +26,8 @@ import com.qualcomm.robotcore.hardware.DcMotor; +import org.openftc.easyopencv.OpenCvCameraRotation; + import TrcCommonLib.trclib.TrcDriveBase.DriveOrientation; import TrcCommonLib.trclib.TrcHomographyMapper; import TrcCommonLib.trclib.TrcPidController; @@ -49,9 +51,11 @@ public static class Preferences // Vision public static boolean useWebCam = true; public static boolean useBuiltinCamBack = false; + public static boolean tuneColorBlobVision = false; public static boolean useAprilTagVision = false; public static boolean useColorBlobVision = false; public static boolean useTensorFlowVision = false; + public static boolean useTfodModelAsset = false; public static boolean showVisionView = true; // Robot public static boolean noRobot = false; @@ -115,6 +119,7 @@ public static class Preferences // public static final int CAM_IMAGE_WIDTH = 640; public static final int CAM_IMAGE_HEIGHT = 480; + public static final OpenCvCameraRotation CAM_ORIENTATION = OpenCvCameraRotation.UPRIGHT; // Camera location on robot. public static final double CAM_FRONT_OFFSET = 2.000;//Camera offset from front of robot in inches public static final double CAM_LEFT_OFFSET = 7.125;//Camera offset from left of robot in inches diff --git a/TeamCode/src/main/java/teamcode/vision/Vision.java b/TeamCode/src/main/java/teamcode/vision/Vision.java index 73e39f97c4a0..0115eabb3a0d 100644 --- a/TeamCode/src/main/java/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/teamcode/vision/Vision.java @@ -30,6 +30,8 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.tfod.TfodProcessor; import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; import java.util.ArrayList; @@ -38,6 +40,8 @@ import TrcCommonLib.trclib.TrcVisionTargetInfo; import TrcFtcLib.ftclib.FtcEocvColorBlobProcessor; import TrcFtcLib.ftclib.FtcOpMode; +import TrcFtcLib.ftclib.FtcRawEocvColorBlobPipeline; +import TrcFtcLib.ftclib.FtcRawEocvVision; import TrcFtcLib.ftclib.FtcVision; import TrcFtcLib.ftclib.FtcVisionAprilTag; import TrcFtcLib.ftclib.FtcVisionEocvColorBlob; @@ -53,6 +57,8 @@ public class Vision { private static final String moduleName = "Vision"; + private static final double[] DEF_COLORBLOB_THRESHOLDS = {0.0, 255.0, 0.0, 255.0, 0.0, 255.0}; + // RGB Color Space. private static final int colorConversion = Imgproc.COLOR_BGRA2BGR; private static final double[] redBlobColorThresholds = {100.0, 255.0, 0.0, 100.0, 0.0, 60.0}; private static final double[] blueBlobColorThresholds = {0.0, 60.0, 0.0, 100.0, 100, 255.0}; @@ -69,6 +75,8 @@ public class Vision private static final float TFOD_MIN_CONFIDENCE = 0.75f; public static final String[] TFOD_TARGET_LABELS = {"MyObject"}; + private FtcRawEocvColorBlobPipeline rawColorBlobPipeline; + public FtcRawEocvVision rawColorBlobVision = null; public FtcVisionAprilTag aprilTagVision; private AprilTagProcessor aprilTagProcessor; public FtcVisionEocvColorBlob redBlobVision; @@ -77,7 +85,7 @@ public class Vision private FtcEocvColorBlobProcessor blueBlobProcessor; public FtcVisionTensorFlow tensorFlowVision; private TfodProcessor tensorFlowProcessor; - private final FtcVision vision; + private FtcVision vision = null; /** * Constructor: Create an instance of the object. @@ -88,69 +96,141 @@ public class Vision public Vision(Robot robot, TrcDbgTrace tracer) { FtcOpMode opMode = FtcOpMode.getInstance(); - ArrayList visionProcessorList = new ArrayList<>(); - - if (RobotParams.Preferences.useAprilTagVision) + if (RobotParams.Preferences.tuneColorBlobVision) { - robot.globalTracer.traceInfo(moduleName, "Starting AprilTagVision..."); - FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters() - .setDrawTagIdEnabled(true) - .setDrawTagOutlineEnabled(true) - .setDrawAxesEnabled(false) - .setDrawCubeProjectionEnabled(false) - .setLensIntrinsics( - RobotParams.WEBCAM_FX, RobotParams.WEBCAM_FY, RobotParams.WEBCAM_CX, RobotParams.WEBCAM_CY) - .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES); - aprilTagVision = new FtcVisionAprilTag(aprilTagParams, AprilTagProcessor.TagFamily.TAG_36h11, tracer); - aprilTagProcessor = aprilTagVision.getVisionProcessor(); - visionProcessorList.add(aprilTagProcessor); - } + OpenCvCamera webcam; + + if (RobotParams.Preferences.showVisionView) + { + int cameraViewId = opMode.hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", opMode.hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam( + opMode.hardwareMap.get(WebcamName.class, RobotParams.HWNAME_WEBCAM), cameraViewId); + webcam.showFpsMeterOnViewport(false); + } + else + { + webcam = OpenCvCameraFactory.getInstance().createWebcam( + opMode.hardwareMap.get(WebcamName.class, RobotParams.HWNAME_WEBCAM)); + } - if (RobotParams.Preferences.useColorBlobVision) + robot.globalTracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision..."); + rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline( + "rawColorBlobPipeline", colorConversion, DEF_COLORBLOB_THRESHOLDS, colorBlobFilterContourParams, + tracer); + // Display colorThresholdOutput. + rawColorBlobPipeline.setVideoOutput(0); + rawColorBlobPipeline.setAnnotateEnabled(true); + rawColorBlobVision = new FtcRawEocvVision( + "rawColorBlobVision", RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT, null, null, + webcam, RobotParams.CAM_ORIENTATION, tracer); + setRawColorBlobVisionEnabled(false); + } + else { - robot.globalTracer.traceInfo(moduleName, "Starting ColorBlobVision..."); + // Creating Vision Processors for VisionPortal. + ArrayList visionProcessorsList = new ArrayList<>(); + + if (RobotParams.Preferences.useAprilTagVision) + { + robot.globalTracer.traceInfo(moduleName, "Starting AprilTagVision..."); + FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters() + .setDrawTagIdEnabled(true) + .setDrawTagOutlineEnabled(true) + .setDrawAxesEnabled(false) + .setDrawCubeProjectionEnabled(false) + .setLensIntrinsics( + RobotParams.WEBCAM_FX, RobotParams.WEBCAM_FY, RobotParams.WEBCAM_CX, RobotParams.WEBCAM_CY) + .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES); + aprilTagVision = new FtcVisionAprilTag(aprilTagParams, AprilTagProcessor.TagFamily.TAG_36h11, tracer); + aprilTagProcessor = aprilTagVision.getVisionProcessor(); + visionProcessorsList.add(aprilTagProcessor); + } - redBlobVision = new FtcVisionEocvColorBlob( - "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, - RobotParams.cameraRect, RobotParams.worldRect, true, tracer); - redBlobProcessor = redBlobVision.getVisionProcessor(); - visionProcessorList.add(redBlobProcessor); + if (RobotParams.Preferences.useColorBlobVision) + { + robot.globalTracer.traceInfo(moduleName, "Starting ColorBlobVision..."); - blueBlobVision = new FtcVisionEocvColorBlob( - "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, - RobotParams.cameraRect, RobotParams.worldRect, true, tracer); - blueBlobProcessor = blueBlobVision.getVisionProcessor(); - visionProcessorList.add(blueBlobProcessor); + redBlobVision = new FtcVisionEocvColorBlob( + "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, + RobotParams.cameraRect, RobotParams.worldRect, true, tracer); + redBlobProcessor = redBlobVision.getVisionProcessor(); + visionProcessorsList.add(redBlobProcessor); + + blueBlobVision = new FtcVisionEocvColorBlob( + "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, + RobotParams.cameraRect, RobotParams.worldRect, true, tracer); + blueBlobProcessor = blueBlobVision.getVisionProcessor(); + visionProcessorsList.add(blueBlobProcessor); + } + + if (RobotParams.Preferences.useTensorFlowVision) + { + robot.globalTracer.traceInfo(moduleName, "Starting TensorFlowVision..."); + tensorFlowVision = new FtcVisionTensorFlow( + null, true, TFOD_MODEL_ASSET, TFOD_TARGET_LABELS, RobotParams.cameraRect, RobotParams.worldRect, + tracer); + tensorFlowProcessor = tensorFlowVision.getVisionProcessor(); + tensorFlowProcessor.setMinResultConfidence(TFOD_MIN_CONFIDENCE); + visionProcessorsList.add(tensorFlowProcessor); + } + + VisionProcessor[] visionProcessors = new VisionProcessor[visionProcessorsList.size()]; + visionProcessorsList.toArray(visionProcessors); + vision = RobotParams.Preferences.useWebCam ? + new FtcVision( + opMode.hardwareMap.get(WebcamName.class, RobotParams.HWNAME_WEBCAM), + RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT, + RobotParams.Preferences.showVisionView, visionProcessors) : + new FtcVision( + RobotParams.Preferences.useBuiltinCamBack ? + BuiltinCameraDirection.BACK : BuiltinCameraDirection.FRONT, + RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT, + RobotParams.Preferences.showVisionView, visionProcessors); + // Disable all vision processors until they are needed. + setAprilTagVisionEnabled(false); + setRedBlobVisionEnabled(false); + setBlueBlobVisionEnabled(false); + setTensorFlowVisionEnabled(false); } + } //Vision - if (RobotParams.Preferences.useTensorFlowVision) + /** + * This method returns the color threshold values of rawColorBlobVision. + * + * @return array of color threshold values. + */ + public double[] getRawColorBlobThresholds() + { + return rawColorBlobPipeline != null? rawColorBlobPipeline.getColorThresholds(): null; + } //getRawColorBlobThresholds + + /** + * This method sets the color threshold values of rawColorBlobVision. + * + * @param colorThresholds specifies an array of color threshold values. + */ + public void setRawColorBlobThresholds(double... colorThresholds) + { + if (rawColorBlobPipeline != null) { - robot.globalTracer.traceInfo(moduleName, "Starting TensorFlowVision..."); - tensorFlowVision = new FtcVisionTensorFlow( - null, TFOD_MODEL_ASSET, TFOD_TARGET_LABELS, RobotParams.cameraRect, RobotParams.worldRect, tracer); - tensorFlowProcessor = tensorFlowVision.getVisionProcessor(); - tensorFlowProcessor.setMinResultConfidence(TFOD_MIN_CONFIDENCE); - visionProcessorList.add(tensorFlowProcessor); + rawColorBlobPipeline.setColorThresholds(colorThresholds); } + } //setRawColorBlobThresholds - VisionProcessor[] visionProcessors = new VisionProcessor[visionProcessorList.size()]; - visionProcessorList.toArray(visionProcessors); - vision = RobotParams.Preferences.useWebCam ? - new FtcVision( - opMode.hardwareMap.get(WebcamName.class, RobotParams.HWNAME_WEBCAM), - RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT, - RobotParams.Preferences.showVisionView, visionProcessors) : - new FtcVision( - RobotParams.Preferences.useBuiltinCamBack ? - BuiltinCameraDirection.BACK : BuiltinCameraDirection.FRONT, - RobotParams.CAM_IMAGE_WIDTH, RobotParams.CAM_IMAGE_HEIGHT, - RobotParams.Preferences.showVisionView, visionProcessors); - // Disable all vision processors until they are needed. - setAprilTagVisionEnabled(false); - setRedBlobVisionEnabled(false); - setBlueBlobVisionEnabled(false); - setTensorFlowVisionEnabled(false); - } //Vision + + /** + * This method enables/disables raw ColorBlob vision. + * + * @param enabled specifies true to enable, false to disable. + */ + public void setRawColorBlobVisionEnabled(boolean enabled) + { + if (rawColorBlobVision != null) + { + rawColorBlobVision.setPipeline(enabled? rawColorBlobPipeline: null); + } + } //setRawColorBlobVisionEnabled /** * This method enables/disables AprilTag vision. @@ -204,6 +284,16 @@ public void setTensorFlowVisionEnabled(boolean enabled) } } //setTensorFlowVisionEnabled + /** + * This method checks if raw ColorBlob vision is enabled. + * + * @return true if enabled, false if disabled. + */ + public boolean isRawColorBlobVisionEnabled() + { + return rawColorBlobVision != null && rawColorBlobVision.getPipeline() != null; + } //isRawColorBlobVisionEnabled + /** * This method checks if AprilTag vision is enabled. *