// 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.frc1073.robot17.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc1073.robot17.Robot;

/**
 *
 */
public class DriveInches extends Command {

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
    private double m_inches;
 
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
    private final double SPEED = .5;
    private final double ANG_MOD_VAL = 20;
    private double currentLeft,currentRight,currentSpeed,destination,currentAvg,currentAng,currentMod;
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    public DriveInches(double inches) {

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

        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
        destination = m_inches*1440;
        // 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() {
    	Robot.driveTrain.setLeftPos(0);
    	Robot.driveTrain.setRightPos(0);
    	Robot.driveTrain.resetGyro();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	currentAvg = (currentLeft+currentRight)/2;
    	
    	currentAng = Robot.driveTrain.getDegrees();
    	currentMod = currentAng/ANG_MOD_VAL;
    	
    	currentSpeed = -1*Math.abs((currentAvg/destination)-SPEED) + SPEED;
    	
    	if(currentSpeed <= .25) currentSpeed = .25;
    	
    	SmartDashboard.putNumber("Left pos ", Robot.driveTrain.getLeftPos());
    	SmartDashboard.putNumber("Right pos ", Robot.driveTrain.getRightPos()*-1);
    	SmartDashboard.putNumber("Robot heading ", Robot.driveTrain.getDegrees());
    	
    	Robot.driveTrain.basicDrive(-1*currentSpeed-currentMod, -1*currentSpeed+currentMod);

    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        if((currentLeft+currentRight)/2 >= destination) return true;
        else return false;
    }

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

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

@@ -996,16 +996,61 @@
 L = 20;%0A
+    private final double ENC_MOD_VAL = .005;%0A
     priv
@@ -1144,16 +1144,43 @@
 entMod;%0A
+    private double encDif;%0A
     // B
@@ -2039,24 +2039,156 @@
 execute() %7B%0A
+    %09currentLeft = Robot.driveTrain.getLeftPos()*Math.PI*3.9;%0A    %09currentRight = Robot.driveTrain.getRightPos()*Math.PI*3.9;%0A    %09%0A
     %09current
@@ -2697,24 +2697,136 @@
 s());%0A    %09%0A
+    %09encDif = Robot.driveTrain.getLeftPos() - Robot.driveTrain.getRightPos();%0A    %09encDif *= ENC_MOD_VAL;%0A    %09%0A
     %09Robot.d
@@ -2849,16 +2849,17 @@
 rive(-1*
+(
 currentS
@@ -2877,13 +2877,15 @@
 tMod
+)
 , -1*
+(
 curr
@@ -2904,16 +2904,105 @@
 rentMod)
+);%0A    %09//Robot.driveTrain.basicDrive(-1*(currentSpeed-encDif), -1*(currentSpeed+encDif))
 ;%0A%0A    %7D

work on driveInches