Ajuda com código menu lcd 4 botões

#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

#define Pin 3
unsigned long Tempo1 = 6000;
unsigned long Tempo2 = 12000;
unsigned long Tempo3 = 19000;
unsigned long Tempo4 = 26000;
unsigned long Tempo5 = 33000;
unsigned long Tempo6 = 40000;

void setup() {
Serial.begin(9600);
Serial.print("TESTE EM ANDAMENTO");
pinMode(Pin, OUTPUT);
lcd.begin(16, 2);
}

void loop() {
lcd.clear();               
lcd.print("300 RPM"); 
             
while(millis() < Tempo1){

digitalWrite(Pin, 1);
delay(600);
digitalWrite(Pin, 0);
delay(600);
}
lcd.clear();               
lcd.print("500 RPM");  
 
while(millis() < Tempo2){

digitalWrite(Pin, 1);
delay(350);
digitalWrite(Pin, 0);
delay(350);
}
lcd.clear();               
lcd.print("800 RPM");

while(millis() < Tempo3){

digitalWrite(Pin, 1);
delay(76);
digitalWrite(Pin, 0);
delay(61);
}
lcd.clear();               
lcd.print("1500 RPM");

while(millis() < Tempo4){

digitalWrite(Pin, 1);
delay(40);
digitalWrite(Pin, 0);
delay(32);
}
lcd.clear();               
lcd.print("3600 RPM");

while(millis() < Tempo5){

digitalWrite(Pin, 1);
delay(17);
digitalWrite(Pin, 0);
delay(14);
}

lcd.clear();               
lcd.print("5000 RPM");
while(millis() < Tempo6){

digitalWrite(Pin, 1);
delay(11);
digitalWrite(Pin, 0);
delay(9);

}
}

Olá a todos,

Sou novo em programação e consegui programar (com muito custo) esse código.
Gostaria de incluir um menu com 4 opções de seleção e confirmação, acompanhado de 4 botões (direita, esquerda, enter e parada de qualquer execução) mais estou quebrando a cabeça para incluir que esse código (e mais outro dois que estou fazendo) só venha a rodar caso eu selecione ele no menu.

Obrigado a todos!

#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
//I2C pins declaration
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);


// =============================================================================================================
// --- Mapeamento de Hardware --- 
#define   bt_r   14   //botão direita
#define   bt_l   15   //botão esquerda
#define   bt_e   16   //botão enter
#define   bt_b   17   //botão voltar

#define pinBico 9 //porta bico
#define pinRele 5 //porta rele

unsigned long Tempo1 = 7000;
unsigned long Tempo2 = 12000;
unsigned long Tempo3 = 19000;
unsigned long Tempo4 = 26000;
unsigned long Tempo5 = 33000;
unsigned long Tempo6 = 40000;






// =============================================================================================================
// --- Constantes e Objetos --- 
#define   menu_max   4                  //número máximo de menus existentes
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;


// =============================================================================================================
// --- Protótipo das Funções ---
void keyboard();
void menu1();
void menu2();
void menu3();
void menu4();

void estanqueidade();
void teste_leque();
void equalizer();
void ultrassom();
unsigned long Tempol = 15000;
unsigned long Tempoest = 20000;
unsigned long Tempoultrassom = 500000;



// =============================================================================================================
// --- Variáveis Globais --- 
int menu_num = 1, sub_menu = 1;


// =============================================================================================================
// --- Configurações Iniciais ---
void setup(){
  
  pinMode(bt_r, INPUT_PULLUP);
  pinMode(bt_l, INPUT_PULLUP);
  pinMode(bt_e, INPUT_PULLUP);
  pinMode(bt_b, INPUT_PULLUP);
  pinMode(pinBico, OUTPUT);
  pinMode(pinRele, OUTPUT);
  lcd.begin (16,2);
  lcd.backlight();
 

} //end setup


// =============================================================================================================
// --- Configurações Iniciais ---
void loop() 
{

  keyboard();
  
  switch(menu_num)
  {
    case 1: menu1(); break;
    case 2: menu2(); break;
    case 3: menu3(); break;
    case 4: menu4(); break;
    
    
  } //end switch

} //end loop


