DriveStraight command with encoder

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commands / driving / DriveStraight.java

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);
  }
}