File content as of revision 0:fb0c24069b95:

#include "mbed.h"
#include "rtos.h"
#include "C12832.h"
#include "RangeFinder.h"
#include "WS2812.h"
#include "PixelArray.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

#define WS2812_BUF 10
#define NUM_COLORS 1
#define NUM_LEDS_PER_COLOR 10
PixelArray px(WS2812_BUF);

// See the program page for information on the timing numbers
// The given numbers are for the K64F
WS2812 ws(p9, WS2812_BUF, 5, 10, 10, 15);

//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;

PwmOut Servo_Y(p22);
PwmOut Servo_X(p21);
PwmOut Servo_Z(p23);

//SONAR_SENSOR INPUT
RangeFinder ping_sensor(p24, 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

//Mutex
Mutex Distance_Mutex;

//Gobal Var
float Servo_z_smooth;
float Distance= 0 ;

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


void read_sonar_thread(void const *argument) 
    {
    
    while (true) 
        {
        Distance_Mutex.lock();
        Distance = ping_sensor.read_m();
        Distance_Mutex.unlock();
        Thread::wait(250);  
    }
} 

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

            led2 =  !led2;
            } 
        Thread::wait(250);
  
    }
}  
                
void detect_event_thread(void const *argument) 
    {
    float dist_thread = 0;
    while (true) 
        {
        Distance_Mutex.lock();
        dist_thread = Distance;
        Distance_Mutex.unlock(); 
        
        if(dist_thread > 0.25 & dist_thread < 2)
            {
            //event via a message queue
            message_t *message = mpool.alloc();
            message->event = DETECT;
            queue.put(message);
        
            led3= !led3;
            }
        Thread::wait(250);            
        }
    }

void too_far_event_thread(void const *argument) 
{
    float dist_thread = 0;
    while (true) 
        {
        Distance_Mutex.lock();
        dist_thread = Distance;
        Distance_Mutex.unlock();  
          
        if (dist_thread > 3) 
            {
            //event via a message queue
            message_t *message = mpool.alloc();
            message->event = TOO_FAR; 
            queue.put(message);
            
            led4 =  !led4;
            }
        Thread::wait(250);
      }
}
   
   

void Servo_Z_Thread(void const *args) {
    float Sevro_Pos;
    float Sevro_Pos_New = 0.1;
    float Sevro_Pos_Old = 0.01;
    
    while (true) {
        
        Sevro_Pos_New = Servo_z_smooth;
        
        if(Sevro_Pos_Old <= Sevro_Pos_New)
            {
            for(float i=Sevro_Pos_Old*10000; i<=Sevro_Pos_New*10000; i=i+25)
                {
                Sevro_Pos = i/10000;
                Servo_Z = Sevro_Pos;
                printf("Servo_Z1: %1.6f %1.6f %1.6f\n\r ",Sevro_Pos, i,Sevro_Pos_Old);
                //Thread::wait(50);
                }
            }
        else
            {
            for(float i=Sevro_Pos_Old*10000; i>=Sevro_Pos_New*10000; i=i-25)
                {
                Sevro_Pos = i/10000;
                Servo_Z = Sevro_Pos;
                printf("Servo_Z2: %1.6f \n\r",Sevro_Pos);
                //Thread::wait(50);
                }            
            }
        Sevro_Pos_Old = Sevro_Pos_New;
        Thread::wait(500);
    }    
}
 
 
 
void LED_All_Colour(int red , int green ,int blue , int bright)
    {
    ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
    
    int colorbuf[NUM_COLORS] = {0x000000};
    //int colorbuf[NUM_COLORS] = {0x000000,0x00f0ff,0x00ff00,0x00ffff,0xffffff,0x00ff00,0x00ffff,0x0000ff,0xff00ff};
    
    red <<= 16;
    green <<= 8;
    
    colorbuf[0] = red + green + blue;
    
    printf("Colour: %6x red:%6x  Green:%6x  Blue:%6x Bright:%x \n\r",colorbuf[0], red , green , blue, bright);
    // for each of the colours (j) write out 10 of them
    // the pixels are written at the colour*10, plus the colour position
    // all modulus 60 so it wraps around
    for (int i = 0; i < WS2812_BUF; i++) {
        px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
    }
    
    // now all the colours are computed, add a fade effect using intensity scaling
    // compute and write the II value for each pixel
    for (int j=0; j<WS2812_BUF; j++) {
        // px.SetI(pixel position, II value)
        //px.SetI(j%WS2812_BUF, 0xff+(0xf*(j%NUM_LEDS_PER_COLOR)));     //full brightness
        px.SetI(j%WS2812_BUF, bright +(bright*(j%NUM_LEDS_PER_COLOR)));        //control brightness    
    }
    //set the colour of the LEDs
    for (int z=WS2812_BUF; z >= 0 ; z--) {
        ws.write_offsets(px.getBuf(),z,z,z);
        

        }    
}


