Menu anidado o en arbol

Hola,

Aclaro que soy nuevo en esto y solo poseo conocimientos basicos de C.
Estoy tratando de hacer un menu en al menos 2 niveles.
Opcion A > Opcion A1 > Opcion A1.1 , creo que se entiende.
Estoy utilizando switch para las opciones y un teclado matricial de 4X4.
Pero no consigue que pase a la segunda etapa del menu (SubMenu1).
Ojala alguien me pueda ayudar o indicar donde estoy equivocado.

#include <Keypad.h> //llama a la librería Keypad.h
 
const byte ROWS = 4; //Numero de filas del teclado que se esta usando
const byte COLS = 4; //Numero de columnas del teclado que se esta usando
 
char hexaKeys[ROWS][COLS] =  //Aquí pondremos la disposición de los caracteres tal cual están en nuestro teclado
{
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
 
byte rowPins[ROWS] = {9, 8, 7, 6}; //Seleccionamos los pines en el arduino donde iran conectadas las filas
byte colPins[COLS] = {5, 4, 3, 2}; //Seleccionamos los pines en el arduino donde iran conectadas las filas
 
Keypad customKeypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); //inicializa el teclado
char tecla; 
void setup()
{
  Serial.begin(9600); //inicializar puerto serie
  Serial.println ("Etapa     A");
  Serial.println ("Etapa     B");
  Serial.println ("Etapa     C");
  pinMode(13, OUTPUT); //Pin Led
}
 
void loop()
{
tecla=customKeypad.getKey();
if (tecla){
  switch(tecla){
    case 'A': 
    Serial.println ();
    Serial.println ("Etapa_A A");
    Serial.println ("Etapa_A B");
    digitalWrite (13, HIGH);
    SubMenu1();
    break;
    case 'B':
    Serial.println ();
    Serial.println ("led apagado");
    digitalWrite (13, LOW);
    break;
  }
}

}

################################################

void SubMenu1(){
  tecla=customKeypad.getKey();
if (tecla){
  switch(tecla){
    case 'A':
    Serial.println ("Primera Etapa A");
    Serial.println ("Segunda Etapa B");
    Serial.println ("Tercera Etapa C");
    Serial.println ("Cuarta  Etapa D");
    SubMenu2();
    break;
    case 'B':
    Serial.println ("Ingrese Tiempo Fijo");
    Serial.println ("Ingrese Temperatura Fijo");
    break;
}
}
}

####################################################

void SubMenu2(){
  tecla=customKeypad.getKey();
  if (tecla){
    switch (tecla){
      case 'A':
      Serial.println ("Ingrese Tiempo");
      Serial.println ("Ingrese Temperatura");
      break;
      case 'B':
      Serial.println ("Ingrese Tiempo B");
      Serial.println ("Ingrese Temperatura B");
      break;
      case 'C':
      Serial.println ("Ingrese Tiempo C");
      Serial.println ("Ingrese Temperatura C");
      break;
      case 'D':
      Serial.println ("Ingrese Tiempo D");
      Serial.println ("Ingrese Temperatura D");
      break;
    }
  }
}

Encontré este código en Internet, lo revise rápidamente y según yo es igual al mio (sin lcd y sin sensores eso si).
Cuando salga del trabajo voy a revisarlo bien y/o modificarlo para ver si me funciona.

/**************************************   www.electroingenio.com *********************************/
/**************************************************************************************************/
/**************************************Programed By Mauricio Hidalgo*******************************/
/******************************************************************************************************/
/*********************************    23 de noviembre de 2015    ************************************/

#include <LiquidCrystal.h>
#include <Keypad.h>

int i=0;
int b=0;
int sensor1 = A0;
int sensor2 = A1;
int sensor3 = A2;
int sensor4 = A3;
int pausa = 10;
int x =0;
int y =0;
int z =0;
char tecla;
int sensorValue = 0;
LiquidCrystal lcd (13,12,11,10,9,8);

//configuracion teclado matricial

const byte ROWS = 4;
const byte COLS = 4;
char hexaKeys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
byte rowPins [ROWS] = {4,5,6,7}; //Asignacion pines filas
byte colPins [COLS] = {0,1,2,3};   //Asignacion pines columnas
Keypad customKeypad = Keypad( makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); // variable_mapa de la matriz

 
void setup() {


}

