main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "C12832.h"
00004 #include "RangeFinder.h"
00005 
00006 //FINITE STATE MACHINE EVENTS
00007 #define TIME_OUT 0
00008 #define TOO_CLOSE 1
00009 #define TOO_FAR 2
00010 #define DETECT 3
00011 
00012 //STATES
00013 #define SCAN_FACE 0
00014 #define PAN_TILT 1
00015 #define LOCK_DOWN 2
00016 
00017 //pass event via message queue
00018 typedef struct {
00019     int    event;   /* AD result of measured voltage */
00020 } message_t;
00021 
00022 MemoryPool<message_t, 16> mpool;
00023 Queue<message_t, 16> queue;
00024 
00025 //SONAR_SENSOR INPUT
00026 RangeFinder ping_sensor(p21, 5, 5800.0, 100000);   
00027 
00028 //local display]
00029 C12832 lcd(p5, p7, p6, p8, p11);
00030 
00031 //leds for debug
00032 DigitalOut led(LED1); //button press
00033 DigitalOut led2(LED2); //fsm thread
00034 DigitalOut led3(LED3);
00035 DigitalOut led4(LED4); //timeout thread
00036 
00037     
00038 void timeout_event(void const *n) 
00039 { 
00040         message_t *message = mpool.alloc();
00041         message->event = TIME_OUT; 
00042         queue.put(message);   
00043        
00044         led =  !led;  
00045 }
00046 
00047 
00048 void too_close_event_thread(void const *argument) 
00049     {
00050     while (true) 
00051         {
00052         float Distance = ping_sensor.read_m();
00053         if (Distance < 0) 
00054             {
00055             //event via a message queue
00056             message_t *message = mpool.alloc();
00057             message->event = TOO_CLOSE; 
00058             queue.put(message);  
00059 
00060             led2 =  !led2;
00061             } 
00062   
00063     }
00064 }  
00065                 
00066 void detect_event_thread(void const *argument) 
00067     {
00068     while (true) 
00069         {
00070         float Distance = ping_sensor.read_m();
00071         if(Distance > 1 & Distance < 2)
00072             {
00073             //event via a message queue
00074             message_t *message = mpool.alloc();
00075             message->event = DETECT;
00076             queue.put(message);
00077         
00078             led3= !led3;
00079             }
00080         }
00081     }
00082 
00083 void too_far_event_thread(void const *argument) 
00084 {
00085     while (true) 
00086         {
00087         float Distance = ping_sensor.read_m();
00088         if (Distance > 3) 
00089             {
00090             //event via a message queue
00091             message_t *message = mpool.alloc();
00092             message->event = TOO_FAR; 
00093             queue.put(message);
00094             
00095             led4 =  !led4;
00096             }
00097       }
00098 }
00099         
00100 int main (void) 
00101 {
00102     
00103 //Thread fsm(fsm_thread); 
00104 Thread too_far_event(too_far_event_thread);
00105 Thread too_close_event(too_close_event_thread);
00106 Thread detect_event(detect_event_thread);
00107 RtosTimer timer(timeout_event, osTimerPeriodic, (void *)0);
00108 
00109 int state = PAN_TILT;
00110 
00111     //start timer with a 2 sec timeout 
00112     timer.start(2000); 
00113     
00114     while(0)
00115     {
00116     osEvent evt = queue.get();
00117     if (evt.status == osEventMessage) 
00118         {
00119         message_t *message = (message_t*)evt.value.p;
00120         mpool.free(message);
00121         }
00122     }
00123     
00124     while (true) 
00125     {
00126         
00127         switch(state) // locked
00128             {
00129             
00130             case SCAN_FACE:
00131         
00132             osEvent evt = queue.get();
00133                 if (evt.status == osEventMessage) 
00134                     {
00135                     message_t *message = (message_t*)evt.value.p;
00136                 
00137                     if(message->event == DETECT)
00138                         {
00139                         //next state
00140                         state = SCAN_FACE;
00141                         lcd.cls();
00142                         lcd.locate(0,2);
00143                         lcd.printf("state scan face - detect");
00144                             
00145                         } 
00146                         
00147                     
00148                     if(message->event == TOO_CLOSE)
00149                         {
00150                         //next state
00151                         state = LOCK_DOWN;
00152                         lcd.cls();
00153                         lcd.locate(0,2);
00154                         lcd.printf("state scan face - too close");
00155              
00156                         }   
00157                     
00158                     
00159                     if(message->event == TOO_FAR)
00160                         {
00161                         state = PAN_TILT;
00162                         lcd.cls();
00163                         lcd.locate(0,2);
00164                         lcd.printf("state scan face - too-far");
00165  
00166                         }
00167                     
00168                     mpool.free(message);
00169                     
00170                     }
00171                 
00172                 timer.start(2000);
00173             
00174             break;
00175             
00176             case PAN_TILT:
00177                 
00178                //osEvent 
00179                evt = queue.get();
00180                 if (evt.status == osEventMessage) 
00181                     {
00182                     message_t *message = (message_t*)evt.value.p;
00183                     
00184                     if(message->event == TOO_FAR)
00185                         {
00186                          //next state
00187                          state = PAN_TILT;  
00188                         lcd.cls();
00189                         lcd.locate(0,2);
00190                         lcd.printf("state pan tilt - too-far");
00191                     
00192                         }
00193                               
00194                     if(message->event == DETECT) 
00195                         {
00196                         //next state
00197                         state = SCAN_FACE;       
00198                         lcd.cls();
00199                         lcd.locate(0,2);
00200                         lcd.printf("state pan tilt - detect");
00201 
00202                         }
00203                                     
00204                     if(message->event == TOO_CLOSE) 
00205                         {
00206                         state = LOCK_DOWN;  
00207                         
00208                         lcd.cls();
00209                         lcd.locate(0,2);
00210                         lcd.printf("state pan tilt - too close");     
00211 
00212                         }
00213              
00214                 mpool.free(message);
00215                 }
00216                      
00217             timer.start(2000);
00218             
00219             break;
00220             
00221             case LOCK_DOWN:
00222             
00223                evt = queue.get();
00224                if (evt.status == osEventMessage) 
00225                     {
00226                     message_t *message = (message_t*)evt.value.p;
00227                              
00228                     if(message->event == TOO_CLOSE)
00229                         {
00230                         state = LOCK_DOWN;
00231                         
00232                         lcd.cls();
00233                         lcd.locate(0,2);
00234                         lcd.printf("state lock down - too close");
00235                         }
00236                     
00237                     if(message->event == TIME_OUT) 
00238                         {
00239                         state = PAN_TILT;
00240                         
00241                         lcd.cls();
00242                         lcd.locate(0,2);
00243                         lcd.printf("state lock down - time out");
00244                         }
00245                         
00246                     mpool.free(message);
00247                     
00248                     }   
00249                     
00250                 timer.start(2000);
00251 
00252             break;          
00253             
00254         }       //End of switch
00255         
00256     //toggle led for local testing    
00257     
00258                                        
00259 }   //end of while(1)
00260     
00261 }