
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.
	 */
	static final boolean TEST = true;

	DriveTrain drive;

	Joystick rJoy;
	Joystick lJoy;

	AHRS navX;

	AnalogInput dSensor;

	CameraServer cam;

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

		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();

		drive.run(YAxisLeft, YAxisRight, (double) rJoy.getPOV(0), rJoy.getTrigger(), lJoy.getTrigger(),
				rJoy.getRawButton(2), rJoy.getThrottle());

		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() {

	}

}

@@ -2286,10 +2286,400 @@
  %7B%0A%0A%09%7D%0A%0A
+%09public int WASDToAngle(boolean W, boolean A, boolean S, boolean D) %7B%0A%09%09if (W) %7B%0A%09%09%09if (A) %7B%0A%09%09%09%09return 45;%0A%09%09%09%7D else if (D) %7B%0A%09%09%09%09return -45;%0A%09%09%09%7D else %7B%0A%09%09%09%09return 0;%0A%0A%09%09%09%7D%0A%09%09%7D else if (S) %7B%0A%09%09%09if (A) %7B%0A%09%09%09%09return 135;%0A%09%09%09%7D else if (D) %7B%0A%09%09%09%09return -135;%0A%09%09%09%7D else %7B%0A%09%09%09%09return 180;%0A%09%09%09%7D%0A%09%09%7D else if (A) %7B%0A%09%09%09return 90;%0A%09%09%7D else if (D) %7B%0A%09%09%09return -90;%0A%09%09%7D else %7B%0A%09%09%09return -5000;%0A%09%09%7D%0A%09%7D%0A
 %7D%0A

put WASD in code to turn WASD into angles