void loop() {
z=0;
y=0;
tecla=customKeypad.getKey();
MSNinicio();
if(tecla)  // Check for a valid key.
  {
switch(tecla)
  {  
    case '1': // SELECCION PARA LEER SENSORE 1 Y 2 
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("MONITOR SELECTED");
    delay(1000); 
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("CHOOSE ZONE");
    lcd.setCursor (0,1);
    lcd.print("1 OR 2");
    delay(1000);
    zone();
    break;  


    case '2': // SELECCION PARA LEER SENSORES 3 Y 4
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("TEST SELECTED");
    delay(1000); 
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("CHOOSE ZONE");
    lcd.setCursor (0, 1);
    lcd.print("1 OR 2");
    delay(1000);
    zone();
    break; 
    
}
}
}

// PREGUNTAS RESET

#########################################################


/*******************************************************FUNCION ZONA****************************************************/

void zone() {
  
    while(z<1)
    {
    tecla=customKeypad.getKey(); // CHOOSE ZONE
    delay(100);
    
    if(tecla =='1'){
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("ZONE 1");
    delay(2000);
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("CHOOSE SENSOR");
    delay(100);
    lcd.setCursor (0, 1);
    lcd.print("AUDIO PRESS 1");
    lcd.setCursor (0, 2);
    lcd.print("POWER PRESS 2");
    ZoneSense1();
    }
         
    if(tecla =='2'){
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("ZONE 2");
    delay(2000);
    lcd.clear();
    lcd.setCursor (0, 0);
    lcd.print("CHOOSE SENSOR");
    delay(100);
    lcd.setCursor (0, 1);
    lcd.print("LIGHT PRESS 1");
    lcd.setCursor (0, 2);
    lcd.print("TEMPERATURE PRESS 2");
    ZoneSense2();
    }

 
   }

}

/****************************************************ZONE 1 SENSORS******************************************************************/

void ZoneSense1() {


  while(y<1)  
        {
        tecla=customKeypad.getKey(); //CHOOSE SENSOR 
          if(tecla =='1'){
          lcd.clear();
          lcd.setCursor (0, 0);
          lcd.print("AUDIO SENSOR");
          delay(3000);
          for(i=0; i < 10; i++){
            SensorAudio();
            lcd.setCursor (0, 1);
            delay(200);
            lcd.print(i);
            }
          y=1;
          z=1;
          }
   
          if(tecla =='2'){  
          lcd.clear();
          lcd.setCursor (0, 0);
          lcd.print("POWER SENSOR");
          delay(3000);
          for(i=0; i < 10; i++){
            SensorPot();
            lcd.setCursor (0, 1);
            delay(200);
            lcd.print(i);
            }
          y=1;
          z=1;
          }
        }
}

/**********************************************ZONE 2 SENSORS*************************************************************************/

void ZoneSense2() {


  while(y<1)  
        {
        tecla=customKeypad.getKey(); //CHOOSE SENSOR 
          if(tecla =='1'){
          lcd.clear();
          lcd.setCursor (0, 0);
          lcd.print("LIGHT SENSOR");
          delay(3000);
          for(i=0; i < 10; i++){
            SensorLuz();
            lcd.setCursor (0, 1);
            delay(200);
            lcd.print(i);
            }
          y=1;
          z=1;
          }
   
          if(tecla =='2'){  
          lcd.clear();
          lcd.setCursor (0, 0);
          lcd.print("TEMPERATURE SENSOR");
          delay(3000);
          for(i=0; i < 10; i++){
            SensorTemperatura();
            lcd.setCursor (0, 1);
            delay(200);
            lcd.print(i);
            }
          y=1;
          z=1;
          }
        }
}


/***********************************************************************FUNCION BIENVENIDA*********************************************************/

void MSNinicio(){

 lcd.clear(); 
 lcd.begin(20, 4);
 lcd.setCursor (0, 0);
 lcd.print("TO MONITOR SENSORS");
 lcd.setCursor (0, 1);
 lcd.print("PRESS 1 ");
 lcd.setCursor (0, 2);
 lcd.print("TO TEST SENSORS");
 lcd.setCursor (0, 3);
 lcd.print("PRESS 2 ");
 delay(500);

}

###########################################################


