<commit_before>// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc2832.Robot2017;

import java.util.ArrayList;

import org.opencv.core.Mat;
import org.opencv.imgproc.Imgproc;
import org.usfirst.frc2832.Robot2017.commands.*;
import org.usfirst.frc2832.Robot2017.commands.gearunload.CloseDoors;
import org.usfirst.frc2832.Robot2017.commands.shooter.AgitatorOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.FeederOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.FeederOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterSequenceOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterSequenceOn;
import org.usfirst.frc2832.Robot2017.pixy.PixyException;
import org.usfirst.frc2832.Robot2017.pixy.PixyI2C;
import org.usfirst.frc2832.Robot2017.pixy.PixyPacket;
import org.usfirst.frc2832.Robot2017.subsystems.*;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
//robot code
public class Robot extends IterativeRobot {

    Command autonomousCommand;

    public static OI oi;
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static DriveTrain driveTrain;
    public static BallIntake ballIntake;
    public static Climbing climb;
    public static GearIntake gearIntake;
    public static GearScore gearScore;
    public static Shooter shooter;
    public static Lights lights;
    public static boolean isClimbing = false, isIngesting = false, povActivated = false;
    public static NavX navX;
    public static double lTrigger, rTrigger;
    public static PixyI2C testPixy;
    public static int shootSpeeed = 50, pov;
    public static SendableChooser<String> auto;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
    	RobotMap.init();
    	RobotMap.driveTrainLeftFront.enableBrakeMode(true);
    	RobotMap.driveTrainRightFront.enableBrakeMode(true);
    	RobotMap.driveTrainLeftRear.enableBrakeMode(true);
    	RobotMap.driveTrainRightRear.enableBrakeMode(true);
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

  //  	lights = new Lights();
    	new Thread(() -> {
	    	
	        
	        UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
	        camera.setResolution(720, 480);
	        CvSink cvSink = CameraServer.getInstance().getVideo();
	        CvSource outputStream = CameraServer.getInstance().putVideo("Blur", 640, 480);
	        
	        Mat source = new Mat();
	        Mat output = new Mat();
	        
	        while(true) {
	            cvSink.grabFrame(source);
	            Imgproc.cvtColor(source, output, Imgproc.COLOR_BGR2GRAY);
	            outputStream.putFrame(output);
	        }
        }).start();
    	
    	new Thread(() -> {
	    	
	        
	        UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
	        camera.setResolution(720, 480);
	        CvSink cvSink = CameraServer.getInstance().getVideo();
	        CvSource outputStream = CameraServer.getInstance().putVideo("Blur", 640, 480);
	        
	        Mat source = new Mat();
	        Mat output = new Mat();
	        
	        while(true) {
	            cvSink.grabFrame(source);
	            Imgproc.cvtColor(source, output, Imgproc.COLOR_BGR2GRAY);
	            outputStream.putFrame(output);
	        }
        }).start();
        

        driveTrain = new DriveTrain();
        ballIntake = new BallIntake();
        climb = new Climbing();
        gearIntake = new GearIntake();
        gearScore = new GearScore();
        shooter = new Shooter();
        lights = new Lights();
        navX = new NavX();
        testPixy = new PixyI2C();
     
        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        // OI must be constructed after subsystems. If the OI creates Commands
        //(which it very likely will), subsystems are not guaranteed to be
        // constructed yet. Thus, their requires() statements may grab null
        // pointers. Bad news. Don't move it.
        oi = new OI();