// =============================================================================================================
// --- Desenvolvimento das Funções ---
void keyboard()
{
   if(!digitalRead(bt_r) && sub_menu == 1)
   {
      delay(200);
      if(menu_num <= menu_max) menu_num += 1;
    
   } //end bt_r

   if(!digitalRead(bt_l) && sub_menu == 1)
   {
      delay(200);
      if(menu_num > 0) menu_num -= 1;
    
   } //end bt_l

   if(!digitalRead(bt_e))
   {
      delay(200);
      if(sub_menu <= 2) sub_menu += 1;
    
   } //end bt_e

   if(!digitalRead(bt_b))
   {
      delay(200);
      if(sub_menu > 0) sub_menu -= 1;
    
   } //end bt_b
   
  
} //end keyboard


void menu1()
{
  switch(sub_menu)
  {
     case 1:
       lcd.setCursor(0,0);
       lcd.print("ESTANQUEIDADE  >");
       lcd.setCursor(0,1);
       lcd.print("                ");
       break;
     case 2:
       lcd.setCursor(0,0);
       lcd.print("                ");
       estanqueidade();
       break;
             
       
 }
  
  
} //end menu1


void menu2()
{
  switch(sub_menu)
  {
     case 1:
       lcd.setCursor(0,0);
       lcd.print("< TESTE LEQUE  >");
       lcd.setCursor(0,1);
       lcd.print("                ");
       break;
     case 2:
      //lcd.setCursor(0,0);
       //lcd.print("                ");
       teste_leque();
       break;
       
  }
  
  
} //end menu2


void menu3()
{
  switch(sub_menu)
  {
     case 1:
       lcd.setCursor(0,0);
       lcd.print("< TESTE EQUAL. >");
       lcd.setCursor(0,1);
       lcd.print("                ");
       break;
     case 2:
       //lcd.setCursor(0,0);
       //lcd.print("                ");
       //lcd.setCursor(0,1);
       //lcd.print("    100 Ohms    ");
       equalizer();
       break;
                    
  }
  
  
} //end menu3


void menu4()
{
  switch(sub_menu)
  {
     case 1:
       lcd.setCursor(0,0);
       lcd.print("<  ULTRASSOM   ");
       lcd.setCursor(0,1);
       lcd.print("                ");
       break;
     case 2:
       lcd.setCursor(0,0);
       lcd.print("                ");
       ultrassom();
       break;
  }

  
  } //end menu4

  void equalizer()
  {
   lcd.clear();               
lcd.print("300 RPM"); 
             
while(millis() < Tempo1){

digitalWrite(pinBico, 1);
delay(600);
digitalWrite(pinBico, 0);
delay(600);
}
lcd.clear();               
lcd.print("500 RPM");  
 
while(millis() < Tempo2){

digitalWrite(pinBico, 1);
delay(350);
digitalWrite(pinBico, 0);
delay(350);
}
lcd.clear();               
lcd.print("800 RPM");

while(millis() < Tempo3){

digitalWrite(pinBico, 1);
delay(76);
digitalWrite(pinBico, 0);
delay(61);
}
lcd.clear();               
lcd.print("1500 RPM");

while(millis() < Tempo4){

digitalWrite(pinBico, 1);
delay(40);
digitalWrite(pinBico, 0);
delay(32);
}
lcd.clear();               
lcd.print("3600 RPM");

while(millis() < Tempo5){

digitalWrite(pinBico, 1);
delay(17);
digitalWrite(pinBico, 0);
delay(14);
}

lcd.clear();               
lcd.print("5000 RPM");
while(millis() < Tempo6){

digitalWrite(pinBico, 1);
delay(11);
digitalWrite(pinBico, 0);
delay(9);    

}
}

void estanqueidade()
 {
   lcd.clear();               
lcd.print("FECHADO"); 

while(millis() < Tempoest){
digitalWrite(pinRele, LOW);             

}
}

void teste_leque()
  {
   lcd.clear();               
lcd.print("ABERTO"); 
             
while(millis() < Tempol){

digitalWrite(pinBico, 1);
delay(2000);
digitalWrite(pinBico,0);
delay(1000);


}
}

  void ultrassom()
  {
   lcd.clear();               
lcd.print("3000 RPM"); 
             
while(millis() < Tempoultrassom){

digitalWrite(pinBico, 1);
delay(25);
digitalWrite(pinBico, 0);
delay(20);
}
}

Consegui avançar um pouco, agreguei um menu ao meu projeto mais não funciona a contento.
Agradeço a quem revisar e dar algumas dicas.

obrigado!