Finished first draft of code with makeshift constants that need to be updated when...

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java
index b9285246d7e6a61682e7ce1e6957faf27305f617..4d55689148e04f6d395927aea60a525772cd4ebe 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveStraight.java
@@ -2,29 +2,67 @@ package org.usfirst.frc.team3501.robot.commands.driving;
 
 import org.usfirst.frc.team3501.robot.Robot;
 
-public class DriveStraight {
+import edu.wpi.first.wpilibj.command.Command;
 
-  double distL;
-  double distR;
-  double moveR = Robot.getDriveTrain().getLeftSpeed();
-  double moveL = Robot.getDriveTrain().getRightSpeed();
+public class DriveStraight extends Command {
 
-  public DriveStraight() {
-    distL = Robot.getDriveTrain().getLeftEncoderDistance();
-    distR = Robot.getDriveTrain().getRightEncoderDistance();
+  double speedL, speedR, tempSpeedR, seconds;
+  // double moveR = Robot.getDriveTrain().getLeftSpeed();
+  // double moveL = Robot.getDriveTrain().getRightSpeed();
+
+  public DriveStraight(double seconds) {
+    this.seconds = seconds;
+    requires(Robot.getDriveTrain());
+    speedL = Robot.getDriveTrain().getLeftSpeed();
+    speedR = Robot.getDriveTrain().getRightSpeed();
+  }
+
+  @Override
+  public void initialize() {
+    this.setTimeout(seconds);
   }
 
-  public void updateMotorSpeed() {
-    if (distL > distR) {
-      double k = distL / distR;
-      moveR *= k;
-      moveL /= k;
-    } else if (distR > distL) {
-      double k = distR / distL;
-      moveR /= k;
-      moveL *= k;
+  @Override
+  public void execute() {
+    Double mvR = null, mvL = null;
+    final double maxSpeedOneR = 16.1;
+    final double maxSpeedTwoR = 49;
+    final double maxSpeedThreeR = 75.2;
+    final double maxSpeedOneL = 13.8;
+    final double maxSpeedTwoL = 46.9;
+    final double maxSpeedThreeL = 74;
+    speedL = Robot.getDriveTrain().getLeftSpeed();
+    speedR = Robot.getDriveTrain().getRightSpeed();
+    tempSpeedR = speedR;
+    speedR = speedL;
+    speedL = tempSpeedR;
+    if (speedR <= maxSpeedOneR) {
+      mvR = speedR / maxSpeedOneR * 5;
+    } else if (speedR <= maxSpeedTwoR) {
+      mvR = speedR / maxSpeedTwoR * 2.5;
+    } else if (speedR <= maxSpeedThreeR) {
+      mvR = speedR / maxSpeedThreeR * 1.66;
+    }
+    if (speedL <= maxSpeedOneL) {
+      mvL = speedL / maxSpeedOneL * 5;
+    } else if (speedL <= maxSpeedTwoL) {
+      mvL = speedL / maxSpeedTwoL * 2.5;
+    } else if (speedL <= maxSpeedThreeL) {
+      mvL = speedL / maxSpeedThreeL * 1.66;
     }
+    if (mvR != null && mvL != null) {
+      Robot.getDriveTrain().setMotorValues(mvR, mvL); // we changed move to //
+                                                      // speed
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return this.isTimedOut();
+  }
 
-    Robot.getDriveTrain().setMotorValues(moveR, moveL);
+  @Override
+  public void end() {
+    Robot.getDriveTrain().stop();
   }
 }