* This commands make the robot drive for a specified time with the motors set
* at a specified value between 1 and -1
*
- * parameters:
- * time: how long the robot should drive for - in seconds
- * motorVal: the motor input to set the motors to
+ * parameters: time: how long the robot should drive for - in seconds motorVal:
+ * the motor input to set the motors to
*
*
*/
public class TimeDrive extends Command {
Timer timer;
- double motorVal, time;
+ double motorVal, motorValTwo, time;
public TimeDrive(final double time, final double motorVal) {
requires(Robot.getDriveTrain());
@Override
protected void execute() {
- Robot.getDriveTrain().setMotorValues(motorVal, motorVal);
+ double[] motorValues;
+ motorValues = Robot.getDriveTrain().correctStraightness();
+ motorVal = motorValues[0];
+ motorValTwo = motorValues[1];
+ Robot.getDriveTrain().setMotorValues(motorVal, motorValTwo);
}
setMotorValues(0, 0);
}
- public void correctStraightness() {
+ public double[] correctStraightness() {
double speedL, speedR, tempSpeedR;
speedL = Robot.getDriveTrain().getLeftSpeed();
speedR = Robot.getDriveTrain().getRightSpeed();
+ System.out.println(speedL + speedR);
// Motor values for both motors
Double mvR = null, mvL = null;
mvL = speedL / (maxSpeedThreeL * 1.66);
}
- // Setting motors at new motor values
- if (mvR != null && mvL != null) {
- Robot.getDriveTrain().setMotorValues(mvR, mvL); // we changed move to //
- } // speed
-
+ double[] motorValues = { mvL, mvR };
+ return motorValues;
}
public double getFrontLeftMotorVal() {