/*****************************************READ SENSORS FUNCTION*******************************************/

void SensorAudio(){
  
  sensorValue = analogRead(sensor1);
  lcd.setCursor (0, 0);
  lcd.print("NOISE LEVEL");
  lcd.setCursor (0, 1);
  lcd.print(sensorValue);
  delay(1000); 
  lcd.clear();
    
  }
  
  
void SensorPot(){
  
  sensorValue = analogRead(sensor2);
  lcd.setCursor (0, 0);
  lcd.print("POWER LEVEL");
  lcd.setCursor (0, 1);
  lcd.print(sensorValue);
  delay(1000); 
  lcd.clear();
  
  }
  

void SensorLuz(){
  
  sensorValue = analogRead(sensor3);
  lcd.setCursor (0, 0);
  lcd.print("LIGHT LEVEL");
  lcd.setCursor (0, 1);
  lcd.print(sensorValue);
  delay(1000); 
  lcd.clear();
  
  }
  
  
void SensorTemperatura(){
  
  sensorValue = analogRead(sensor4);
  lcd.setCursor (0, 0);
  lcd.print("TEMPERATURE LEVEL");
  lcd.setCursor (0, 1);
  lcd.print(sensorValue);
  delay(1000); 
  lcd.clear();
  
  }

Me funciono, vi algunos ejemplos en internet y pude adaptarlos.
Así quedo:

#include <Keypad.h>

int z =0;
int inicio =0;
char tecla;

const byte ROWS = 4; //Numero de filas del teclado que se esta usando
const byte COLS = 4; //Numero de columnas del teclado que se esta usando
 
char hexaKeys[ROWS][COLS] =  //Aquí pondremos la disposición de los caracteres tal cual están en nuestro teclado
{
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
 
byte rowPins[ROWS] = {9, 8, 7, 6}; //Seleccionamos los pines en el arduino donde iran conectadas las filas
byte colPins[COLS] = {5, 4, 3, 2}; //Seleccionamos los pines en el arduino donde iran conectadas las filas
 
Keypad customKeypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); //inicializa el teclado
 
void setup() {
Serial.begin(9600);
}

void loop() {
z=0;
tecla=customKeypad.getKey();
MSNinicio();
if(tecla)  // Check for a valid key.
  {

switch(tecla)
  {  
    case 'A': 
    
    Serial.println("Maceracion");
    delay(1000); 
    Serial.println("Escalonada A");
    Serial.println("Fija B");
    delay(1000);
    zone();
    break;  

    case 'B': 
    
    Serial.println("Hervido");
    delay(1000); 
    Serial.println("Ingrese Tiempo");
    Serial.println("Ingrese Temperatura");
    delay(1000);
    inicio=0;
    break;   
}
}
}


/*******************************************************FUNCION ZONA****************************************************/

void zone() {
  
    while(z<1)
    {
    tecla=customKeypad.getKey(); // CHOOSE ZONE
    delay(100);
    
    if(tecla =='A'){
    Serial.println("Primera Etapa");
    Serial.println("Ingrese Tiempo 1");
    Serial.println("Ingrese Temperatura 1");
    delay(2000);
    Serial.println("Segunda Etapa");
    Serial.println("Ingrese Tiempo 2");
    Serial.println("Ingrese Temperatura 2");
    delay(2000);
    Serial.println("Tercera Etapa");
    Serial.println("Ingrese Tiempo 3");
    Serial.println("Ingrese Temperatura 3");
    delay(2000);
    Serial.println("Cuarta Etapa");
    Serial.println("Ingrese Tiempo 4");
    Serial.println("Ingrese Temperatura 4");
    delay(2000);
    z++;
    inicio=0;
    }
         
    if(tecla =='B'){
    Serial.println("Maceracion Fija");
    Serial.println("Tiempo Fijo");
    Serial.println("Temperatura Fija");
    delay(2000);
    z++;
    inicio=0;
    }
   }
}

/*******************************************************FUNCION INICIO****************************************************/

void MSNinicio(){
if (inicio == 0){
 Serial.println("Maceracion   A");
 Serial.println("Hervido      B");
 delay(500);
 inicio ++;
}
}

Ahora seguiré agregando acciones y avanzando en el proyecto, cuando yo lo tengas mas armado lo publicare.