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

[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;

import edu.wpi.first.wpilibj.command.Command;

public class DriveStraight extends Command {

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

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

  @Override
  public void end() {
    Robot.getDriveTrain().stop();
  }
}