void LED_Colour_Pos(int position, int red , int green ,int blue , int bright)
    {
    ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
    
    int colorbuf = 0x000000;
    
    red <<= 16;
    green <<= 8;
    
    colorbuf = red + green + blue;
    
    printf("Colour: %6x red:%6x  Green:%6x  Blue:%6x Bright:%x \n\r",colorbuf, red , green , blue, bright);
    // for each of the colours (j) write out 10 of them
    // the pixels are written at the colour*10, plus the colour position
    // all modulus 60 so it wraps around
    px.Set(position, colorbuf);
    
    // now all the colours are computed, add a fade effect using intensity scaling
    // compute and write the II value for each pixel
    //px.SetI(pixel position, II value)
    px.SetI(position, bright);        //control brightness    
}

void LED_Colour_Pos_Set()
{
    //set the colour of the LEDs
    for (int z=WS2812_BUF-1; z >= 0 ; z--) 
    {
        ws.write_offsets(px.getBuf(),z,z,z);
    } 
}


void Smooth_Fade(int h , int speed)
{
   for(int j = 0; j<=h;j++)
    {
        for(int i = 0;i<=255;i=i+speed)
        {
            LED_All_Colour(i, 0 ,255-i , 255);
            //wait(0.01);
        }    
        for(int i = 0;i<=255;i=i+speed)
        {
            LED_All_Colour(255-i, i ,0 , 255);
            //wait(0.01);
        } 
        for(int i = 0;i<=255;i=i+speed)
        {
            LED_All_Colour(0, 255-i ,i , 255);
            //wait(0.01);
        } 
    }  
}

