public void driveWithCorrection(double speed, int ThreshholdDistance) {
    if (stateManager.getStateStage() == 0) {
      if (sensorManager.getColorSensorHolder().getColorSensor().blue()
          > sensorManager.getColorSensorHolder().getColorSensor().red()) {
        servoManager.getBeaconServo().setPosition(0); // Update this value
      } else {
        servoManager.getBeaconServo().setPosition(0); // Update this value
      }

      stateManager.continueCommand();
    } else if (stateManager.getStateStage() == 1) {
      motorManager.getRightMotor2().setMode(DcMotorController.RunMode.RESET_ENCODERS);
      motorManager.getLeftMotor2().setMode(DcMotorController.RunMode.RESET_ENCODERS);

      motorManager.getLeftMotor2().setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
      motorManager.getRightMotor2().setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);

      if (speed > 0) {
        motorManager.getLeftMotor1().setPower(speed);
        motorManager.getRightMotor1().setPower(-speed);
        motorManager.getLeftMotor2().setPower(speed);
        motorManager.getRightMotor2().setPower(-speed);
      } else if (speed < 0) {
        motorManager.getLeftMotor1().setPower(-speed);
        motorManager.getRightMotor1().setPower(speed);
        motorManager.getLeftMotor2().setPower(-speed);
        motorManager.getRightMotor2().setPower(speed);
      }

      stateManager.continueCommand();
    } else if (stateManager.getStateStage() == 2) {
      if (ThreshholdDistance >= sensorManager.getUltrasonicSensorHolder().getLevel()
          && sensorManager.getUltrasonicSensorHolder().getLevel() != 0) {
        motorManager.getLeftMotor1().setPower(0);
        motorManager.getRightMotor1().setPower(0);
        motorManager.getLeftMotor2().setPower(0);
        motorManager.getRightMotor2().setPower(0);

        stateManager.continueProgram();
      } else {
        leftCorrecion =
            speed
                - (sensorManager.getGyroscopeHolder().getRotation()
                    * presets.getGYRO_ERROR_CORRECTION());
        rightCorrection =
            speed
                + (sensorManager.getGyroscopeHolder().getRotation()
                    * presets.getGYRO_ERROR_CORRECTION());

        if (leftCorrecion > 1) {
          leftCorrecion = 1;
        } else if (leftCorrecion < -1) {
          leftCorrecion = -1;
        }

        if (rightCorrection > 1) {
          rightCorrection = 1;
        } else if (rightCorrection < -1) {
          rightCorrection = -1;
        }

        motorManager.getLeftMotor1().setPower(leftCorrecion);
        motorManager.getRightMotor1().setPower(rightCorrection);
        motorManager.getLeftMotor2().setPower(leftCorrecion);
        motorManager.getRightMotor2().setPower(rightCorrection);
      }
    }
  }
 private double calcEncoderValue(double inches) {
   return (1440 * (inches / (Math.PI * presets.getWHEEL_DIAMETER()))) * presets.getGEAR_RATIO();
 } // calcEncoderValue