wh1ter0se / PowerUp-2018

public class Vision extends IterativeRobot {

    //Two cameras for double FOV

    private UsbCamera cameraLeft;

    private UsbCamera cameraRight;

    public void robotInit() {

        //Places the vision in a separate thread from everything else as recommended by FIRST

        //It should never be accessed by other code, so protection isn't necessary.

        new Thread(() -> {

            //Initialize cameras as the left and right respectively.

            cameraLeft = CameraServer.getInstance().startAutomaticCapture("Left", 0);

            cameraRight = CameraServer.getInstance().startAutomaticCapture("Right", 1);

            //Dummy sinks to keep camera connections open.

            CvSink cvsinkLeft = new CvSink("leftSink");

            cvsinkLeft.setSource(cameraLeft);

            cvsinkLeft.setEnabled(true);

            CvSink cvsinkRight = new CvSink("rightSink");

            cvsinkRight.setSource(cameraRight);

            cvsinkRight.setEnabled(true);

            //Matrices to store each image from the cameras.

            //Labeled left and right respectively.

            Mat leftSource = new Mat();

            Mat rightSource = new Mat();

            //The arraylist of left and right sources is needed for concatenating

            ArrayList<Mat> sources = new ArrayList<>();

            sources.add(leftSource);

            sources.add(rightSource);

            //Concatenation of both matrices

            Mat concat = new Mat();

            //Puts the combined video on the SmartDashboard (I think)

            while (!Thread.interrupted()) {

                //Provide each mat with the current frame

                cvsinkLeft.grabFrame(leftSource);

                cvsinkRight.grabFrame(rightSource);

                //Combine the frames into a single mat in the Output

                Core.hconcat(sources, concat);

                outputStream.putFrame(concat);

            }

        }).start();