This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() { - m_robotContainer.disableInit(); - } - - @Override - public void disabledPeriodic() {} - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - m_robotContainer.calibrateDrivetrainSteering(); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + private Thread m_visionThread; + + /** + * This function is run when the robot is first started up and should be used + * for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + m_visionThread = new Thread( + () -> { + UsbCamera visionCamera = CameraServer.startAutomaticCapture(); + visionCamera.setResolution(1920, 1080); + CvSink image = CameraServer.getVideo(); + CvSource outputStream = CameraServer.putVideo("video test", 1920, 1080); + Mat imageMat = new Mat(); + while (!Thread.interrupted()) { + image.grabFrame(imageMat); + outputStream.putFrame(imageMat); + } + }); + m_visionThread.setDaemon(true); + m_visionThread.start(); } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + + /** + * This function is called every robot packet, no matter the mode. Use this for + * items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and + * test. + * + *
+ * This runs after the mode specific periodic functions, but before LiveWindow + * and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled + // commands, running already-scheduled commands, removing finished or + // interrupted commands, + // and running subsystem periodic() methods. This must be called from the + // robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); } - m_robotContainer.calibrateDrivetrainSteering(); + /** This function is called once each time the robot enters Disabled mode. */ + @Override + public void disabledInit() { + m_robotContainer.disableInit(); + } - } + @Override + public void disabledPeriodic() { + } - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() { - } + /** + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_robotContainer.calibrateDrivetrainSteering(); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + m_robotContainer.calibrateDrivetrainSteering(); - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() { + } }