main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "C12832.h"
00004 #include "RangeFinder.h"
00005 #include "WS2812.h"
00006 #include "PixelArray.h" 
00007 
00008 //FINITE STATE MACHINE EVENTS
00009 #define TIME_OUT 0
00010 #define TOO_CLOSE 1
00011 #define TOO_FAR 2
00012 #define DETECT 3
00013 
00014 //STATES
00015 #define SCAN_FACE 0
00016 #define PAN_TILT 1
00017 #define LOCK_DOWN 2
00018 
00019 #define WS2812_BUF 10
00020 #define NUM_COLORS 1
00021 #define NUM_LEDS_PER_COLOR 10
00022 PixelArray px(WS2812_BUF);
00023 
00024 // See the program page for information on the timing numbers
00025 // The given numbers are for the K64F
00026 WS2812 ws(p9, WS2812_BUF, 5, 10, 10, 15);
00027 
00028 //pass event via message queue
00029 typedef struct {
00030     int    event;   /* AD result of measured voltage */
00031 } message_t;
00032 
00033 MemoryPool<message_t, 16> mpool;
00034 Queue<message_t, 16> queue;
00035 
00036 PwmOut Servo_Y(p22);
00037 PwmOut Servo_X(p21);
00038 PwmOut Servo_Z(p23);
00039 
00040 //SONAR_SENSOR INPUT
00041 RangeFinder ping_sensor(p24, 5, 5800.0, 100000);   
00042 
00043 //local display]
00044 C12832 lcd(p5, p7, p6, p8, p11);
00045 
00046 //leds for debug
00047 DigitalOut led(LED1); //button press
00048 DigitalOut led2(LED2); //fsm thread
00049 DigitalOut led3(LED3);
00050 DigitalOut led4(LED4); //timeout thread
00051 
00052 //Mutex
00053 Mutex Distance_Mutex;
00054 
00055 //Gobal Var
00056 float Servo_z_smooth;
00057 float Distance= 0 ;
00058 
00059     
00060 void timeout_event(void const *n) 
00061 { 
00062         message_t *message = mpool.alloc();
00063         message->event = TIME_OUT; 
00064         queue.put(message);   
00065        
00066         led =  !led;  
00067 }
00068 
00069 
00070 void read_sonar_thread(void const *argument) 
00071     {
00072     
00073     while (true) 
00074         {
00075         Distance_Mutex.lock();
00076         Distance = ping_sensor.read_m();
00077         Distance_Mutex.unlock();
00078         Thread::wait(100);  
00079     }
00080 } 
00081 
00082 void too_close_event_thread(void const *argument) 
00083     {
00084     float dist_thread = 0;
00085     while (true) 
00086         {
00087         Distance_Mutex.lock();
00088         dist_thread = Distance;
00089         Distance_Mutex.unlock(); 
00090         
00091         if (dist_thread < 0.25) 
00092             {
00093             //event via a message queue
00094             message_t *message = mpool.alloc();
00095             message->event = TOO_CLOSE; 
00096             queue.put(message);  
00097 
00098             led2 =  !led2;
00099             } 
00100         Thread::wait(250);
00101   
00102     }
00103 }  
00104                 
00105 void detect_event_thread(void const *argument) 
00106     {
00107     float dist_thread = 0;
00108     while (true) 
00109         {
00110         Distance_Mutex.lock();
00111         dist_thread = Distance;
00112         Distance_Mutex.unlock(); 
00113         
00114         if(dist_thread > 0.25 & dist_thread < 2)
00115             {
00116             //event via a message queue
00117             message_t *message = mpool.alloc();
00118             message->event = DETECT;
00119             queue.put(message);
00120         
00121             led3= !led3;
00122             }
00123         Thread::wait(250);            
00124         }
00125     }
00126 
00127 void too_far_event_thread(void const *argument) 
00128 {
00129     float dist_thread = 0;
00130     while (true) 
00131         {
00132         Distance_Mutex.lock();
00133         dist_thread = Distance;
00134         Distance_Mutex.unlock();  
00135           
00136         if (dist_thread > 3) 
00137             {
00138             //event via a message queue
00139             message_t *message = mpool.alloc();
00140             message->event = TOO_FAR; 
00141             queue.put(message);
00142             
00143             led4 =  !led4;
00144             }
00145         Thread::wait(250);
00146       }
00147 }
00148    
00149    
00150 
00151 void Servo_Z_Thread(void const *args) {
00152     float Sevro_Pos;
00153     float Sevro_Pos_New = 0.1;
00154     float Sevro_Pos_Old = 0.01;
00155     
00156     while (true) {
00157         
00158         Sevro_Pos_New = Servo_z_smooth;
00159         
00160         if(Sevro_Pos_Old <= Sevro_Pos_New)
00161             {
00162             for(float i=Sevro_Pos_Old*10000; i<=Sevro_Pos_New*10000; i=i+25)
00163                 {
00164                 Sevro_Pos = i/10000;
00165                 Servo_Z = Sevro_Pos;
00166                 printf("Servo_Z1: %1.6f %1.6f %1.6f\n\r ",Sevro_Pos, i,Sevro_Pos_Old);
00167                 //Thread::wait(50);
00168                 }
00169             }
00170         else
00171             {
00172             for(float i=Sevro_Pos_Old*10000; i>=Sevro_Pos_New*10000; i=i-25)
00173                 {
00174                 Sevro_Pos = i/10000;
00175                 Servo_Z = Sevro_Pos;
00176                 printf("Servo_Z2: %1.6f \n\r",Sevro_Pos);
00177                 //Thread::wait(50);
00178                 }            
00179             }
00180         Sevro_Pos_Old = Sevro_Pos_New;
00181         Thread::wait(500);
00182     }    
00183 }
00184  
00185  
00186  
00187 void LED_All_Colour(int red , int green ,int blue , int bright)
00188     {
00189     ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
00190     
00191     int colorbuf[NUM_COLORS] = {0x000000};
00192     //int colorbuf[NUM_COLORS] = {0x000000,0x00f0ff,0x00ff00,0x00ffff,0xffffff,0x00ff00,0x00ffff,0x0000ff,0xff00ff};
00193     
00194     red <<= 16;
00195     green <<= 8;
00196     
00197     colorbuf[0] = red + green + blue;
00198     
00199     printf("Colour: %6x red:%6x  Green:%6x  Blue:%6x Bright:%x \n\r",colorbuf[0], red , green , blue, bright);
00200     // for each of the colours (j) write out 10 of them
00201     // the pixels are written at the colour*10, plus the colour position
00202     // all modulus 60 so it wraps around
00203     for (int i = 0; i < WS2812_BUF; i++) {
00204         px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
00205     }
00206     
00207     // now all the colours are computed, add a fade effect using intensity scaling
00208     // compute and write the II value for each pixel
00209     for (int j=0; j<WS2812_BUF; j++) {
00210         // px.SetI(pixel position, II value)
00211         //px.SetI(j%WS2812_BUF, 0xff+(0xf*(j%NUM_LEDS_PER_COLOR)));     //full brightness
00212         px.SetI(j%WS2812_BUF, bright +(bright*(j%NUM_LEDS_PER_COLOR)));        //control brightness    
00213     }
00214     //set the colour of the LEDs
00215     for (int z=WS2812_BUF; z >= 0 ; z--) {
00216         ws.write_offsets(px.getBuf(),z,z,z);
00217         
00218 
00219         }    
00220 }
00221 
00222 
00223 void LED_Colour_Pos(int position, int red , int green ,int blue , int bright)
00224     {
00225     ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
00226     
00227     int colorbuf = 0x000000;
00228     
00229     red <<= 16;
00230     green <<= 8;
00231     
00232     colorbuf = red + green + blue;
00233     
00234     printf("Colour: %6x red:%6x  Green:%6x  Blue:%6x Bright:%x \n\r",colorbuf, red , green , blue, bright);
00235     // for each of the colours (j) write out 10 of them
00236     // the pixels are written at the colour*10, plus the colour position
00237     // all modulus 60 so it wraps around
00238     px.Set(position, colorbuf);
00239     
00240     // now all the colours are computed, add a fade effect using intensity scaling
00241     // compute and write the II value for each pixel
00242     //px.SetI(pixel position, II value)
00243     px.SetI(position, bright);        //control brightness    
00244 }
00245 
00246 void LED_Colour_Pos_Set()
00247 {
00248     //set the colour of the LEDs
00249     for (int z=WS2812_BUF-1; z >= 0 ; z--) 
00250     {
00251         ws.write_offsets(px.getBuf(),z,z,z);
00252     } 
00253 }
00254 
00255 void Smooth_Fade_Thread(void const *args)
00256 {
00257    int speed = 10;
00258    int h = 1;
00259    while(1)
00260    {
00261        Thread::signal_wait(0x2);   //wait for flag to be set
00262        for(int j = 0; j<=h;j++)
00263         {
00264             for(int i = 0;i<=255;i=i+speed)
00265             {
00266                 LED_All_Colour(i, 0 ,255-i , 255);
00267                 //wait(0.01);
00268             }    
00269             for(int i = 0;i<=255;i=i+speed)
00270             {
00271                 LED_All_Colour(255-i, i ,0 , 255);
00272                 //wait(0.01);
00273             } 
00274             for(int i = 0;i<=255;i=i+speed)
00275             {
00276                 LED_All_Colour(0, 255-i ,i , 255);
00277                 //wait(0.01);
00278             } 
00279         }
00280     }  
00281 }
00282 
00283 void Smooth_Fade(int h , int speed)
00284 {
00285 
00286    for(int j = 0; j<=h;j++)
00287     {
00288         for(int i = 0;i<=255;i=i+speed)
00289         {
00290             LED_All_Colour(i, 0 ,255-i , 255);
00291             //wait(0.01);
00292         }    
00293         for(int i = 0;i<=255;i=i+speed)
00294         {
00295             LED_All_Colour(255-i, i ,0 , 255);
00296             //wait(0.01);
00297         } 
00298         for(int i = 0;i<=255;i=i+speed)
00299         {
00300             LED_All_Colour(0, 255-i ,i , 255);
00301             //wait(0.01);
00302         } 
00303     }
00304      
00305 }
00306 
00307 void Demo_thread(void const *argument) {
00308     
00309     Thread thread_smooth_fade(Smooth_Fade_Thread);
00310     
00311     while (true) 
00312         {
00313         Thread::signal_wait(0x1);   //wait for flag to be set
00314     
00315          /////////////////////////////////////////////////////////////////////////////////////
00316        ////////////////////////////////////   X   //////////////////////////////////////////
00317        /////////////////////////////////////////////////////////////////////////////////////
00318         Servo_Y = 0.06;
00319         Servo_X = 0.075;
00320         Servo_z_smooth = 0.09;
00321         LED_Colour_Pos(0,255,0,0,128);
00322         LED_Colour_Pos(1,0,255,0,128);
00323         LED_Colour_Pos(2,0,0,255,128);
00324         LED_Colour_Pos(3,255,255,0,128);  
00325         LED_Colour_Pos(4,255,0,255,128); 
00326         LED_Colour_Pos(5,128,255,0,128);
00327         LED_Colour_Pos(6,128,50,0,255);
00328         LED_Colour_Pos(7,0,255,128,128);
00329         LED_Colour_Pos(8,128,0,128,255);  
00330         LED_Colour_Pos(9,128,128,255,128); 
00331         LED_Colour_Pos_Set();        
00332         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4 \n\r",Servo_X, Servo_Y, Servo_z_smooth);
00333         Thread::wait(500);
00334         Servo_Y = 0.06;
00335         Servo_X = 0.075;
00336         Servo_z_smooth = 0.05;
00337         LED_Colour_Pos(0,128,255,0,128);
00338         LED_Colour_Pos(1,128,50,0,255);
00339         LED_Colour_Pos(2,0,255,128,128);
00340         LED_Colour_Pos(3,128,0,128,255);  
00341         LED_Colour_Pos(4,128,128,255,128); 
00342         LED_Colour_Pos(5,255,255,0,255);
00343         LED_Colour_Pos(6,128,50,127,255);
00344         LED_Colour_Pos(7,255,255,0,128);
00345         LED_Colour_Pos(8,128,0,128,255);  
00346         LED_Colour_Pos(9,0,128,255,128);         
00347         LED_Colour_Pos_Set();         
00348         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00349         Thread::wait(500);
00350         Servo_Y = 0.06;
00351         Servo_X = 0.08;
00352         Servo_z_smooth = 0.08;
00353         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00354         Thread::wait(500); 
00355         Servo_Y = 0.06;
00356         Servo_X = 0.05;
00357         Servo_z_smooth = 0.1;
00358         LED_All_Colour(255, 0 , 0 , 255);
00359         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00360         Thread::wait(500); 
00361         Servo_Y = 0.06;
00362         Servo_X = 0.05;
00363         Servo_z_smooth = 0.05;
00364         LED_All_Colour(255, 255 , 0 , 255);        
00365         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00366         Thread::wait(500); 
00367         Servo_Y = 0.06;
00368         Servo_X = 0.09;
00369         Servo_z_smooth = 0.05;
00370         LED_All_Colour(0, 0 , 255 , 255);        
00371         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00372         Thread::wait(500); 
00373         Servo_Y = 0.06;
00374         Servo_X = 0.05;
00375         Servo_z_smooth = 0.05;
00376         LED_All_Colour(0, 255 , 0 , 255);        
00377         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00378         Thread::wait(500);     
00379         Servo_Y = 0.06;
00380         Servo_X = 0.09;
00381         Servo_z_smooth = 0.05;
00382         LED_Colour_Pos(0,255,0,0,128);
00383         LED_Colour_Pos(1,0,255,0,128);
00384         LED_Colour_Pos(2,0,0,255,128);
00385         LED_Colour_Pos(3,255,255,0,128);  
00386         LED_Colour_Pos(4,255,0,255,128); 
00387         LED_Colour_Pos(5,128,255,0,128);
00388         LED_Colour_Pos(6,128,50,0,255);
00389         LED_Colour_Pos(7,0,255,128,128);
00390         LED_Colour_Pos(8,128,0,128,255);  
00391         LED_Colour_Pos(9,128,128,255,128); 
00392         LED_Colour_Pos_Set(); 
00393         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00394         Thread::wait(500); 
00395         Servo_Y = 0.06;
00396         Servo_X = 0.05;
00397         Servo_z_smooth = 0.05;
00398         thread_smooth_fade.signal_set(0x2);
00399         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00400         Thread::wait(500);  
00401         Servo_Y = 0.055;
00402         Servo_X = 0.08;
00403         Servo_z_smooth = 0.05;
00404         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00405         Thread::wait(500); 
00406         
00407        /////////////////////////////////////////////////////////////////////////////////////
00408        ////////////////////////////////////   y   //////////////////////////////////////////
00409        /////////////////////////////////////////////////////////////////////////////////////
00410         Servo_Y = 0.055;
00411         Servo_X = 0.08;
00412         Servo_z_smooth = 0.05;
00413         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00414         Thread::wait(500);   
00415         Servo_Y = 0.08;
00416         Servo_X = 0.04;
00417         Servo_z_smooth = 0.05;
00418         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00419         Thread::wait(500); 
00420         Servo_Y = 0.055;
00421         Servo_X = 0.075;
00422         Servo_z_smooth = 0.05;
00423         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00424         Thread::wait(500); 
00425         Servo_Y = 0.065;
00426         Servo_X = 0.09;
00427         Servo_z_smooth = 0.05;
00428         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00429         Thread::wait(1000); 
00430         Servo_Y = 0.055;
00431         Servo_X = 0.075;
00432         Servo_z_smooth = 0.05;
00433         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00434         Thread::wait(500);   
00435         Servo_Y = 0.065;
00436         Servo_X = 0.075;
00437         Servo_z_smooth = 0.05;
00438         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00439         Thread::wait(500); 
00440         Servo_Y = 0.055;
00441         Servo_X = 0.075;
00442         Servo_z_smooth = 0.05;
00443         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00444         Thread::wait(500);  
00445         Servo_Y = 0.065;
00446         Servo_X = 0.04;
00447         Servo_z_smooth = 0.05;
00448         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00449         Thread::wait(500); 
00450         Servo_Y = 0.06;
00451         Servo_X = 0.075;
00452         Servo_z_smooth = 0.09;
00453         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00454         Thread::wait(750);                             
00455         
00456         /////////////////////////////////////////////////////////////////////////////////////
00457         ////////////////////////////////////   Z   //////////////////////////////////////////
00458         /////////////////////////////////////////////////////////////////////////////////////
00459         Servo_Y = 0.06;
00460         Servo_X = 0.075;
00461         Servo_z_smooth = 0.08;
00462         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00463         Thread::wait(500);  
00464         Servo_Y = 0.06;
00465         Servo_X = 0.9;
00466         Servo_z_smooth = 0.03;
00467         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00468         Thread::wait(1000);                             
00469         Servo_Y = 0.06;
00470         Servo_X = 0.075;
00471         Servo_z_smooth = 0.07;
00472         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00473         Thread::wait(500);
00474         Servo_Y = 0.06;
00475         Servo_X = 0.04;
00476         Servo_z_smooth = 0.11;
00477         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00478         Thread::wait(750); 
00479         Servo_Y = 0.06;
00480         Servo_X = 0.075;
00481         Servo_z_smooth = 0.07;
00482         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00483         Thread::wait(750);  
00484         Servo_Y = 0.06;
00485         Servo_X = 0.075;
00486         Servo_z_smooth = 0.11;
00487         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00488         Thread::wait(750);                             
00489         Servo_Y = 0.06;
00490         Servo_X = 0.08;
00491         Servo_z_smooth = 0.07;
00492         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00493         Thread::wait(750);
00494         Servo_Y = 0.06;
00495         Servo_X = 0.075;
00496         Servo_z_smooth = 0.11;
00497         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00498         Thread::wait(750);                            
00499         Servo_Y = 0.06;
00500         Servo_X = 0.04;
00501         Servo_z_smooth = 0.07;
00502         printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00503         Thread::wait(1000);                          
00504         Servo_Y = 0.09;
00505         Servo_X = 0.075;
00506         Servo_z_smooth = 0.03;
00507         LED_All_Colour(0, 0 , 0 , 255);  
00508         printf("Servo X: %f1.4 Servo Y: %f1.4 Servo Z: %f1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
00509         Thread::wait(30000);
00510         Thread::signal_wait(0x1);   //wait for flag to be set
00511         Thread::wait(1000);
00512         }
00513      
00514 } 
00515 
00516 void Red_Blue_Flash(int h)
00517 {
00518    for(int i = 0;i<=h;i++)
00519         {
00520             
00521             LED_All_Colour(255, 0 ,0 , 50);
00522             wait(0.02);
00523             LED_All_Colour(255, 0 ,0 , 255);
00524             wait(0.02);
00525             LED_All_Colour(0, 0 , 255 , 50);
00526             wait(0.02);            
00527             LED_All_Colour(0, 0 , 255 , 255);
00528             wait(0.02);
00529         }   
00530 }
00531 
00532 
00533     
00534 int main (void) 
00535 {
00536     
00537 //Thread fsm(fsm_thread); 
00538 Thread too_far_event(too_far_event_thread);
00539 Thread too_close_event(too_close_event_thread);
00540 Thread detect_event(detect_event_thread);
00541 Thread thread_Servo_Z(Servo_Z_Thread );
00542 Thread thread_Demo(Demo_thread );
00543 Thread thread_Sonar_Read(read_sonar_thread );
00544 
00545 //RtosTimer timer(timeout_event, osTimerPeriodic, (void *)0);
00546 
00547 int state = PAN_TILT;
00548 
00549     //start timer with a 2 sec timeout 
00550     //timer.start(2000); 
00551     
00552     while(0)
00553     {
00554     osEvent evt = queue.get();
00555     if (evt.status == osEventMessage) 
00556         {
00557         message_t *message = (message_t*)evt.value.p;
00558         mpool.free(message);
00559         }
00560     }
00561     
00562     while (true) 
00563     {
00564         
00565         switch(state) // locked
00566             {
00567             
00568             case SCAN_FACE:
00569         
00570             osEvent evt = queue.get();
00571                 if (evt.status == osEventMessage) 
00572                     {
00573                     message_t *message = (message_t*)evt.value.p;
00574                 
00575                     if(message->event == DETECT)
00576                         {
00577                         //next state
00578                         state = SCAN_FACE;
00579                         lcd.cls();
00580                         lcd.locate(0,2);
00581                         lcd.printf("state scan face - detect");
00582                             
00583                         } 
00584                         
00585                     
00586                     if(message->event == TOO_CLOSE)
00587                         {
00588                         //next state
00589                         state = LOCK_DOWN;
00590                         lcd.cls();
00591                         lcd.locate(0,2);
00592                         lcd.printf("state scan face - too close");
00593              
00594                         }   
00595                     
00596                     
00597                     if(message->event == TOO_FAR)
00598                         {
00599                         state = PAN_TILT;
00600                         lcd.cls();
00601                         lcd.locate(0,2);
00602                         lcd.printf("state scan face - too-far");
00603  
00604                         }
00605                     
00606                     mpool.free(message);
00607                     
00608                     }
00609                 
00610                 //timer.start(2000);
00611             
00612             break;
00613             
00614             case PAN_TILT:
00615                 
00616                //osEvent 
00617                evt = queue.get();
00618                 if (evt.status == osEventMessage) 
00619                     {
00620                     message_t *message = (message_t*)evt.value.p;
00621                     
00622                     if(message->event == TOO_FAR)
00623                         {
00624                          //next state
00625                          state = PAN_TILT;  
00626                         lcd.cls();
00627                         lcd.locate(0,2);
00628                         lcd.printf("state pan tilt - too-far");
00629                     
00630                         }
00631                               
00632                     if(message->event == DETECT) 
00633                         {
00634                         //next state
00635                         state = PAN_TILT;   
00636                         thread_Demo.signal_set(0x1);     
00637                         lcd.cls();
00638                         lcd.locate(0,2);
00639                         lcd.printf("state pan tilt - detect");
00640 
00641                         }
00642                                     
00643                     if(message->event == TOO_CLOSE) 
00644                         {
00645                         state = PAN_TILT;  
00646                         
00647                         lcd.cls();
00648                         lcd.locate(0,2);
00649                         lcd.printf("state pan tilt - too close");     
00650 
00651                         }
00652              
00653                 mpool.free(message);
00654                 }
00655                      
00656             //timer.start(2000);
00657             
00658             break;
00659             
00660             case LOCK_DOWN:
00661             
00662                evt = queue.get();
00663                if (evt.status == osEventMessage) 
00664                     {
00665                     message_t *message = (message_t*)evt.value.p;
00666                              
00667                     if(message->event == TOO_CLOSE)
00668                         {
00669                         state = LOCK_DOWN;
00670                         
00671                         lcd.cls();
00672                         lcd.locate(0,2);
00673                         lcd.printf("state lock down - too close");
00674                         }
00675                     
00676                     if(message->event == TIME_OUT) 
00677                         {
00678                         state = PAN_TILT;
00679                         
00680                         lcd.cls();
00681                         lcd.locate(0,2);
00682                         lcd.printf("state lock down - time out");
00683                         }
00684                         
00685                     mpool.free(message);
00686                     
00687                     }   
00688                     
00689                 //timer.start(2000);
00690 
00691             break;          
00692             
00693         }       //End of switch
00694         
00695     //toggle led for local testing    
00696     
00697                                        
00698 }   //end of while(1)
00699     
00700 }