Otras cositas que note:
Deberias esperar hasta que se pulse alguna opcion y no los 3 segundos, porque si me demore tengo que volver a empezar, y lo mismo con los datos del teclado matricial.
Saludos!!!
while(input(pin_C0)==1 || input(pin_C1)==1)
{
.....}