package edu.stuy.commands.auton;

import edu.stuy.Robot;
import edu.wpi.first.wpilibj.command.Command;
import static edu.stuy.RobotMap.*;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
 *
 */
public class AutonDriveForwardInchesCommand extends Command {

    private double inches;
    
    
    /** 
     * If distance < 0, get it from SmartDashboard
     * @param dist
     */

    public AutonDriveForwardInchesCommand(double dist) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        inches = dist;
        requires(Robot.drivetrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        if (inches < 0) {
            inches = SmartDashboard.getNumber(INCHES_LABEL);
        }
        Robot.drivetrain.resetEncoders();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        Robot.drivetrain.tankDrive(1.0, 1.0);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return Robot.drivetrain.getDistance() >= inches;
    }

    // Called once after isFinished returns true
    protected void end() {
        Robot.drivetrain.stopDriveStraight();
    }

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

@@ -30,24 +30,31 @@
 on;%0A%0Aimport 
+static 
 edu.stuy.Rob
@@ -55,16 +55,80 @@
 uy.Robot
+Map.*;%0Aimport edu.stuy.Robot;%0Aimport edu.wpi.first.wpilibj.Timer
 ;%0Aimport
@@ -171,43 +171,8 @@
 nd;%0A
-import static edu.stuy.RobotMap.*;%0A
 impo
@@ -328,24 +328,49 @@
 inches;%0A    
+private double startTime;
 %0A    %0A    /*
@@ -762,24 +762,70 @@
 tialize() %7B%0A
+        startTime = Timer.getFPGATimestamp();%0A
         if (
@@ -1064,45 +1064,686 @@
     
-Robot.drivetrain.tankDrive(1.0, 1.0);
+double speed = getRampSpeed();%0A        Robot.drivetrain.tankDrive(speed, speed);%0A    %7D%0A    %0A    /**%0A     * Ramping algorithm for linear acceleration and smooth velocity%0A     * Takes one second to ramp up to max speed%0A     * if t %3C 0.5, then speed = 2t%5E2;%0A     * if t %3C 1, then speed = -2t%5E2 + 4t - 1;%0A     * if t %3E= 1, then speed = 1;%0A     * @return the speed the robot should go at the current time%0A     */%0A    %0A    private double getRampSpeed() %7B%0A        double t = Timer.getFPGATimestamp() - startTime;%0A        if (t %3C 0.5) %7B%0A            return 2 * t * t;%0A        %7D else if (t %3C 1) %7B%0A            return -2 * t * t + 4 * t - 1;%0A        %7D else %7B%0A            return 1;%0A        %7D
 %0A   

Add ramping logic to AutonDriveForwardInchesCommand.