// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc5933.StevenDave.commands;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc5933.StevenDave.PreferenceConstants;
import org.usfirst.frc5933.StevenDave.Robot;
import org.usfirst.frc5933.StevenDave.RobotMap;
import org.usfirst.frc5933.StevenDave.subsystems.DriveTrain;

/**
 *
 */
public class GyroTurnDegrees extends Command {

    private final CANTalon frontLeftMotor = RobotMap.driveTrainFrontLeftMotor;
    private final CANTalon frontRightMotor = RobotMap.driveTrainFrontRightMotor;
    private final RobotDrive robotDrive = RobotMap.driveTrainRobotDrive;

    private double speed_ = 0;
    private double degrees_ = 0;
    private boolean useDumbDashboard_ = true;
    private boolean debug_ = false;
    private boolean finished = false;

    private static final int TURN_MAX_TRIES = 1000;
    private int tries = TURN_MAX_TRIES;

    private int desired;
    private double startingAngle;
    private double now;

    public GyroTurnDegrees(double speed, double degrees) {
        speed_ = speed;
        degrees_ = degrees;
        useDumbDashboard_ = false;
        // use the preferences to determine if we should debug this subsystem
        if (Preferences.getInstance().containsKey(PreferenceConstants.DEBUG_SUBSYSTEM_GYRO_KEY)) {
            debug_ = Preferences.getInstance().getBoolean(PreferenceConstants.DEBUG_SUBSYSTEM_GYRO_KEY, false);
        }
    }

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    public GyroTurnDegrees() {

        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING

        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        requires(Robot.driveTrain);

        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES

    }

    // Called just before this Command runs the first time
    protected void initialize() {
        if (useDumbDashboard_) {
            speed_ = .5;
            degrees_ = SmartDashboard.getNumber("Degrees for turning");
        }
        Robot.driveTrain.enableBrakeMode(true);
        Robot.driveTrain.configForTeleopMode();

        RobotMap.helmsman.resetGyro();
        startingAngle = RobotMap.helmsman.getCurrentGyroAngle();
        now = startingAngle;
        //final double desired = now + degrees;
        desired = 57;

        // This call blocks execution, (not really ideal, but hey lets go with it for now)
        // so the operation will return finish during init, so there is no need for us
        // to call anything in execute. This allows us to return true for the isFinished
        // method since we will have finished the turn before isFinished has been called.
        //Robot.driveTrain.gyroTurnDegrees(-degrees_);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
            double speed = 0.5;
            if (now < desired - 50) {
                speed = 0.8;
            }
            else if ( now >= desired-5) {
                System.out.println("It should be stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
                finished = true;
                Robot.driveTrain.stop();
//                end();
            }

            if (desired > startingAngle) {
                //robotDrive.tankDrive(-speed, speed);
                frontLeftMotor.set(-speed);
            } else {
                //robotDrive.tankDrive(speed, -speed);
                frontRightMotor.set(-speed);
            }
            now = RobotMap.helmsman.getCurrentGyroAngle();
            System.out.println("Angle: " + now);
            --tries;
            if (tries == 0) {
                finished = true;
                end();
                if (debug_)
                    System.err.println("Failed to turn specified degrees.");
            }

    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return finished;
    }

    // Called once after isFinished returns true
    protected void end() {
        Robot.driveTrain.enableBrakeMode(false);
    }

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

@@ -1556,19 +1556,22 @@
 private 
-int
+double
  desired
@@ -1631,16 +1631,32 @@
 e now;%0A%0A
+    @Deprecated%0A
     publ
@@ -1718,23 +1718,74 @@
     
-speed_ = speed;
+this(degrees);%0A    %7D%0A%0A    public GyroTurnDegrees(double degrees) %7B
 %0A   
@@ -3187,10 +3187,16 @@
 d = 
-57
+degrees_
 ;%0A%0A 

Remove hardcoded degree, take only the degrees as a parameter.