        // instantiate the command used for the autonomous period
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
        auto = new SendableChooser<String>();<commit_msg>Added code that tests if there is a camera before it starts the stream<commit_after>// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc2832.Robot2017;

import java.util.ArrayList;

import org.opencv.core.Mat;
import org.opencv.imgproc.Imgproc;
import org.usfirst.frc2832.Robot2017.commands.*;
import org.usfirst.frc2832.Robot2017.commands.gearunload.CloseDoors;
import org.usfirst.frc2832.Robot2017.commands.shooter.AgitatorOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.FeederOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.FeederOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterSequenceOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterSequenceOn;
import org.usfirst.frc2832.Robot2017.pixy.PixyException;
import org.usfirst.frc2832.Robot2017.pixy.PixyI2C;
import org.usfirst.frc2832.Robot2017.pixy.PixyPacket;
import org.usfirst.frc2832.Robot2017.subsystems.*;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
//robot code
public class Robot extends IterativeRobot {

    Command autonomousCommand;

    public static OI oi;
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static DriveTrain driveTrain;
    public static BallIntake ballIntake;
    public static Climbing climb;
    public static GearIntake gearIntake;
    public static GearScore gearScore;
    public static Shooter shooter;
    public static Lights lights;
    public static boolean isClimbing = false, isIngesting = false, povActivated = false;
    public static NavX navX;
    public static double lTrigger, rTrigger;
    public static PixyI2C testPixy;
    public static int shootSpeeed = 50, pov;
    public static SendableChooser<String> auto;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
    	RobotMap.init();
    	RobotMap.driveTrainLeftFront.enableBrakeMode(true);
    	RobotMap.driveTrainRightFront.enableBrakeMode(true);
    	RobotMap.driveTrainLeftRear.enableBrakeMode(true);
    	RobotMap.driveTrainRightRear.enableBrakeMode(true);
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

  //  	lights = new Lights();
    	
    	UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
    	camera.setResolution(720, 480);
    	if (camera.isConnected() == true) {
    		
    		new Thread(() -> {
		        	
			        CvSink cvSink = CameraServer.getInstance().getVideo();
			        CvSource outputStream = CameraServer.getInstance().putVideo("Blur", 640, 480);
			        
			        Mat source = new Mat();
			        Mat output = new Mat();
			        
			        while(true) {
			        	System.out.println("Camera1 is conncted");
			            cvSink.grabFrame(source);
			            Imgproc.cvtColor(source, output, Imgproc.COLOR_BGR2GRAY);
			            outputStream.putFrame(output);
			        }
			        
    		}).start();
    		
    	}
    	
    	UsbCamera cameraTwo = CameraServer.getInstance().startAutomaticCapture();
    	cameraTwo.setResolution(720, 480);
    	if (cameraTwo.isConnected() == true) {
    		
    		new Thread(() -> {
		        	
			        CvSink cvSink = CameraServer.getInstance().getVideo();
			        CvSource outputStream = CameraServer.getInstance().putVideo("Blur", 640, 480);
			        
			        Mat source = new Mat();
			        Mat output = new Mat();
			        
			        while(true) {
			        	System.out.println("Camera2 is conncted");
			            cvSink.grabFrame(source);
			            Imgproc.cvtColor(source, output, Imgproc.COLOR_BGR2GRAY);
			            outputStream.putFrame(output);
			        }
			        
    		}).start();
    	}

        driveTrain = new DriveTrain();
        ballIntake = new BallIntake();
        climb = new Climbing();
        gearIntake = new GearIntake();
        gearScore = new GearScore();
        shooter = new Shooter();
        lights = new Lights();
        navX = new NavX();
        testPixy = new PixyI2C();
     
        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        // OI must be constructed after subsystems. If the OI creates Commands
        //(which it very likely will), subsystems are not guaranteed to be
        // constructed yet. Thus, their requires() statements may grab null
        // pointers. Bad news. Don't move it.
        oi = new OI();

        // instantiate the command used for the autonomous period
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
        auto = new SendableChooser<String>();
{'fraction_non_alphanumeric': 0.0895471569628873, 'fraction_numerical': 0.024855294518215866, 'mean_word_length': 3.076682859125607, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7548586', 'n_tokens_mistral': 3960, 'n_tokens_neox': 3560, 'n_words': 920}