Robot.java

//Located in RobotMap.java
public static CvSource outputStream = CameraServer.getInstance().putVideo("Tracking", 320, 240);

//Located in Robot.java
public UsbCamera turretCameraRaw;
private CvSink cvSink;

public void robotInit() {
        oi = new OI();
        //Camera
        turretCameraRaw = CameraServer.getInstance().startAutomaticCapture("Turret", 0);
        // Get the UsbCamera from CameraServer
        //cameraRaw = CameraServer.getInstance().startAutomaticCapture();
        turret = new Turret(RobotMap.rotation, RobotMap.pitch, RobotMap.shooter, RobotMap.loader);
        turretCameraRaw.setExposureManual(30);
        turretCameraRaw.setResolution(320, 240);
        drivetrain.gyro.calibrate();
        cvSink = CameraServer.getInstance().getVideo();
        visionThread = new Thread(() -> {
            while(!Thread.interrupted()){
                switch(visionState){
                case -1:
                    calibrateTurret();
                    break;
                case 0:
                    sleepTurret();
                    break;
                case 1:
                    trackTarget();
                    break;
                case 2:
                    manualTurret();
                    break;
                default:
                    break;
                }
                ++vitr;
            }
        });
        visionThread.setDaemon(true);
        visionThread.start();
    }

private boolean trackTarget(){  
        if (cvSink.grabFrame(mat) == 0) {
            // Send the output the error.
            RobotMap.outputStream.notifyError(cvSink.getError());
            // skip the rest of the current iteration
            return false;
        }
  
        Core.inRange(mat, new Scalar(r, g, b, 0), new Scalar(rr, gg, bb, 255), cvt);
        // Process Image        
        /* State representation
         * 0
         *|\\\\\\1\\\\\\|
         *|\\\\\\1\\\\\\|
         *2
         *|\\\\\\3\\\\\\|
         *end
         */
        
        int colorState = 0; //0 = top, 1 = large, 2 = middle, 3 = small,
        int i = 0;
        for(int j = 0; j < cvt.rows(); ++j){
            curColor = cvt.get(j, i);
            switch(colorState){
            case 0:
                if(curColor[0] >= 175){
                    colorState = 1;
                    ++largeHCur;
                }
                break;
            case 1:
                if(curColor[0] >= 175){
                    ++largeHCur;
                }else {
                    if(largeHCur > 5){
                        colorState = 2;
                    }
                }
                break;
            case 2:
                if(largeHCur > largeHMax){
                    largeHMax = largeHCur;
                    largeHCur = 0;
                }else largeHCur = 0;
                if(curColor[0] >= 175){
                    colorState = 3;
                    ++smallHCur;
                }
                break;
            case 3:
                if(curColor[0] >= 175){
                    ++smallHCur;
                }else colorState = 4;
                break;
            case 4:
                if(smallHCur > smallHMax){
                    smallHMax = smallHCur;
                    colorState = 5;
                    smallHCur = 0;
                }else colorState = 3;
                break;
            default:
                i += 3;
                j = 0;
                colorState = 0;
                break;
            }
        }
        SmartDashboard.putNumber("Large H: ", largeHMax);
        SmartDashboard.putNumber("Small H: ", smallHMax);
        return true;
    }