File content as of revision 0:3bdd926bec58:

#include "mbed.h"
#include "rtos.h"
#include "C12832.h"
#include "RangeFinder.h"

//FINITE STATE MACHINE EVENTS
#define TIME_OUT 0
#define TOO_CLOSE 1
#define TOO_FAR 2
#define DETECT 3

//STATES
#define SCAN_FACE 0
#define PAN_TILT 1
#define LOCK_DOWN 2

//pass event via message queue
typedef struct {
    int    event;   /* AD result of measured voltage */
} message_t;

MemoryPool<message_t, 16> mpool;
Queue<message_t, 16> queue;

//SONAR_SENSOR INPUT
RangeFinder ping_sensor(p21, 5, 5800.0, 100000);   

//local display]
C12832 lcd(p5, p7, p6, p8, p11);

//leds for debug
DigitalOut led(LED1); //button press
DigitalOut led2(LED2); //fsm thread
DigitalOut led3(LED3);
DigitalOut led4(LED4); //timeout thread

    
void timeout_event(void const *n) 
{ 
        message_t *message = mpool.alloc();
        message->event = TIME_OUT; 
        queue.put(message);   
       
        led =  !led;  
}


void too_close_event_thread(void const *argument) 
    {
    while (true) 
        {
        float Distance = ping_sensor.read_m();
        if (Distance < 0) 
            {
            //event via a message queue
            message_t *message = mpool.alloc();
            message->event = TOO_CLOSE; 
            queue.put(message);  

            led2 =  !led2;
            } 
  
    }
}  
                
void detect_event_thread(void const *argument) 
    {
    while (true) 
        {
        float Distance = ping_sensor.read_m();
        if(Distance > 1 & Distance < 2)
            {
            //event via a message queue
            message_t *message = mpool.alloc();
            message->event = DETECT;
            queue.put(message);
        
            led3= !led3;
            }
        }
    }

void too_far_event_thread(void const *argument) 
{
    while (true) 
        {
        float Distance = ping_sensor.read_m();
        if (Distance > 3) 
            {
            //event via a message queue
            message_t *message = mpool.alloc();
            message->event = TOO_FAR; 
            queue.put(message);
            
            led4 =  !led4;
            }
      }
}
        
int main (void) 
{
    
//Thread fsm(fsm_thread); 
Thread too_far_event(too_far_event_thread);
Thread too_close_event(too_close_event_thread);
Thread detect_event(detect_event_thread);
RtosTimer timer(timeout_event, osTimerPeriodic, (void *)0);

int state = PAN_TILT;

    //start timer with a 2 sec timeout 
    timer.start(2000); 
    
    while(0)
    {
    osEvent evt = queue.get();
    if (evt.status == osEventMessage) 
        {
        message_t *message = (message_t*)evt.value.p;
        mpool.free(message);
        }
    }
    
    while (true) 
    {
        
        switch(state) // locked
            {
            
            case SCAN_FACE:
        
            osEvent evt = queue.get();
                if (evt.status == osEventMessage) 
                    {
                    message_t *message = (message_t*)evt.value.p;
                
                    if(message->event == DETECT)
                        {
                        //next state
                        state = SCAN_FACE;
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state scan face - detect");
                            
                        } 
                        
                    
                    if(message->event == TOO_CLOSE)
                        {
                        //next state
                        state = LOCK_DOWN;
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state scan face - too close");
             
                        }   
                    
                    
                    if(message->event == TOO_FAR)
                        {
                        state = PAN_TILT;
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state scan face - too-far");
 
                        }
                    
                    mpool.free(message);
                    
                    }
                
                timer.start(2000);
            
            break;
            
            case PAN_TILT:
                
               //osEvent 
               evt = queue.get();
                if (evt.status == osEventMessage) 
                    {
                    message_t *message = (message_t*)evt.value.p;
                    
                    if(message->event == TOO_FAR)
                        {
                         //next state
                         state = PAN_TILT;  
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state pan tilt - too-far");
                    
                        }
                              
                    if(message->event == DETECT) 
                        {
                        //next state
                        state = SCAN_FACE;       
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state pan tilt - detect");

                        }
                                    
                    if(message->event == TOO_CLOSE) 
                        {
                        state = LOCK_DOWN;  
                        
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state pan tilt - too close");     

                        }
             
                mpool.free(message);
                }
                     
            timer.start(2000);
            
            break;
            
            case LOCK_DOWN:
            
               evt = queue.get();
               if (evt.status == osEventMessage) 
                    {
                    message_t *message = (message_t*)evt.value.p;
                             
                    if(message->event == TOO_CLOSE)
                        {
                        state = LOCK_DOWN;
                        
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state lock down - too close");
                        }
                    
                    if(message->event == TIME_OUT) 
                        {
                        state = PAN_TILT;
                        
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state lock down - time out");
                        }
                        
                    mpool.free(message);
                    
                    }   
                    
                timer.start(2000);

            break;          
            
        }       //End of switch
        
    //toggle led for local testing    
    
                                       
}   //end of while(1)
    
}