clean up MoveDistance

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / commands / MoveDistance.java

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
index 6608c33904679f63abc05eb76ecb47b8861261d3..84b11c96ac5dc345e02cd6bfa74ee3c2eeee13bc 100644 (file)
--- 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();
+  }
 }