wh1ter0se / PowerUp-2018

public class Vision extends IterativeRobot {

    /// Two cameras for double FOV

    private UsbCamera cameraLeft;

    private UsbCamera cameraRight;

    /**

     * Initialization of class

     */

    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(this::startCamerasStream).start();

    }

    /**

     * Start both the left and right camera streams and combine them into a single one which is then pushed

     * to an output stream titled Concat

     */

    private void startCamerasStream() {

        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.

        Mat leftSource = new Mat();

        Mat rightSource = new Mat();

        /// The ArrayList of left and right sources is needed for the hconcat method used to combine the streams

        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)

        /// The width is multiplied by 2 as the dimensions of the stream will have a width two times that of a single webcam

        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 and stream the image.

            Core.hconcat(sources, concat);

            outputStream.putFrame(concat);