void Demo_thread(void const *argument) {
    while (true) 
        {
        Thread::signal_wait(0x1);   //wait for flag to be set
    
         /////////////////////////////////////////////////////////////////////////////////////
       ////////////////////////////////////   X   //////////////////////////////////////////
       /////////////////////////////////////////////////////////////////////////////////////
        Servo_Y = 0.05;
        Servo_X = 0.06;
        Servo_z_smooth = 0.01;
        LED_Colour_Pos(0,255,0,0,128);
        LED_Colour_Pos(1,0,255,0,128);
        LED_Colour_Pos(2,0,0,255,128);
        LED_Colour_Pos(3,255,255,0,128);  
        LED_Colour_Pos(4,255,0,255,128); 
        LED_Colour_Pos(5,128,255,0,128);
        LED_Colour_Pos(6,128,50,0,255);
        LED_Colour_Pos(7,0,255,128,128);
        LED_Colour_Pos(8,128,0,128,255);  
        LED_Colour_Pos(9,128,128,255,128); 
        LED_Colour_Pos_Set();        
        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);
        Thread::wait(1000);
        Servo_Y = 0.05;
        Servo_X = 0.06;
        Servo_z_smooth = 0.05;
        LED_Colour_Pos(0,128,255,0,128);
        LED_Colour_Pos(1,128,50,0,255);
        LED_Colour_Pos(2,0,255,128,128);
        LED_Colour_Pos(3,128,0,128,255);  
        LED_Colour_Pos(4,128,128,255,128); 
        LED_Colour_Pos(5,255,255,0,255);
        LED_Colour_Pos(6,128,50,127,255);
        LED_Colour_Pos(7,255,255,0,128);
        LED_Colour_Pos(8,128,0,128,255);  
        LED_Colour_Pos(9,0,128,255,128);         
        LED_Colour_Pos_Set();         
        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);
        Thread::wait(500);
        Servo_Y = 0.05;
        Servo_X = 0.08;
        Servo_z_smooth = 0.08;
        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);
        Thread::wait(500); 
        Servo_Y = 0.06;
        Servo_X = 0.05;
        Servo_z_smooth = 0.1;
        LED_All_Colour(255, 0 , 0 , 255);
        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);
        Thread::wait(500); 
        Servo_Y = 0.05;
        Servo_X = 0.05;
        Servo_z_smooth = 0.05;
        LED_All_Colour(255, 255 , 0 , 255);        
        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);
        Thread::wait(500); 
        Servo_Y = 0.05;
        Servo_X = 0.09;
        Servo_z_smooth = 0.05;
        LED_All_Colour(0, 0 , 255 , 255);        
        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);
        Thread::wait(500); 
        Servo_Y = 0.05;
        Servo_X = 0.05;
        Servo_z_smooth = 0.05;
        LED_All_Colour(0, 255 , 0 , 255);        
        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);
        Thread::wait(500);     
        Servo_Y = 0.05;
        Servo_X = 0.09;
        Servo_z_smooth = 0.05;
        LED_Colour_Pos(0,255,0,0,128);
        LED_Colour_Pos(1,0,255,0,128);
        LED_Colour_Pos(2,0,0,255,128);
        LED_Colour_Pos(3,255,255,0,128);  
        LED_Colour_Pos(4,255,0,255,128); 
        LED_Colour_Pos(5,128,255,0,128);
        LED_Colour_Pos(6,128,50,0,255);
        LED_Colour_Pos(7,0,255,128,128);
        LED_Colour_Pos(8,128,0,128,255);  
        LED_Colour_Pos(9,128,128,255,128); 
        LED_Colour_Pos_Set(); 
        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);
        Thread::wait(500); 
        Servo_Y = 0.05;
        Servo_X = 0.05;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500);  
        Servo_Y = 0.05;
        Servo_X = 0.08;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500); 
        
       /////////////////////////////////////////////////////////////////////////////////////
       ////////////////////////////////////   y   //////////////////////////////////////////
       /////////////////////////////////////////////////////////////////////////////////////
        Servo_Y = 0.055;
        Servo_X = 0.08;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500);   
        Servo_Y = 0.08;
        Servo_X = 0.075;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500); 
        Servo_Y = 0.055;
        Servo_X = 0.075;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500); 
        Servo_Y = 0.065;
        Servo_X = 0.075;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(1000); 
        Servo_Y = 0.055;
        Servo_X = 0.075;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500);   
        Servo_Y = 0.065;
        Servo_X = 0.075;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500); 
        Servo_Y = 0.055;
        Servo_X = 0.075;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500);  
        Servo_Y = 0.065;
        Servo_X = 0.075;
        Servo_z_smooth = 0.05;
        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);
        Thread::wait(500); 
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.09;
        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);
        Thread::wait(750);                             
        
        /////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////   Z   //////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.08;
        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);
        Thread::wait(500);  
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.1;
        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);
        Thread::wait(500);                             
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.07;
        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);
        Thread::wait(500);
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.11;
        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);
        Thread::wait(750); 
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.07;
        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);
        Thread::wait(750);  
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.11;
        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);
        Thread::wait(750);                             
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.07;
        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);
        Thread::wait(750);
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.11;
        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);
        Thread::wait(750);                            
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.07;
        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);
        Thread::wait(1500);                          
        Servo_Y = 0.06;
        Servo_X = 0.75;
        Servo_z_smooth = 0.03;
        printf("Servo X: %f1.4 Servo Y: %f1.4 Servo Z: %f1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
        Thread::wait(1500);
        }
     
} 

void Red_Blue_Flash(int h)
{
   for(int i = 0;i<=h;i++)
        {
            
            LED_All_Colour(255, 0 ,0 , 50);
            wait(0.02);
            LED_All_Colour(255, 0 ,0 , 255);
            wait(0.02);
            LED_All_Colour(0, 0 , 255 , 50);
            wait(0.02);            
            LED_All_Colour(0, 0 , 255 , 255);
            wait(0.02);
        }   
}

void Demo()
{
                      
}
        
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);
Thread thread_Servo_Z(Servo_Z_Thread );
Thread thread_Demo(Demo_thread );
Thread thread_Sonar_Read(read_sonar_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 = PAN_TILT;   
                        thread_Demo.signal_set(0x1);     
                        lcd.cls();
                        lcd.locate(0,2);
                        lcd.printf("state pan tilt - detect");

                        }
                                    
                    if(message->event == TOO_CLOSE) 
                        {
                        state = PAN_TILT;  
                        
                        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)
    
}