Putting Nick's code into the dev branch

build.properties

@@ -3,4 +3,3 @@ package=org.usfirst.frc.team4624.robot
 robot.class=${package}.Robot
 simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world
 #Uncomment and point at user libraries to include them in the build. Do not put libraries in the \wpilib\java folder, this folder is completely overwritten on plugin update.
-#userLibs=C:/Users/Developer/Documents/GitHub/2017RobotCode/navx-mxp/java/src/com/kauailabs/navx/frc/AHRS.java

src/org/usfirst/frc/team4624/robot/OI.java

@@ -25,6 +25,13 @@ public class OI {
 	public static JoystickButton	button2_1, button2_2, button2_3, button2_4, button2_5, button2_6, button2_7, button2_8, button2_9,
 			button2_10, button2_11;
 
+	public static final int resetPosition = 8;
+	public static final int turnFoward = 9;
+	public static final int turnLeft = 10;
+	public static final int turnRight = 12;
+	public static final int turnBack = 11;
+
+
 	public OI() {
 
 		button1 = new JoystickButton(joystick, 1);
@@ -54,6 +61,7 @@ public OI() {
 		xboxController.rb.whenPressed(new ShootUp());
 		xboxController.lb.whenPressed(new ShootDown());
 		xboxController.x.whenPressed(new ShootStop());
+		//YMNTL
 
 	}
 

src/org/usfirst/frc/team4624/robot/Robot.java

@@ -2,10 +2,13 @@
 package org.usfirst.frc.team4624.robot;
 
 import org.usfirst.frc.team4624.robot.subsystems.DriveTrain;
+
 import org.usfirst.frc.team4624.robot.subsystems.Shooter;
 import org.usfirst.frc.team4624.template.ExampleCommand;
 import org.usfirst.frc.team4624.template.ExampleSubsystem;
 
+import com.kauailabs.navx.frc.AHRS;
+
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.PIDController;
@@ -23,16 +26,16 @@
  * 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 {
+public class Robot extends IterativeRobot{
 
 	public static final ExampleSubsystem	exampleSubsystem	= new ExampleSubsystem();
 	public static final DriveTrain			driveTrain			= new DriveTrain();
 	public static OI						oi;
 	public static final Shooter				shooter				= new Shooter();
+	public static AHRS                      navX;
 
 	Command									autonomousCommand;
 	SendableChooser							chooser;
-
 	/**
 	 * This function is run when the robot is first started up and should be used for any initialization code.
 	 */
@@ -43,6 +46,13 @@ public void robotInit() {
 		chooser.addDefault("Default Auto", new ExampleCommand());
 		// chooser.addObject("My Auto", new MyAutoCommand());
 		SmartDashboard.putData("Auto mode", chooser);
+
+		try{
+			navX = new AHRS(SPI.Port.kMXP);
+		}catch(RuntimeException ex){
+			DriverStation.reportError("Error instantiating navX MXP: " + ex.getMessage(),true);
+		}
+
 	}
 
 	/**
@@ -99,12 +109,16 @@ public void teleopInit() {
 	 */
 	public void teleopPeriodic() {
 		Scheduler.getInstance().run();
+		SmartDashboard.putNumber("NavX Gyro: ", Robot.navX.getYaw());
+
 	}
 
+
 	/**
 	 * This function is called periodically during test mode
 	 */
 	public void testPeriodic() {
 		LiveWindow.run();
 	}
+
 }

src/org/usfirst/frc/team4624/robot/RobotMap.java

@@ -12,6 +12,13 @@ public class RobotMap {
 	public static final int	backRightMotor	= 3;
 	public static final int	shooterMotor	= 4;
 
+	//DIO
+	public static final int echoChannel = 0;
+	public static final int pingChannel = 1;
+
+	//AutoRotate buttons
+
+
 	// For example to map the left and right motors, you could define the
 	// following variables to use with your drivetrain subsystem.
 	// public static int leftMotor = 1;

src/org/usfirst/frc/team4624/robot/commands/Drive.java

@@ -2,43 +2,67 @@
 package org.usfirst.frc.team4624.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import org.usfirst.frc.team4624.robot.Robot;
 
 /**
  *
  */
 public class Drive extends Command {
-
+
+	float tagetAngle;
+
 	public Drive() {
 		// Use requires() here to declare subsystem dependencies
 		requires(Robot.driveTrain);
 	}
-	
+
 	// Called just before this Command runs the first time
 	protected void initialize() {
 		Robot.driveTrain.stop();
 	}
-	
+
 	// Called repeatedly when this Command is scheduled to run
 	protected void execute() {
 		// use this for Xbox controls
-		//Robot.driveTrain.driveRaw(Robot.oi.xboxController.leftStick.getX(), Robot.oi.xboxController.leftStick.getY(), 
-		//		Robot.oi.xboxController.rightStick.getX());
-		//use this for the twisty joystick
+		// Robot.driveTrain.driveRaw(Robot.oi.xboxController.leftStick.getX(),
+		// Robot.oi.xboxController.leftStick.getY(),
+		// Robot.oi.xboxController.rightStick.getX());
+		// use this for the twisty joystick
 		Robot.driveTrain.driveRaw(Robot.oi.joystick.getX(), Robot.oi.joystick.getY(), Robot.oi.joystick.getTwist());
+
+		if (Robot.oi.joystick.getRawButton(Robot.oi.turnBack)) {
+			Robot.driveTrain.autoRotate(179.9f);
+		}
+		if (Robot.oi.joystick.getRawButton(Robot.oi.turnFoward)) {
+			Robot.driveTrain.autoRotate(0.1f);
+		}
+
+		if (Robot.oi.joystick.getRawButton(Robot.oi.turnLeft)) {
+			Robot.driveTrain.autoRotate(90.0f);
+		}
+
+		if (Robot.oi.joystick.getRawButton(Robot.oi.turnRight)) {
+			Robot.driveTrain.autoRotate(-90.0f);
+		}
+
+		if (Robot.oi.joystick.getRawButton(Robot.oi.resetPosition)) {
+			Robot.navX.reset();
+		}
+
 	}
-	
+
 	// Make this return true when this Command no longer needs to run execute()
 	protected boolean isFinished() {
 		return false;
 	}
-	
+
 	// Called once after isFinished returns true
 	protected void end() {
 		Robot.driveTrain.stop();
 	}
-	
+
 	// Called when another command which requires one or more of the same
 	// subsystems is scheduled to run
 	protected void interrupted() {

src/org/usfirst/frc/team4624/robot/subsystems/DriveTrain.java

@@ -10,45 +10,43 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.VictorSP;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  *
  */
 public class DriveTrain extends Subsystem {
-	
+
 	// Put methods for controlling this subsystem
 	// here. Call these from Commands.
-
-	final VictorSP			fLMotor			= new VictorSP(RobotMap.frontLeftMotor);
-	final VictorSP			fRMotor			= new VictorSP(RobotMap.frontRightMotor);
-	final VictorSP			bLMotor			= new VictorSP(RobotMap.backLeftMotor);
-	final VictorSP			bRMotor			= new VictorSP(RobotMap.backRightMotor);
-
-	public static double	rotateToAngleRate;
-	boolean					rotateToAngle	= false;
-
-	RobotDrive				driveTrain		= new RobotDrive(fLMotor, bLMotor, fRMotor, bRMotor);
-
+
+	final VictorSP fLMotor = new VictorSP(RobotMap.frontLeftMotor);
+	final VictorSP fRMotor = new VictorSP(RobotMap.frontRightMotor);
+	final VictorSP bLMotor = new VictorSP(RobotMap.backLeftMotor);
+	final VictorSP bRMotor = new VictorSP(RobotMap.backRightMotor);
+
+	RobotDrive driveTrain = new RobotDrive(fLMotor, bLMotor, fRMotor, bRMotor);
+
 	public void initDefaultCommand() {
 		// Set the default command for a subsystem here.
 		setDefaultCommand(new Drive());
 	}
-	
+
 	public void drive(double x, double y, double rotation) {
 		double xmove = Math.pow(x, 3);
 		double ymove = Math.pow(y, 3);
 		double rmove = Math.pow(rotation, 3);
 		driveTrain.mecanumDrive_Cartesian(xmove, ymove, rmove, 0);
-		
+
 	}
-	
+
 	public void driveRaw(double x1, double y1, double x2) {
 		double x1move = (Math.pow(x1, 3));
 		double x2move = Math.pow(x2, 3);
 		double y1move = Math.pow(y1, 3);
-		
+
 		double deadZone = .07;
-		
+
 		if (x1move < deadZone && x1move > -deadZone) {
 			x1move = 0;
 		}
@@ -58,28 +56,90 @@ public void driveRaw(double x1, double y1, double x2) {
 		if (x2move < deadZone && x2move > -deadZone) {
 			x2move = 0;
 		}
-		
+
 		if ((Robot.oi.joystick.getRawButton(1))) { // should invert controls
 			set(fLMotor, (-x1move + y1move + (x2move / 2)));
 			set(bLMotor, (x1move + y1move + (x2move / 2)));
 			set(fRMotor, (-x1move - y1move + (x2move / 2)));
 			set(bRMotor, (x1move - y1move + (x2move / 2)));
-		}
-		else {
+		} else {
 			set(fLMotor, (x1move - y1move + (x2move / 2)));
 			set(bLMotor, (-x1move - y1move + (x2move / 2)));
 			set(fRMotor, (x1move + y1move + (x2move / 2)));
 			set(bRMotor, (-x1move + y1move + (x2move / 2)));
 		}
 	}
-
+
+	/**
+	 * Rotates to a given angle based on the NavX's gyro
+	 * 
+	 * @param targetAngle
+	 *            targeted angle
+	 */
+
+	@SuppressWarnings("deprecation")
+	public void autoRotate(float targetAngle) {
+		float acceptedError = 2.0f;
+		boolean turn = false;
+		int timeSince = 1;
+
+		while (Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw())) > acceptedError) {
+			//SmartDashboard.putNumber("NavX Gyro: ", Robot.navX.getYaw());
+			turn = whichWay(targetAngle);
+
+			if (!turn) {
+				set(fLMotor, ((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw()))) / 360) + .1);
+				set(fRMotor, ((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw()))) / 360) + .1);
+				set(bLMotor, ((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw()))) / 360) + .1);
+				set(bRMotor, ((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw())))/ 360) + .1);
+
+			} else {
+				set(fLMotor, -((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw()))) / 360) - .1);
+				set(fRMotor, -((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw()))) / 360) - .1);
+				set(bLMotor, -((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw()))) / 360) - .1);
+				set(bRMotor, -((Math.abs(Math.abs(targetAngle) - Math.abs(Robot.navX.getYaw()))) / 360) - .1);
+
+			}
+			SmartDashboard.putInt("Time Since: ", timeSince);
+		}
+
+	}
+
+	/**
+	 * Determines which way the robot should turn.
+	 * 
+	 * @param target
+	 *            The targeted angle (Float)
+	 * @return True if it should turn counter-clockwise
+	 */
+
+	public static boolean whichWay(float target) {
+		// true = counter-clockwise
+
+		float currentPos = Robot.navX.getYaw();
+		float targetPos;
+		targetPos = target + 180;
+		currentPos = currentPos + 180;
+		if (targetPos > currentPos) {
+			if (targetPos - currentPos >= 180) {
+				return true;
+			} else
+				return false;
+		} else {
+			if (currentPos - targetPos >= 180) {
+				return false;
+			} else
+				return true;
+		}
+	}
+
 	public void stop() {
 		fLMotor.setDisabled();
 		bLMotor.setDisabled();
 		fRMotor.setDisabled();
 		bRMotor.setDisabled();
 	}
-	
+
 	public void set(VictorSP motor, double speed) {
 		if (speed == 0) {
 			motor.setDisabled();