File content as of revision 0:18f9b716c753:

#// NOTES:
// 1. mbed.h MUST BE the revision from 30 Mar 2016, otherwise it will behave strange and unexpected! Older or newer versions won't work.
//    For example, with the latest version of mbed.h from 25 May 2016, the "wait" function does not work at all, it's the same as if it's not there.
#include "mbed.h"
#include "TextLCD.h"

////////////////////////////////
// instantiations
////////////////////////////////
Serial pc(SERIAL_TX, SERIAL_RX);
AnalogIn button(A0); // Init button (SELECT, LEFT, UP, DOWN, RIGHT)
TextLCD lcd(D8, D9, D4, D5, D6, D7);
PwmOut backlight(D10); // Backlight LCD
PwmOut blueLed(D15);
Timer t;
DigitalOut led(LED1);
DigitalOut load(D3);
DigitalIn measure(D2);

////////////////////////////////
// variables and constants
////////////////////////////////

// keyboard keys
#define COMMAND_NONE 0
#define COMMAND_UP 1
#define COMMAND_DOWN 2
#define COMMAND_LEFT 3
#define COMMAND_RIGHT 4
#define COMMAND_SELECT 5

// config menu options
#define MENU_NONE 0
#define MENU_LED_STATE 1
#define MENU_LED_VALUE 2

int menuOptions[2] = {
    MENU_LED_STATE,
    MENU_LED_VALUE
};

// config
struct config_t {
    bool ledLit;
    bool ledChanged;
    float ledValue;

    config_t():ledLit(true), ledChanged(true), ledValue(0.5){}

} config;

// variables
int meas;
int check=0;
int millard=0;
long counter=0;
int counterfailure=0;
int command = COMMAND_NONE;
int lastCommand = COMMAND_NONE;
bool isInConfig = false;

int currentOption = MENU_NONE;
int lastOption = currentOption;

int lastDebounceTime; // the last time the output pin was toggled
int debounceDelay = 100; // the debounce time; increase if the output flickers
int debounceReading = COMMAND_NONE; // last read command from keyboard
int debounceCommand = COMMAND_NONE; // current command
int debounceLastCommand = COMMAND_NONE; // last command

void printFirstScreen()
{
    lcd.cls(); // Clear LCD
    lcd.locate(0, 0); // Set the pointer to the first column, first row
    lcd.printf("Press SELECT to enter setup"); // first time initialization
}

void initialize()
{
    // Start serial USB
    pc.baud(9600);

    // Set the LCD backlight
    backlight.period(0.002);
    backlight = 0.7;

    // Print on LCD
    printFirstScreen();

     // Set the LED backlight
    blueLed.period(0.001); // blueLed.period: 0.01 = 100Hz; 0.02 = 50Hz; 0.001 = 1kHz; 0.0001 = 10kHz;

    // Start the timer
    t.start();

    // Serial debug
    pc.printf("Initialization complete\n");
}

// the debounce was taken from https://www.arduino.cc/en/Tutorial/Debounce
void readKeyboard()
{
    meas = button.read() * 1000; // Read the analog input value (value from 0.0 to 1.0) and convert to int value (from 0 to 1000)

    // read the buttons
    // right < 40
    // up: 130 - 170
    // down: 360 - 400
    // left: 580 - 620
    // select: 920 - 960
    // no button pressed: 1000

    // set a temporary command for the button that has been pushed
    if (meas < 40) { // right button
        debounceReading = COMMAND_RIGHT;
    }
    else if (meas > 130 && meas < 170) { // up button
        debounceReading = COMMAND_UP;
    }
    else if (meas > 360 && meas < 400) { // down button
        debounceReading = COMMAND_DOWN;
    }
    else if (meas > 580 && meas < 620) { // left button
        debounceReading = COMMAND_LEFT;
    }
    else if (meas > 920 && meas < 960) { // select button
        debounceReading = COMMAND_SELECT;
    }
    else {
        debounceReading = COMMAND_NONE;
    }

    int milliseconds = t.read_ms();
    int lastPushedTime = milliseconds - lastDebounceTime;

    // a button has been pushed or released
    if (debounceReading != debounceLastCommand) {
        // reset the debouncing timer
        lastDebounceTime = t.read_ms();
    }

    if (lastPushedTime > debounceDelay && debounceReading != debounceCommand) {
        debounceCommand = debounceReading;
        command = debounceCommand;
    }

    debounceLastCommand = debounceReading;

    // reset the timer if keyboard is idle for a specific amount of time
    if (debounceReading == COMMAND_NONE && lastPushedTime > 1200e3) { // 1200e3 = 20min
        t.reset();
    }
}

/*
void printKeyboardCommandOnLCD()
{
    if (command != lastCommand)
    {
        if (command != COMMAND_NONE) {
            lcd.cls();
            lcd.locate(0, 0);
        }

        if (command == COMMAND_RIGHT) {
            lcd.printf("BUTTON: Right");
        }
        else if (command == COMMAND_UP) {
            lcd.printf("BUTTON: Up");
        }
        else if (command == COMMAND_DOWN) {
            lcd.printf("BUTTON: Down");
        }
        else if (command == COMMAND_LEFT) {
            lcd.printf("BUTTON: Left");
        }
        else if (command == COMMAND_SELECT) {
            lcd.printf("BUTTON: Select");
        }
    }
}
*/

void resetKeyboardCommand()
{
    lastCommand = command;
}

void resetMenuOption()
{
    lastOption = currentOption;
}

