TrackTarget.java

public TrackTarget() {
        // Use requires() here to declare subsystem dependencies
        requires(Robot.turret);
        /* Calibration */
        Robot.turret.cameraRaw.setResolution(640, 480);
        Robot.turret.cameraRaw.setWhiteBalanceAuto();
        DriverStation.reportWarning(")))Calibration Starting!(((", false);
        try {
            Thread.sleep(15000);
        } catch (Exception e) {
            DriverStation.reportError(e.getMessage(), true);
        }
        Robot.turret.cameraRaw.setWhiteBalanceHoldCurrent();
        Robot.turret.cameraRaw.setExposureManual(30);
        DriverStation.reportWarning(")))Finished Calibration(((", false);
        Robot.visionThread = new Thread(() -> {
           
           // Get a CvSink. This will capture Mats from the camera
           CvSink cvSink = CameraServer.getInstance().getVideo();
           // Setup a CvSource. This will send images back to the Dashboard
           CvSource outputStream = CameraServer.getInstance().putVideo("Tracking", 640, 480);
           
           // The while loop cannot be always 'true'. The program will never exit if it is. This
           // lets the robot stop this thread when restarting robot code or
           // deploying.
           
           int r = 10;
           int g = 120;
           int b = 0;
           int rr = 255;
           int gg = 255;
           int bb = 100;
           double[] curColor = new double[3];
           int timeItr = 0;
           
           //Too many error occurs even when not looking at the Tracking CameraServer. Could be a fault with how the initialization happens?
           
           while (!Thread.interrupted()) {
               // Tell the CvSink to grab a frame from the camera and put it
               // in the source mat.  If there is an error notify the output.
               if(timeItr >= 3){
                   timeItr = 0;
                   if (cvSink.grabFrame(mat) == 0) {
                       // Send the output the error.
                       outputStream.notifyError(cvSink.getError());
                       // skip the rest of the current iteration
                       continue;
                   }
                   //Imgproc.cvtColor(mat, cvt, Imgproc.COLOR_BGR2HLS);
                   Core.inRange(mat, new Scalar(r, g, b, 0), new Scalar(rr, gg, bb, 255), cvt);
                   if(OI.driverPad.getBackButton()){
                       if(OI.driverPad.getBButton()){
                           ++bb;
                       }
                       if(OI.driverPad.getYButton()){
                           ++gg;
                       }
                       if(OI.driverPad.getXButton()){
                           ++rr;
                       }
                       if(OI.driverPad.getStartButton()){
                           rr = 180;
                           gg = 220;
                           bb = 100;
                       }
                   }
                   else{
                       if(OI.driverPad.getBButton()){
                           ++b;
                       }
                       if(OI.driverPad.getYButton()){
                           ++g;
                       }
                       if(OI.driverPad.getXButton()){
                           ++r;
                       }
                       if(OI.driverPad.getStartButton()){
                           r = 10;
                           g = 100;
                           b = 0;
                       }
                   }
                   SmartDashboard.putNumber("R: ", r);
                   SmartDashboard.putNumber("G: ", g);
                   SmartDashboard.putNumber("B: ", b);
                   SmartDashboard.putNumber("RR: ", rr);
                   SmartDashboard.putNumber("GG: ", gg);
                   SmartDashboard.putNumber("BB: ", bb);
                   //DriverStation.reportWarning("Mat: " + mat.type(), false);
                   // Process Image
                   int largeHMax = 0;
                   int largeHCur = 0;
                   int smallHMax = 0;
                   int smallHCur = 0;
                   
                   int state = 0; //0 = top, 1 = large, 2 = middle, 3 = small,
                   for(int i = 0; i < cvt.cols(); i+=3){
                       for(int j = 0; j < cvt.rows(); ++j){
                           curColor = cvt.get(j, i);
                           switch(state){
                           case 0:
                               if(curColor[0] > 127){
                                   state = 1;
                               }else{
                                   
                               }
                               break;
                           case 1:
                               if(curColor[0] > 127){
                                   ++largeHCur;
                               }else{
                                   if(largeHCur > 5){
                                       state = 2;
                                   }else{
                                       largeHCur = 0;
                                       state = 0;
                                   }
                               }
                               break;
                           case 2:
                               if(curColor[0] > 127){
                                   ++smallHCur;
                                   state = 3;
                               }else{
                                   if(largeHCur > 5){
                                       state = 2;
                                       if(largeHCur > largeHMax){
                                           largeHMax = largeHCur;
                                           largeHCur = 0;
                                       }
                                   }else{
                                       largeHCur = 0;
                                       state = 0;
                                   }
                               }
                               break;
                           case 3:
                               if(curColor[0] > 127){
                                   ++smallHCur;
                               }else{
                                   if(smallHCur > smallHMax){
                                       smallHMax = smallHCur;
                                       smallHCur = 0;
                                   }
                                   state = 0;
                                   i += 3;
                                   j = 0;
                               }
                               break;
                           default:
                               i += 3;
                               j = 0;
                               state = 0;
                               break;
                           }
                       }
                   }
                   ++timeItr;
                   SmartDashboard.putNumber("Large H: ", largeHMax);
                   SmartDashboard.putNumber("Small H: ", smallHMax);
                   SmartDashboard.putNumber("Mat H: ", cvt.rows());
                   // Give the output stream a new image to display
                   outputStream.putFrame(cvt);
               }
           }
       });
       Robot.visionThread.setDaemon(true);
       Robot.visionThread.start();
    }