Tuesday, 5 May 2015

Final Blog Donal Murphy


Applied Embedded Operating Systems
Final blog - Donal Murphy

In the final build stage, the objective was for three lamps to work simultaneously. These three lamps constructed by the three individual teams would have a set of pre-defined moods/action (exited, scared, etc). Incorporating my sonar into the project the idea was that if motion was detected within a certain range then face track, if no object is being detected then move into a default state (head down). As time was against us on this project, the final incorporation of all the applications to the lamp bot succumbed to this lack of time. Instead, separate application can be demonstrated on the lamp bot, this also leaves the lamp bot open to future improvement in the coming years.


Final build objective

As we see above the idea was for an overall interaction with the target audience all the while remaining anonymous. The idea was that as kids, stressed students, lecturers, anybody approaches the lamp bot, interaction would begin. This would be initiated by the sonar sensor detecting movement and presence nearby and the party trick, to track the movement of peoples faces as they move in and out of frame. This would bring a sense of life and personality to the lamp bot as we associate with the well known PIXAR lamp shown below.


PIXAR lamp

From a final implementation point of view, the best option was viewed to be a state machine type setup whereby the lamp bot would only operate in one state depending on the input from the sonar sensor. For example, state one = face track, state two = default (head drop), etc. Using the original sonar sensor program I had developed as shown below I modified it to work in an event driven state machine. 

Code for original sonar sensor with Dii's servo's incorporated:

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

// Ultrasonic range finder
RangeFinder ping_sensor(p21, 5, 5800.0, 100000);    //10, 5800.0, 100000);
DigitalOut led(LED1);
C12832 lcd(p5, p7, p6, p8, p11);
PwmOut s1(p24);

//PwmOut r (p23);
//PwmOut g (p24);
//PwmOut b (p25);

int main()  
{
    led = 1;
    float Distance;
    float servo_position = 0;
    
    while (1) 
    {
        Distance = ping_sensor.read_m();
        // returns -0.1 if the pulse times out
        if (Distance == -1.0)  
            {
            lcd.cls();
            lcd.locate(0,0);
            lcd.printf("Timeout Error");
            printf("Timeout Error.\n");
            //r = 1.0;
            //g = 1.0;
            //b = 1.0;  
            } 
        // if the distance to an object is greater than 3 metres - no object detected
        else if (Distance > 3) 
            {  
            lcd.cls();
            lcd.locate(0,0);
            lcd.printf("No object being detected");
            printf("No object being detected.\n");
            //r = 0.0;
            //g = 1.0;
            //b = 1.0;
            } 
        else 
        // Display distance to object in metres 
            {
            lcd.cls();
            lcd.locate(0,0);
            lcd.printf("Distance = %f m.\n", Distance);
            printf("Distance = %f m.\n", Distance);
            //r = 1.0;
            //g = 0.0;
            //b = 1.0;
            }
          
        servo_position = Distance/4.41176470588;  //servo centre position @ 0.3 metres

        if(servo_position > 0.019 & servo_position < 0.118)
        {
            s1.write(servo_position); // starts at 90 to the right no vibration
            
            lcd.locate(0,15);    // Go to the first row of the LCD
            lcd.printf("Dii's %f",servo_position); //Display the following text
        }
                 
        wait(0.5);
        led = !led; // flash LED   
        s1.write(Distance); 
    }
}  


From here see below the code for the sonar event driven state machine:

#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)
    
}


This program was used as a benchmark to incorporate the entire lamp bot functionality. This program was then given to Cathal Deehy-Power to incorporate his servo code to control the lamp bot. Cathal also used his programming excellence to fully amend the errors encountered as it was not a case of just copying and pasting his code into mine. After some trial and error the lamp bot now detects and responds to movement within its vicinity.  See below the fully functioning program Cathal produced.

Fully functioning sonar-servo program:

#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, 5101015);
 
//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, 55800.0100000);   
 
//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(100);  
    }
} 
 
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_Thread(void const *args)
{
   int speed = 10;
   int h = 1;
   while(1)
   {
       Thread::signal_wait(0x2);   //wait for flag to be set
       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(0255-i ,i , 255);
                //wait(0.01);
            } 
        }
    }  
}
 
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(0255-i ,i , 255);
            //wait(0.01);
        } 
    }
     
}
 
void Demo_thread(void const *argument) {
    
    Thread thread_smooth_fade(Smooth_Fade_Thread);
    
    while (true) 
        {
        Thread::signal_wait(0x1);   //wait for flag to be set
    
         /////////////////////////////////////////////////////////////////////////////////////
       ////////////////////////////////////   X   //////////////////////////////////////////
       /////////////////////////////////////////////////////////////////////////////////////
        Servo_Y = 0.06;
        Servo_X = 0.075;
        Servo_z_smooth = 0.09;
        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.06;
        Servo_X = 0.075;
        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.06;
        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(2550 , 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.06;
        Servo_X = 0.05;
        Servo_z_smooth = 0.05;
        LED_All_Colour(255255 , 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.06;
        Servo_X = 0.09;
        Servo_z_smooth = 0.05;
        LED_All_Colour(00 , 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.06;
        Servo_X = 0.05;
        Servo_z_smooth = 0.05;
        LED_All_Colour(0255 , 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.06;
        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.06;
        Servo_X = 0.05;
        Servo_z_smooth = 0.05;
        thread_smooth_fade.signal_set(0x2);
        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.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.04;
        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.09;
        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.04;
        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.9;
        Servo_z_smooth = 0.03;
        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.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.04;
        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.08;
        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.04;
        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(1000);                          
        Servo_Y = 0.09;
        Servo_X = 0.075;
        Servo_z_smooth = 0.03;
        LED_All_Colour(00 , 0 , 255);  
        printf("Servo X: %f1.4 Servo Y: %f1.4 Servo Z: %f1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
        Thread::wait(30000);
        Thread::signal_wait(0x1);   //wait for flag to be set
        Thread::wait(1000);
        }
     
} 
 
void Red_Blue_Flash(int h)
{
   for(int i = 0;i<=h;i++)
        {
            
            LED_All_Colour(2550 ,0 , 50);
            wait(0.02);
            LED_All_Colour(2550 ,0 , 255);
            wait(0.02);
            LED_All_Colour(00 , 255 , 50);
            wait(0.02);            
            LED_All_Colour(00 , 255 , 255);
            wait(0.02);
        }   
}
 
 
    
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)
    
}

A small demonstration was carried out using both the lamp bot and BENGiE for an aesthetically pleasing show, this can be viewed below.




Below we see the final sonar sensor to servo demonstration:


Final sonar to servo demo

By Donal Murphy.


Keep Moving Forward


No comments:

Post a Comment