From aa9c330eeadb1ab6b945c8d14b9a920ae7732cf7 Mon Sep 17 00:00:00 2001
From: David <david.dobervich@gmail.com>
Date: Fri, 20 Nov 2015 10:31:14 -0800
Subject: [PATCH] clean up MoveDistance

---
 .../RiceCatRobot/commands/MoveDistance.java   | 79 ++++++++++---------
 1 file changed, 40 insertions(+), 39 deletions(-)

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
index 6608c33..84b11c9 100644
--- a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
@@ -6,43 +6,44 @@ import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
 import edu.wpi.first.wpilibj.command.Command;
 
 public class MoveDistance extends Command {
-
-	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() {
-		Robot.driveTrain.resetEncoders();
-	}
-
-	@Override
-	protected void execute() {
-		double speed = 4 * (minSpeed - maxSpeed) * 
-				Math.pow((Robot.driveTrain.getAverageSpeed() / distance - 0.5), 2) 
-				+ maxSpeed;
-		Robot.driveTrain.setMotorSpeeds(speed, speed);
-	}
-
-	@Override
-	protected boolean isFinished() {
-		if (Robot.driveTrain.getLeftDistance() > distance
-				&& Robot.driveTrain.getRightDistance() > distance)
-			return true;
-		return false;
-	}
-
-	@Override
-	protected void end() {
-		Robot.driveTrain.stop();
-	}
-
-	@Override
-	protected void interrupted() {
-		end();
-	}
+  private double distance; // in units of TODO
+  private double minSpeed; // in range (0, 1]
+  private double maxSpeed; // in range (0, 1]
+
+  public MoveDistance(double distance, double minSpeed, double maxSpeed) {
+    requires(Robot.driveTrain);
+    this.distance = distance;
+    this.minSpeed = Math.min(minSpeed, maxSpeed);
+    this.maxSpeed = Math.max(minSpeed, maxSpeed);
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.driveTrain.resetEncoders();
+  }
+
+  @Override
+  protected void execute() {
+    // TODO: create a math helper library and move this function there
+    double speed = 4 * (minSpeed - maxSpeed)
+        * Math.pow((Robot.driveTrain.getAverageSpeed() / distance - 0.5), 2)
+        + maxSpeed;
+    Robot.driveTrain.setMotorSpeeds(speed, speed);
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return Robot.driveTrain.getLeftDistance() > distance
+        && Robot.driveTrain.getRightDistance() > distance;
+  }
+
+  @Override
+  protected void end() {
+    Robot.driveTrain.stop();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
 }
-- 
2.30.2