--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+public class DriveStraight {
+
+ double distL;
+ double distR;
+ double moveR = Robot.getDriveTrain().getLeftSpeed();
+ double moveL = Robot.getDriveTrain().getRightSpeed();
+
+ public DriveStraight() {
+ distL = Robot.getDriveTrain().getLeftEncoderDistance();
+ distR = Robot.getDriveTrain().getRightEncoderDistance();
+ }
+
+ 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;
+ }
+
+ Robot.getDriveTrain().setMotorValues(moveR, moveL);
+ }
+}