package org.usfirst.frc2832.Robot_2016.commands.autonomous;

import org.usfirst.frc2832.Robot_2016.RobotMap;
import org.usfirst.frc2832.Robot_2016.TrajectoryController;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class RotateAngle extends Command {
	static double angle, initVal;
	static final double TOLERANCE = 2;
	static TrajectoryController tc;
	
	//IN DEGREES
    public RotateAngle(double theta) {
        // Use requires() here to declare subsystem dependencies
        // e.g. requires(chassis);
    	angle = theta;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	initVal = RobotMap.imu.getYaw();
    	tc = new TrajectoryController(angle, 0.3, 0.4, 0.5, 1, -1); //TO-DO: would be nice to test these numbers!
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	RobotMap.driveTrain.arcadeDrive(0, tc.get(RobotMap.imu.getYaw())); 
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return Math.abs(RobotMap.imu.getYaw() - initVal - angle) < TOLERANCE;
    }

    // Called once after isFinished returns true
    protected void end() {
    	RobotMap.driveTrain.arcadeDrive(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}

@@ -216,16 +216,17 @@
 %0A%0A/**%0A *
+ 
 %0A */%0Apub
@@ -297,16 +297,18 @@
 itVal;%0A%09
+//
 static f
@@ -367,16 +367,271 @@
 ler tc;%0A
+%09private double isPos, curVal, curDisplacement = 0;%0A%09//isPos: turns into a 1 is moving positive angle, -1 if moving negative angle%0A%09//curVal: the current (NOT adjusted) value of our angle%0A%09//curDisplacement: the adjustment factor due to gyro wraparound%0A%09%0A
 %09%0A%09//IN 
@@ -979,17 +979,17 @@
 ngle, 0.
-3
+4
 , 0.4, 0
@@ -993,16 +993,20 @@
 , 0.
-5, 1, -1
+8, 0.9, -0.9
 ); /
@@ -1050,16 +1050,72 @@
 umbers!%0A
+    %09isPos = Math.signum(angle);%0A    %09curVal = initVal;%0A
     %7D%0A%0A 
@@ -1216,50 +1216,260 @@
    %09
-RobotMap.driveTrain.arcadeDrive(0, tc.get(
+if (curVal * isPos %3E RobotMap.imu.getYaw() * isPos) //This will be true iff the gyro val has just wrapped around%0A    %09%09curDisplacement += 180*isPos;%0A    %09%09%09%0A    %09RobotMap.driveTrain.arcadeDrive(0, tc.get(curVal + curDisplacement));%0A    %09%0A    %09curVal = 
 Robo
@@ -1489,12 +1489,9 @@
 aw()
-)); 
+;
 %0A   
@@ -1630,69 +1630,68 @@
 urn 
-Math.abs(RobotMap.imu.getYaw() - initVal - angle) %3C TOLERANCE
+(curVal + curDisplacement - initVal) * isPos %3E angle * isPos
 ;%0A  

Fix RotateAngle.java to account for gyro wraparound (it jumps from -180 to 180 and vice versa).