Java Code Examples for edu.wpi.first.wpilibj.DriverStation#reportWarning()

private void frameCameraStream(){
	cameraFrame = CameraServer.getInstance().startAutomaticCapture("Frame", VisionConstants.HOOK_ID);
	
	CvSink cvsinkFrame = new CvSink("frameSink");
	cvsinkFrame.setSource(cameraFrame);
	cvsinkFrame.setEnabled(true);
	
	Mat streamImages = new Mat();

	CvSource outputFrame = CameraServer.getInstance().putVideo("Frame", VisionConstants.CAM_WIDTH, VisionConstants.CAM_HEIGHT);
	 while (!Thread.interrupted()){
         try {
             cvsinkFrame.grabFrame(streamImages);
             if ((Robot.bot == Bot.TEUFELSKIND && Constants.TEUFELSKIND.FRAME_CAM_FLIP)
                     || (Robot.bot == Bot.OOF && Constants.OOF.FRAME_CAM_FLIP)) {
                 Core.rotate(streamImages, streamImages, Core.ROTATE_180);
             }
             outputFrame.putFrame(streamImages);
         } catch (CvException cameraFail){
             DriverStation.reportWarning("Frame Camera: " + cameraFail.toString(), false);
             outputFrame.putFrame(failImage);
         }
	 }
}
 

private void screwCameraStream(){

    	cameraScrew = CameraServer.getInstance().startAutomaticCapture("Screw", VisionConstants.SCREW_ID);
    	
    	CvSink cvsinkScrew = new CvSink("screwSink");
    	cvsinkScrew.setSource(cameraScrew);
    	cvsinkScrew.setEnabled(true);
    	
    	Mat streamImages = new Mat();
    	
    	CvSource outputScrew = CameraServer.getInstance().putVideo("Screw", VisionConstants.CAM_WIDTH, VisionConstants.CAM_HEIGHT);
    	 while (!Thread.interrupted()){
    	     try {
                 cvsinkScrew.grabFrame(streamImages);
                 if ((Robot.bot == Bot.TEUFELSKIND && Constants.TEUFELSKIND.SCREW_CAM_FLIP)
                         || (Robot.bot == Bot.OOF && Constants.OOF.SCREW_CAM_FLIP)) {
                     Core.rotate(streamImages, streamImages, Core.ROTATE_180);
                 }
                 outputScrew.putFrame(streamImages);
             } catch (CvException cameraFail){
    	         DriverStation.reportWarning("Screw Camera: " + cameraFail.toString(), false);
    	         outputScrew.putFrame(failImage);
             }
    	 }
    }
 

/** runs when autonomous start */
public void autonomousInit() {
	DriverStation.reportWarning("AUTONOMOUS IS STARTING...", false);
	if(goalChooser.getSelected() != null) {
		auto = new CommandGroupAuto(positionChooser.getSelected(), goalChooser.getSelected());
		auto.start(); 
	} 
}
 

protected void initialize() {
	DriverStation.reportWarning("ROTATING " + (inches / SCALAR) + " DEGREES" + ((inches > 0) ? "CW" : "CCW"), false);
    time = System.currentTimeMillis() + TIME_WAIT;
    inRange = false;
    Robot.SUB_DRIVE.pid.reset();
    time = System.currentTimeMillis() + TIME_WAIT;
    inches = Util.getAndSetDouble("Rotate Degrees", 0) * SCALAR;
    Robot.SUB_DRIVE.pid.setPIDF(Util.getAndSetDouble("P", .11),
            Util.getAndSetDouble("I", 0),
            Util.getAndSetDouble("D", 0),
            Util.getAndSetDouble("F", 0));
    inRange = Robot.SUB_DRIVE.driveDistance(inches, -1* inches);
}
 

protected boolean isFinished() {
	DriverStation.reportWarning("DONE ROTATING", false);
    if(!inRange) {
        time = System.currentTimeMillis() + TIME_WAIT;
    }
    return time < System.currentTimeMillis();
}
 

protected void execute() {
	DriverStation.reportWarning("DRIVING " + inches + " INCHES", false);
	SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches());
	SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches());

    SmartDashboard.putNumber("Error", Robot.SUB_DRIVE.pid.getError());
}
 

/** runs when teleop starts*/
public void teleopInit() {
	DriverStation.reportWarning("TELEOP IS ENABLED", false);
	if (auto != null)
		auto.cancel(); 
}
 

