package org.usfirst.frc.team997.robot.commands;

import org.usfirst.frc.team997.robot.Robot;

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

/**
 *
 */
public class DriveToSetpoint extends Command {
	double setpoint;
	double speed = 0.5;
	
    public DriveToSetpoint(double speed, double distance) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.driveTrain);
    	this.setpoint = distance;
    	this.speed = speed;
    }
    
    // set up a default of half speed.  This might still
    // be too fast.
    public DriveToSetpoint(double distance) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.driveTrain);
    	this.setpoint = distance;
    	this.speed = 0.5;
    }


    // Called just before this Command runs the first time
    protected void initialize() {
    	Robot.driveTrain.resetEncoders();
    	Robot.driveTrain.resetGyro();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	double adjust = Robot.driveTrain.getGyro() / 10;

    	final double threshold = .1;
    	// ensure doesn't drastically swerve from readings.
    	if (adjust > threshold) adjust = threshold;
    	else if (adjust < -threshold) adjust = -threshold;
    	
    	SmartDashboard.putNumber("Gyro Drive Straight Adjustment: ", adjust);

    	Robot.driveTrain.driveVoltage(this.speed - adjust, this.speed + adjust);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return Robot.driveTrain.getAverageEncoders() > setpoint;
    }

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

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	Robot.driveTrain.driveVoltage(0, 0);
    }
}

@@ -253,16 +253,30 @@
 mand %7B%0A%09
+private final 
 double s
@@ -286,29 +286,15 @@
 oint
-;%0A%09double speed = 0.5
+, speed
 ;%0A%09%0A
@@ -357,107 +357,8 @@
 ) %7B%0A
-        // Use requires() here to declare subsystem dependencies%0A        // eg. requires(chassis);%0A
     
@@ -588,184 +588,27 @@
     
-// Use requires() here to declare subsystem dependencies%0A        // eg. requires(chassis);%0A    %09requires(Robot.driveTrain);%0A    %09this.setpoint = distance;%0A    %09this.speed = 0.5
+this(0.5, distance)
 ;%0A  
@@ -612,17 +612,16 @@
 %0A    %7D%0A%0A
-%0A
     // C

Use `this()` instead of copy and paste