From ba4d69ce1108cae1741a75ad77ad0f0518c27a82 Mon Sep 17 00:00:00 2001
From: Kevin Zhang <kevin.zhang.13499@gmail.com>
Date: Sat, 14 Nov 2015 16:05:29 -0800
Subject: [PATCH] finish MoveDistance command with acceleration function

---
 .../RiceCatRobot/commands/MoveDistance.java   | 33 +++++++++++--------
 .../RiceCatRobot/subsystems/DriveTrain.java   |  8 +++++
 2 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
index 88827f9..2243bc7 100644
--- a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
@@ -1,43 +1,48 @@
 package org.usfirst.frc3501.RiceCatRobot.commands;
 
 import org.usfirst.frc3501.RiceCatRobot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
 
 import edu.wpi.first.wpilibj.command.Command;
 
 public class MoveDistance extends Command {
 
-	double distance;
-	
-	public MoveDistance(double distance){
+	double distance, minSpeed, maxSpeed;
+
+	public MoveDistance(double distance, double minSpeed, double maxSpeed) {
 		requires(Robot.driveTrain);
 		this.distance = distance;
+		this.minSpeed = minSpeed;
+		this.maxSpeed = maxSpeed;
 	}
-	
-	protected void initialize(){
-		
+
+	protected void initialize() {
+		Robot.driveTrain.resetEncoders();
 	}
 
 	@Override
 	protected void execute() {
-		// TODO Auto-generated method stub
-		
+		double speed = 4 * (minSpeed - maxSpeed) * 
+				Math.pow((Robot.driveTrain.getAverageSpeed() / distance - 0.5), 2) 
+				+ maxSpeed;
+		Robot.driveTrain.setMotorSpeeds(speed, speed);
 	}
 
 	@Override
 	protected boolean isFinished() {
-		// TODO Auto-generated method stub
+		if (Robot.driveTrain.getLeftDistance() > distance
+				&& Robot.driveTrain.getRightDistance() > distance)
+			return true;
 		return false;
 	}
-	
+
 	@Override
 	protected void end() {
-		// TODO Auto-generated method stub
-		
+		Robot.driveTrain.stop();
 	}
 
 	@Override
 	protected void interrupted() {
-		// TODO Auto-generated method stub
-		
+		end();
 	}
 }
diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
index 1dfe9d8..a8716b6 100644
--- a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java
@@ -37,6 +37,10 @@ public class DriveTrain extends Subsystem {
     public double getLeftSpeed() {
         return leftEncoder.getRate();
     }
+    
+    public double getAverageSpeed() {
+    	return (getRightSpeed() + getLeftSpeed())/2;
+    }
 
     public double getRightDistance() {
         // Returns distance in inches
@@ -48,6 +52,10 @@ public class DriveTrain extends Subsystem {
         return leftEncoder.getDistance();
     }
 
+    public void stop() {
+    	setMotorSpeeds(0, 0);
+    }
+    
     public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
         if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
             leftSpeed = 0;
-- 
2.30.2