Donal
thread working with sonar and demo
Dependencies: C12832 PixelArray RangeFinder WS2812 mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:fb0c24069b95
- Child:
- 1:6ce5517b28b8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 27 12:13:07 2015 +0000
@@ -0,0 +1,666 @@
+#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)
+
+}
\ No newline at end of file