
package org.usfirst.frc.team910.robot;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SPI;
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.
 */
public class Robot extends IterativeRobot {
	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	boolean test = true;

	DriveTrainTest testdrive;
	DriveTrain drive;

	Joystick rJoy;
	Joystick lJoy;

	AHRS navX;

	AnalogInput dSensor;

	CameraServer cam;

	public void robotInit() {
		navX = new AHRS(SPI.Port.kMXP); // SPI.Port.kMXP

		if (test == true) {
			testdrive = new DriveTrainTest(navX);
		} else {
			drive = new DriveTrain(navX);
		}

		lJoy = new Joystick(IO.LEFT_JOYSTICK);
		rJoy = new Joystick(IO.RIGHT_JOYSTICK);

		dSensor = new AnalogInput(1);

		cam = CameraServer.getInstance();
		cam.startAutomaticCapture("cam0");

	}

	/**
	 * This function is called periodically during autonomous
	 */
	public void autonomousPeriodic() {

	}

	/**
	 * This function is called periodically during operator control
	 */
	public void teleopPeriodic() {

		double YAxisLeft = -lJoy.getY();
		double YAxisRight = -rJoy.getY();

		if (test == false) {

			drive.run(YAxisLeft, YAxisRight, rJoy.getX(), rJoy.getTrigger(), lJoy.getTrigger(), rJoy.getRawButton(2));
		} else {
			testdrive.run(YAxisLeft, YAxisRight, rJoy.getX(), rJoy.getTrigger(), lJoy.getTrigger(),
					rJoy.getRawButton(2));
		}

		if (rJoy.getRawButton(3)) {
			navX.zeroYaw();
		}

		SmartDashboard.putNumber("navX Pitch", navX.getPitch());
		SmartDashboard.putNumber("navX Yaw", navX.getYaw());
		SmartDashboard.putNumber("navX Roll", navX.getRoll());
		SmartDashboard.putNumber("navX X", navX.getRawGyroX());
		SmartDashboard.putNumber("navX Y", navX.getRawGyroY());
		SmartDashboard.putNumber("navX Z", navX.getRawGyroZ());
		SmartDashboard.putNumber("Distance", dSensor.getVoltage());

	}

	/**
	 * This function is called periodically during test mode
	 */
	public void testPeriodic() {

	}

}

@@ -117,51 +117,8 @@
 ut;%0A
-import edu.wpi.first.wpilibj.CameraServer;%0A
 impo
@@ -929,28 +929,8 @@
 r;%0A%0A
-%09CameraServer cam;%0A%0A
 %09pub
@@ -1237,82 +1237,8 @@
 );%0A%0A
-%09%09cam = CameraServer.getInstance();%0A%09%09cam.startAutomaticCapture(%22cam0%22);%0A%0A
 %09%7D%0A%0A

Revert "added basic camera code"