thread working with sonar and demo
Dependencies: C12832 PixelArray RangeFinder WS2812 mbed-rtos mbed
main.cpp
- Committer:
- cathal66
- Date:
- 2015-04-27
- Revision:
- 1:6ce5517b28b8
- Parent:
- 0:fb0c24069b95
File content as of revision 1:6ce5517b28b8:
#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(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(0, 255-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(0, 255-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(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.06; 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.06; 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.06; 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.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(0, 0 , 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(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); } } 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) }