File content as of revision 0:84f05cd2f197:

#include "Debug.h"
#include "UI.h"
#include "pinconfig.h"
#include "BEAR_Protocol.h"

DigitalIn button(USER_BUTTON);
Serial pc(SERIAL_TX,SERIAL_RX);

void DebugMode();

int main()
{
    if(!button) {
        while(!button);
        DebugMode(); //-->Debug.h
    }
    
    UI ui(SW_WALK1,SW_WALK2,SW_WALK3,SW_SWEEP,SW_WATER,EMERGENCY);
    
    pc.printf("System Start\n");
    while(1)
    {
        while(!ui.getEmergencyStatus()) {
            ui.RunSystem();
        }
    }
}

void DebugMode()
{
    float temp;
    int option;
    bool first_time = true;
    Debug debug(SERIAL_TX,SERIAL_RX);
    do {
        debug.PrintListMode();
        option = debug.ScanInputData(1);

        if(option == 1) {
            do {
                temp = debug.Mode1();
                debug.PrintAll(temp);
            } while(temp!=9999);
        }

        else if(option == 2) {
            do {
                if(first_time==false) {
                    temp = debug.Mode2();
                    debug.PrintAll(temp);
                } else {
                    temp = debug.Mode2();
                    if(temp!=0) first_time = false;
                }

                if(temp==9999) first_time = true;
            } while(temp!=9999);
        }


        debug.setChange();
    } while(option!=9999);
}