Hacer menú en lcd

Hola.

Tengo un proyecto entre manos en el que tengo que hacer mostrar un menú en un lcd y los botones de control son un keypad. He hecho el código siguiente pero no funciona. Alguien me puede hechar una mano.

#include <Keypad.h>
#include <LiquidCrystal_I2C.h>

const byte rowsCount = 4;
const byte columsCount = 4;
char keys[rowsCount][columsCount] = {
   { '1','2','3', 'A' },
   { '4','5','6', 'B' },
   { '7','8','9', 'C' },
   { '*','0','#', 'D' }
};
LiquidCrystal_I2C lcd(0x27,16,2);

const byte rowPins[rowsCount] = { 9, 8, 7, 6 };
const byte columnPins[columsCount] = { 5, 4, 3, 2 };
Keypad keypad = Keypad(makeKeymap(keys), rowPins, columnPins, rowsCount, columsCount);

void Init_menu(){
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("1 Config 2 Clock");
  lcd.setCursor(0,1);
  lcd.print("3 valule");
  
}
void setup() {
  lcd.init();
  time = millis();

}

void loop() {
  
  char key = keypad.getKey();
  if (key){
    Init_menu();
    key = keypad.getKey();
    switch (key){
      case '1':
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("Press A, B, C");
        lcd.setCursor(0,1);
        lcd.print("or D. * return");
        break;
      case '2':
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("Reloj");
        break;
      case '3':
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("Press A, B, C");
        lcd.setCursor(0,1);
        lcd.print("or D.");
        break;
      case'*':
        Init_menu();
        break;  
      default: Init_menu(); break;
    }
    }
}

Saludos

Hola.

Ya lo he solucionado. Le he puesto una función de la librería del teclado para que espere a que se presione un tecla.

#include <Keypad.h>
#include <LiquidCrystal_I2C.h>

const byte rowsCount = 4;
const byte columsCount = 4;
char keys[rowsCount][columsCount] = {
   { '1','2','3', 'A' },
   { '4','5','6', 'B' },
   { '7','8','9', 'C' },
   { '*','0','#', 'D' }
};
LiquidCrystal_I2C lcd(0x27,16,2);

const byte rowPins[rowsCount] = { 9, 8, 7, 6 };
const byte columnPins[columsCount] = { 5, 4, 3, 2 };
Keypad keypad = Keypad(makeKeymap(keys), rowPins, columnPins, rowsCount, columsCount);
unsigned long time;

void Init_menu(){
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("1 Config 2 Clock");
  lcd.setCursor(0,1);
  lcd.print("3 Set valvule");
  
}
void setup() {
  lcd.init();
  time = millis();

}

void loop() {
  
  char key = keypad.getKey();
  if (key){
    Init_menu();
    
    key = keypad.waitForKey();
    switch (key){
      case '1':
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("Press A, B, C");
        lcd.setCursor(0,1);
        lcd.print("or D");
          key = keypad.waitForKey();
          switch (key){
            case 'A':
              lcd.clear();
              lcd.setCursor(4,0);
              lcd.print("Valvules");
              lcd.setCursor(0,1);
              //lcd.print("or D");
              break;
            case '*':
              Init_menu();
              break;
          }
        break;
      case '2':
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("Reloj");
        break;
      case '3':
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("Valvule");
        lcd.setCursor(0,1);
        
        break;
      case'*':
        Init_menu();
        break;  
      default: break;
    }
    }
}

Saludos