@Override
protected void execute() {
- updateMotorVals();
- updateDistances();
Robot.getDriveTrain().setMotorValues(leftMotorVal, rightMotorVal);
+ updateDistances();
if (Math.abs(leftDist - rightDist) > DISTANCE_THRESHOLD) {
+
if (leftDist > rightDist) {
rightMotorVal *= SPEED_UP_K;
leftMotorVal *= SLOW_DOWN_K;
+
} else if (rightDist > leftDist) {
rightMotorVal *= SLOW_DOWN_K;
leftMotorVal *= SPEED_UP_K;
}
}
- private void updateMotorVals() {
- rightMotorVal = Robot.getDriveTrain().getFrontRightMotorVal();
- leftMotorVal = Robot.getDriveTrain().getFrontLeftMotorVal();
- }
-
private void updateDistances() {
leftDist = Robot.getDriveTrain().getLeftEncoderDistance();
rightDist = Robot.getDriveTrain().getRightEncoderDistance();