void clearLCDSecondRow()
{
    lcd.locate(0, 1);
    lcd.printf("                ");
    //lcd.printf("%*.0i", 16, 0);
}

void printLedStateConfig()
{
    lcd.locate(0, 0);
    lcd.printf("1 Million test");
    lcd.locate(0, 1);
    lcd.printf("Press UP/DOWN"); 
    
}

void printLedValueConfig()
{
    lcd.locate(0, 0);
    lcd.printf("5 Million test");
    lcd.locate(0, 1);
    lcd.printf("Press UP/DOWN"); 
}

void program1()
{
start:        
    resetKeyboardCommand();
    resetMenuOption();
    while(millard<1) 
    {
        load=1; //change status
        led=0;
        wait_ms(100);
        check=DigitalIn(measure);
        if ((check==1)||(counterfailure>3))
        {
            wait_ms(100);
            
                        
        }
        if ((check==0)&& (counterfailure<4))
        {
            lcd.cls();
            lcd.locate(0, 0);
            lcd.printf(" FAILURE CLOSING \n");
            lcd.locate(0, 1);
            lcd.printf("Cycles:%d\n\r", counter); //Contact didn't close. NC contact 
            goto failure;
            
            
            
        }
        load=0;
        wait_ms(100);
        check=DigitalIn(measure);
        if ((check==0)||(counterfailure>3))
        {
            wait_ms(100);
            
        }
        if ((check==1)&&(counterfailure<4))
        {
            lcd.cls();
            lcd.locate(0, 0);
            lcd.printf("FAILURE OPENING \n");
            lcd.locate(0, 1);
            lcd.printf("Cycles:%d\n\r", counter);
            goto failure;
        }
        counter++;
        if (counter>999999)
        {
            millard=millard+1;
            counter=0;
        }
        led=1;
        lcd.cls();
        lcd.locate(0, 0);
        lcd.printf("Cycles:%d\n\r", counter);
        lcd.locate(0, 1);
        lcd.printf("Million:%d\n\r", millard);
        

    }
    
failure:      
    counterfailure++;

        while(counterfailure<4)
        {  
            readKeyboard();
            if (command != COMMAND_NONE)
            {
                goto start; 
            }
        }
        if (counterfailure>3)
        {
            goto start;
        }
}  

void program2()
{   
    millard=1;
    resetKeyboardCommand();
    resetMenuOption();
    while(millard<5) 
    {
        load=1; //change status
        led=0;
        wait_ms(200);
        load=0;
        wait_ms(200);
        counter++;
        if (counter>999999)
        {
            millard=millard+1;
            counter=0;
        }
        lcd.cls();
        lcd.locate(0, 0);
        lcd.printf("Cycles:%d\n\r", counter);
        lcd.locate(0, 1);
        lcd.printf("Million:%d\n\r", millard);    

    }
}
    

// handle keyboard input
void handleConfigMenu()
{
    // a new command has been triggered
    if (command != lastCommand)
    {
        if (isInConfig)
        {
             // exit the config menu
            if (command == COMMAND_SELECT)
            {
                printFirstScreen();
                isInConfig = false;

                // revert/reset everything to the first state
                currentOption = MENU_NONE;
                resetMenuOption();
                command = COMMAND_NONE;
                resetKeyboardCommand();
            }
            // scroll through menu options
            else if (command == COMMAND_LEFT || command == COMMAND_RIGHT)
            {
                // search for current element position
                int menuOptionsSize = sizeof(menuOptions) / sizeof(int);
                int pos = 0;
                while (pos < menuOptionsSize && currentOption != menuOptions[pos]) {
                    pos++;
                }

                if (command == COMMAND_LEFT)
                {
                    if (pos == 0) {
                        currentOption = menuOptions[menuOptionsSize - 1];
                    } else {
                        currentOption = menuOptions[--pos];
                    }
                }
                else if (command == COMMAND_RIGHT)
                {
                    if (pos == (menuOptionsSize - 1)) {
                        currentOption = menuOptions[0];
                    } else {
                        currentOption = menuOptions[++pos];
                    }
                }
            }
            // scroll through menu settings
            else if (command == COMMAND_UP || command == COMMAND_DOWN)
            {
                // the LED state has changed
                if (currentOption == MENU_LED_STATE) {
                    config.ledLit = !config.ledLit;
                    config.ledChanged = true;

                    program1();
                }
                // the LED value has changed
                else if (currentOption == MENU_LED_VALUE) {
                    if (command == COMMAND_UP || command == COMMAND_DOWN) 
                    {
                        program2();
        
                    } 

                }
            }
        }
        else {
             // enter in config menu
            if (command == COMMAND_SELECT)
            {
                lcd.cls();
                lcd.locate(0, 0);
                lcd.printf("MENU CONFIG");

                isInConfig = true;
                currentOption = MENU_LED_STATE;
            }
        }

        // a new command has been triggered
        // browse through options
        if (currentOption != lastOption)
        {
            // clear second row
            clearLCDSecondRow();

            // LED state
            if (currentOption == MENU_LED_STATE)
            {
                printLedStateConfig();
            }
            // LED value
            else if (currentOption == MENU_LED_VALUE)
            {
                printLedValueConfig();
            }
        }
    }
}


int main()
{
    initialize();

    while(1)
    {
        // keyboard routines
        readKeyboard();
        //printKeyboardCommandOnLCD();

        // config menu routines
        handleConfigMenu();


        // reset everything
        resetKeyboardCommand();
        resetMenuOption();
    }
}