/** assigns what every SmartDash and controller button does */
public OI() {
	/// manipulator wheels
		Button spinIn = new JoystickButton(OPERATOR, Xbox.RB);
				spinIn.whileHeld(new ButtonCommandEat());
		Button spinOut = new JoystickButton(OPERATOR, Xbox.LB);
				spinOut.whileHeld(new ButtonCommandSpit());
	/// manipulator clamp
		Button toggleClamp = new JoystickButton(OPERATOR, Xbox.A);
			toggleClamp.toggleWhenActive(new ToggleCommandClamp());
	/// candy cane
		Button toggleHook = new JoystickButton(OPERATOR, Xbox.B);
			toggleHook.toggleWhenActive(new ToggleCommandHook());
	/// drop the mast
		Button dropIt = new JoystickButton(OPERATOR, Xbox.X);
			dropIt.toggleWhenPressed(new ButtonCommandHitTheDeck());
	/// Reversing mode
		Button toggleReverse = new JoystickButton(DRIVER, Xbox.Y);
			toggleReverse.toggleWhenPressed(new ToggleCommandReverse());
	/// Docking mode
		Button toggleDock = new JoystickButton(DRIVER, Xbox.X);
			toggleDock.toggleWhenPressed(new ToggleCommandDock());
	/// To Compress, or Not To Compress. It is now an option.
		SmartDashboard.putData("Disable Compressor", new ToggleCommandKillCompressor());
		

	/// PID
		SmartDashboard.putData("Kill PID", new ToggleCommandKillPID());
		SmartDashboard.putNumber("Right Encoder Position", 0);
		SmartDashboard.putNumber("Left Encoder Position", 0);
		
	/// limit switch displays
		SmartDashboard.putBoolean("Lower Screw", true);
    	SmartDashboard.putBoolean("Upper Screw", false);
    	SmartDashboard.putBoolean("Lower Pinion", true);
    	SmartDashboard.putBoolean("Upper Pinion", false);
    	
    	SmartDashboard.putNumber("Left inches", 0);
    	SmartDashboard.putNumber("Right inches", 0);
    	
    	DriverStation.reportWarning("OI IS INSTANTIATED", false);

    /// Cyborg command testers
		SmartDashboard.putData("Drive Direct", new CyborgCommandDriveDirect(Util.getAndSetDouble("Drive Direct Power", 0)));
		SmartDashboard.putData("Drive Distance", new CyborgCommandDriveDistance(Util.getAndSetDouble("Drive Distance Inches", 0)));
		SmartDashboard.putData("Drive Until Error", new CyborgCommandDriveUntilError());
		SmartDashboard.putData("Rotate Degree", new CyborgCommandRotateDegrees(Util.getAndSetDouble("Rotate Degrees", 0)));
		SmartDashboard.putData("Spit", new CyborgCommandSpit((long)Util.getAndSetDouble("Spit Time", 500)));
		SmartDashboard.putData("Raise to Position: Pinion Up", new CyborgCommandGrow(Mast.PINION_UP));
		SmartDashboard.putData("Raise to Position: Pinion Down", new CyborgCommandGrow(Mast.PINION_DOWN));
		SmartDashboard.putData("Raise to Position: Screw Up", new CyborgCommandGrow(Mast.SCREW_UP));
		SmartDashboard.putData("Raise to Position: Screw Down", new CyborgCommandGrow(Mast.SCREW_DOWN));
}
 

/** runs when robot gets disabled */
public void disabledInit() { 
	DriverStation.reportWarning("TELEOP IS DISABLED", false);
	Scheduler.getInstance().removeAll();
}
 

