File content as of revision 6:18a4dd77057e:

#include "mbed.h"
#include "ultrasonic.h"
#include "N5110.h"
#include "Joystick.h"
#include "main.h"


int main()
{

    R.mode(PullDown);
    L.mode(PullDown);
    Start.mode(PullDown);
    Start.rise(Start_isr);
    Back.mode(PullDown);
    Back.rise(Back_isr);
    mu.startUpdates();//start mesuring the distance
    
    joystick.init();
    init_display();
    main_menu();
    
}

void init_display(){
        
    lcd.init();
    lcd.setContrast(0.4);
    lcd.clear(); /// clear screen (init)  
        
    }
        
void dist(int distance){

    printf("Safe Distance %dmm\r\n", distance);
    }
    
void main_menu(){
    
    lcd.clear();
    Start_flag = 0;
    int select = 0;
    output = 63; //set all LEDs to off

    while (1) {
        
    Direction d = joystick.get_direction();   
    printf("Direction = %i\n",d);
    
        switch(select) {
            case 0:
                switch(d) {
                    case 1:
                        select = 1;
                    printf("UP");
                        break;
                    case 5:
                        select = 1;
                    printf("Down");
                        break;
}
                break;
            case 1:
                switch(d) {
                    case 1:
                        select = 0;
                    printf("UP");
                        break;
                    case 5:
                        select = 0;
                    printf("Down");
                        break;
}
                break;
 
        }
                wait(0.1);
        if (select == 0) {
            lcd.clear();
            lcd.printString("Detect",27,0);
            lcd.printString("Sense Object",7,2);
            lcd.printString("Calibration",7,3);
            lcd.drawCircle(3,19,2,FILL_TRANSPARENT);
            lcd.refresh();
            wait(0.15);
            if (Start_flag == 1) {
                Start_flag = 0;
                sense_object();
            }
        } else if (select == 1) {
            lcd.clear();
            lcd.printString("Detect",27,0);
            lcd.printString("Sense Object", 7,2);
            lcd.printString("Calibration",7,3);
            lcd.drawCircle(3,27,2,FILL_TRANSPARENT);
            lcd.refresh();
            wait(0.15);
            if (Start_flag == 1) {
                Start_flag = 0;
                calibrate_object();
            }
        }
    }
}

void sense_object(){
        
        
    // set inital state 
    int state = 0;
    
    while(1)
    {
    output = fsm[state];  // output current state
        // check which state we are in and see which the next state should be
    switch(state) {
        case 0:
        lcd.clear();
        lcd.printString(" object at 0'",0,0);
        lcd.printString("   R + 60'",0,2);
        lcd.printString("   L - 60'",0,4);
        lcd.refresh();
            if (R == 1){
                state = 1;}
            else if (L == 1){
                state = 5;}
            else state = 0;
                break;
        case 1:
        lcd.clear();
        lcd.printString(" object at 60'",0,0);
        lcd.printString("   R + 60'",0,2);
        lcd.printString("   L - 60'",0,4);
        lcd.refresh();
            if (R == 1){
                state = 2;}
            else if (L == 1){
                state = 0;}
            else state = 1;
                break;
        case 2:
        lcd.clear();
        lcd.printString(" object at 120'",0,0);
        lcd.printString("   R + 60'",0,2);
        lcd.printString("   L - 60'",0,4);
        lcd.refresh();
            if (R == 1){
                state = 3;}
            else if (L == 1){
                state = 1;}
            else state = 2;
                break;
        case 3:
        lcd.clear();
        lcd.printString(" object at 180'",0,0);
        lcd.printString("   R + 60'",0,2);
        lcd.printString("   L - 60'",0,4);
        lcd.refresh();
            if (R == 1){
                state = 4;}
            else if (L == 1){
                state = 2;}
            else state = 3;
                break;
        case 4:
        lcd.clear();
        lcd.printString(" object at 240'",0,0);
        lcd.printString("   R + 60'",0,2);
        lcd.printString("   L - 60'",0,4);
        lcd.refresh();
            if (R == 1){
                state = 5;}
            else if (L == 1){
                state = 3;}
            else state = 4;
                break;
         case 5:
        lcd.clear();
        lcd.printString(" object at 300'",0,0);
        lcd.printString("   R + 60'",0,2);
        lcd.printString("   L - 60'",0,4);
        lcd.refresh();
            if (R == 1){
                state = 0;}
            else if (L == 1){
                state = 4;}
            else state = 5;
                break;
            default:
                error("Invalid state");  //invalid state - call error routine
                // or could jump to starting state i.e. state = 0
                break;  
        }
        if (Back_flag == 1) {
            Back_flag = 0;
            main_menu();}
            
        ThisThread::sleep_for(200);
        }
        }
        
void calibrate_object(){
     
     int distance; 
     
     while(1)
    {
      lcd.clear();
      mu.checkDistance();     //call checkDistance() as much as possible, as this is where
                                //the class checks if dist needs to be called.
      printf("Safe Distance %dmm\r\n", distance);
    
        char displaydist[14];
        sprintf(displaydist,"%dmm", distance); // print formatted data to buffer
        lcd.printString("set object to xxx",0,0);
        lcd.printString(displaydist,0,3); // display on screen
        lcd.refresh();
        
        if (Back_flag == 1) {
            Back_flag = 0;
            main_menu();}
            
        wait (1);
        }
        }
        
void Start_isr()
    {
    Start_flag = 1;
    }

void Back_isr()
    {
    Back_flag = 1;
    }