implement DriveStraightForTime

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraightForTime.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraightForTime.java
index c1bf1b7933d79316b205c0971ae955f0cf71225c..57f352be662282a9e96ae05386f90831aa211abc 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraightForTime.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraightForTime.java
@@ -18,8 +18,16 @@ import edu.wpi.first.wpilibj.command.Command;
  *
  */
 public class DriveStraightForTime extends Command {
+  // TODO: test for all these constants
+  final double DISTANCE_THRESHOLD = 3; // in inches
+  final double SPEED_UP_K = 1.1;
+  final double SLOW_DOWN_K = 0.9;
+  final double MOTOR_VAL_THRESHOLDS = 0.2;
   Timer timer;
-  double time, motorVal;
+  double time, origMotorVal;
+
+  double leftDist, rightDist;
+  double rightMotorVal, leftMotorVal;
 
   public DriveStraightForTime(double time, double motorVal) {
     requires(Robot.getDriveTrain());
@@ -27,7 +35,12 @@ public class DriveStraightForTime extends Command {
     timer = new Timer();
     this.time = time;
 
-    this.motorVal = motorVal;
+    this.origMotorVal = motorVal;
+    rightMotorVal = motorVal;
+    leftMotorVal = motorVal;
+
+    leftDist = Robot.getDriveTrain().getLeftEncoderDistance();
+    rightDist = Robot.getDriveTrain().getRightEncoderDistance();
 
   }
 
@@ -38,6 +51,38 @@ public class DriveStraightForTime extends Command {
 
   @Override
   protected void execute() {
+    updateMotorVals();
+    updateDistances();
+    Robot.getDriveTrain().setMotorValues(leftMotorVal, rightMotorVal);
+
+    if (Math.abs(leftDist - rightDist) > DISTANCE_THRESHOLD) {
+      if (leftDist > rightDist) {
+        rightMotorVal *= SPEED_UP_K;
+        leftMotorVal *= SLOW_DOWN_K;
+      } else if (rightDist > leftDist) {
+        rightMotorVal *= SLOW_DOWN_K;
+        leftMotorVal *= SPEED_UP_K;
+      }
+    }
+
+    if (Math.abs(averageMotorVal() - origMotorVal) > MOTOR_VAL_THRESHOLDS) {
+      Robot.getDriveTrain().setMotorValues(origMotorVal, origMotorVal);
+    }
+  }
+
+  private void updateMotorVals() {
+    rightMotorVal = Robot.getDriveTrain().getFrontRightMotorVal();
+    leftMotorVal = Robot.getDriveTrain().getFrontLeftMotorVal();
+  }
+
+  private void updateDistances() {
+    leftDist = Robot.getDriveTrain().getLeftEncoderDistance();
+    rightDist = Robot.getDriveTrain().getRightEncoderDistance();
+
+  }
+
+  private double averageMotorVal() {
+    return (rightMotorVal + leftMotorVal) / 2;
   }
 
   @Override