/** runs when robot is turned on */
	public void robotInit() {
		/// instantiate bot chooser
		botChooser = new SendableChooser<>();
		botChooser.addDefault(Bot.TEUFELSKIND.toString(), Bot.TEUFELSKIND);
		botChooser.addObject(Bot.OOF.toString(), Bot.OOF);
		SmartDashboard.putData("Bot", botChooser);

		if (botChooser.getSelected() != null){
			bot = botChooser.getSelected();
		} else {
			bot = Bot.OOF;
		}

			DriverStation.reportWarning("ROBOT STARTED; GOOD LUCK", false);
		/// instantiate subsystems
//			SUB_ARDUINO = new SubsystemArduino();
			SUB_MANIPULATOR = new SubsystemManipulator();
			SUB_CLAMP = new SubsystemClamp();
			SUB_COMPRESSOR = new SubsystemCompressor();
			SUB_DRIVE = new SubsystemDrive();
			
			SUB_DRIVE.pid.setPIDF(.5, 0, 0, 0);
			
			SUB_HOOK = new SubsystemHook();
			SUB_MAST = new SubsystemMast();
			vision = new Vision();

		/// instantiate operator interface
			oi = new OI();

			
			
		/// instantiate drivetrain chooser
				driveChooser = new SendableChooser<>();
				driveChooser.addDefault(Drivetrain.ROCKET_LEAGUE.toString(), Drivetrain.ROCKET_LEAGUE); // set default to RL drive
				for(int i = 1; i < Drivetrain.values().length; i++) { 
					driveChooser.addObject(Drivetrain.values()[i].toString(), Drivetrain.values()[i]); } // add each drivetrain enum value to chooser
				SmartDashboard.putData("Drivetrain", driveChooser); //display the chooser on the dash
			
		/// instantiate position chooser
				positionChooser = new SendableChooser<>();
				positionChooser.addDefault(Position.CENTER.toString(), Position.CENTER); // set default to center
				for(int i = 1; i < Position.values().length; i++) { 
					positionChooser.addObject(Position.values()[i].toString(), Position.values()[i]); } // add each position enum value to chooser
				SmartDashboard.putData("Position", positionChooser); //display the chooser on the dash

		/// instantiate goal chooser
				goalChooser = new SendableChooser<>();
				goalChooser.addDefault(Goal.NOTHING.toString(), Goal.NOTHING); // set default to nothing
				for(int i = 1; i < Goal.values().length; i++) { 
					goalChooser.addObject(Goal.values()[i].toString(), Goal.values()[i]); } // add each autonomous goal to chooser
				SmartDashboard.putData("Goal", goalChooser); //display the chooser on the dash
			
		/// instantiate bot chooser
				botChooser = new SendableChooser<>();
				botChooser.addDefault(Bot.TEUFELSKIND.toString(), Bot.TEUFELSKIND);
					botChooser.addObject(Bot.OOF.toString(), Bot.OOF); 
				SmartDashboard.putData("Bot", botChooser);
				
				
				
		/// instantiate cameras
			vision.startScrewCameraThread();
			vision.startFrameCameraThread();

		SmartDashboard.putData("Sub_Drive", SUB_DRIVE);
		DriverStation.reportWarning("SUBSYSTEMS, CHOOSERS INSTANTIATED", false);
	}
 

/**
 * 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.
 * This method should only be used for starting the camera stream.
 */
private void concatCameraStream() {
    cameraLeft = CameraServer.getInstance().startAutomaticCapture("Left", VisionConstants.LEFT_ID);
    cameraRight = CameraServer.getInstance().startAutomaticCapture("Right", VisionConstants.RIGHT_ID);

    /// 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
    CvSource outputStream = CameraServer.getInstance().putVideo("Concat", 2*VisionConstants.CAM_WIDTH, VisionConstants.CAM_HEIGHT);

    while (!Thread.interrupted()) {
        try {
            /// 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);
        } catch (CvException cameraFail){
            DriverStation.reportWarning("Concat Cameras: " + cameraFail.toString(), false);
            outputStream.putFrame(failImage);
        }
    }
}
 

protected void end() {
    DriverStation.reportWarning("CyborgCommandDriveUntilError finished", false);
    Robot.SUB_DRIVE.driveDirect(0, 0);
}
 

protected void end() {
    DriverStation.reportWarning("CyborgCommandDriveDistance finished", false);
    Robot.SUB_DRIVE.driveDirect(0, 0);
}
 

protected void end() {
	DriverStation.reportWarning("DONE WAITING", false);
}
 

protected void initialize() {
	DriverStation.reportWarning("WAITING " + wait + " MILISECONDS", false);
	startTime = System.currentTimeMillis();
}
 

protected void end() {
    DriverStation.reportWarning("CyborgCommandRotateDegrees finished", false);
    Robot.SUB_DRIVE.driveDirect(0, 0);
}
 

protected void execute() {
    DriverStation.reportWarning("ROTATING " + (inches / SCALAR) + " DEGREES" + ((inches > 0) ? "CW" : "CCW"), false);
    SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches());
    SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches());
    SmartDashboard.putNumber("Error", Robot.SUB_DRIVE.pid.getError());
}
 

protected void end() {
    DriverStation.reportWarning("CyborgCommandDriveDirect finished", false);
    Robot.SUB_DRIVE.driveDirect(0, 0);
}
 

protected void initialize() {
	DriverStation.reportWarning("DRIVING BY POWER", false);
    Robot.SUB_DRIVE.pid.reset();
    time = System.currentTimeMillis() + TIME_WAIT;
}