Donal / Mbed 2 deprecated Lamp_04

thread working with sonar and demo

Dependencies:   C12832 PixelArray RangeFinder WS2812 mbed-rtos mbed

main.cpp@1:6ce5517b28b8, 2015-04-27 (annotated)

Committer:
cathal66
Date:
Mon Apr 27 14:26:42 2015 +0000
Revision:
1:6ce5517b28b8
Parent:
0:fb0c24069b95 
All working. but without serial comms

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cathal66 0:fb0c24069b95 1 #include "mbed.h"
cathal66 0:fb0c24069b95 2 #include "rtos.h"
cathal66 0:fb0c24069b95 3 #include "C12832.h"
cathal66 0:fb0c24069b95 4 #include "RangeFinder.h"
cathal66 0:fb0c24069b95 5 #include "WS2812.h"
cathal66 0:fb0c24069b95 6 #include "PixelArray.h"
cathal66 0:fb0c24069b95 7
cathal66 0:fb0c24069b95 8 //FINITE STATE MACHINE EVENTS
cathal66 0:fb0c24069b95 9 #define TIME_OUT 0
cathal66 0:fb0c24069b95 10 #define TOO_CLOSE 1
cathal66 0:fb0c24069b95 11 #define TOO_FAR 2
cathal66 0:fb0c24069b95 12 #define DETECT 3
cathal66 0:fb0c24069b95 13
cathal66 0:fb0c24069b95 14 //STATES
cathal66 0:fb0c24069b95 15 #define SCAN_FACE 0
cathal66 0:fb0c24069b95 16 #define PAN_TILT 1
cathal66 0:fb0c24069b95 17 #define LOCK_DOWN 2
cathal66 0:fb0c24069b95 18
cathal66 0:fb0c24069b95 19 #define WS2812_BUF 10
cathal66 0:fb0c24069b95 20 #define NUM_COLORS 1
cathal66 0:fb0c24069b95 21 #define NUM_LEDS_PER_COLOR 10
cathal66 0:fb0c24069b95 22 PixelArray px(WS2812_BUF);
cathal66 0:fb0c24069b95 23
cathal66 0:fb0c24069b95 24 // See the program page for information on the timing numbers
cathal66 0:fb0c24069b95 25 // The given numbers are for the K64F
cathal66 0:fb0c24069b95 26 WS2812 ws(p9, WS2812_BUF, 5, 10, 10, 15);
cathal66 0:fb0c24069b95 27
cathal66 0:fb0c24069b95 28 //pass event via message queue
cathal66 0:fb0c24069b95 29 typedef struct {
cathal66 0:fb0c24069b95 30 int event; /* AD result of measured voltage */
cathal66 0:fb0c24069b95 31 } message_t;
cathal66 0:fb0c24069b95 32
cathal66 0:fb0c24069b95 33 MemoryPool<message_t, 16> mpool;
cathal66 0:fb0c24069b95 34 Queue<message_t, 16> queue;
cathal66 0:fb0c24069b95 35
cathal66 0:fb0c24069b95 36 PwmOut Servo_Y(p22);
cathal66 0:fb0c24069b95 37 PwmOut Servo_X(p21);
cathal66 0:fb0c24069b95 38 PwmOut Servo_Z(p23);
cathal66 0:fb0c24069b95 39
cathal66 0:fb0c24069b95 40 //SONAR_SENSOR INPUT
cathal66 0:fb0c24069b95 41 RangeFinder ping_sensor(p24, 5, 5800.0, 100000);
cathal66 0:fb0c24069b95 42
cathal66 0:fb0c24069b95 43 //local display]
cathal66 0:fb0c24069b95 44 C12832 lcd(p5, p7, p6, p8, p11);
cathal66 0:fb0c24069b95 45
cathal66 0:fb0c24069b95 46 //leds for debug
cathal66 0:fb0c24069b95 47 DigitalOut led(LED1); //button press
cathal66 0:fb0c24069b95 48 DigitalOut led2(LED2); //fsm thread
cathal66 0:fb0c24069b95 49 DigitalOut led3(LED3);
cathal66 0:fb0c24069b95 50 DigitalOut led4(LED4); //timeout thread
cathal66 0:fb0c24069b95 51
cathal66 0:fb0c24069b95 52 //Mutex
cathal66 0:fb0c24069b95 53 Mutex Distance_Mutex;
cathal66 0:fb0c24069b95 54
cathal66 0:fb0c24069b95 55 //Gobal Var
cathal66 0:fb0c24069b95 56 float Servo_z_smooth;
cathal66 0:fb0c24069b95 57 float Distance= 0 ;
cathal66 0:fb0c24069b95 58
cathal66 0:fb0c24069b95 59
cathal66 0:fb0c24069b95 60 void timeout_event(void const *n)
cathal66 0:fb0c24069b95 61 {
cathal66 0:fb0c24069b95 62 message_t *message = mpool.alloc();
cathal66 0:fb0c24069b95 63 message->event = TIME_OUT;
cathal66 0:fb0c24069b95 64 queue.put(message);
cathal66 0:fb0c24069b95 65
cathal66 0:fb0c24069b95 66 led = !led;
cathal66 0:fb0c24069b95 67 }
cathal66 0:fb0c24069b95 68
cathal66 0:fb0c24069b95 69
cathal66 0:fb0c24069b95 70 void read_sonar_thread(void const *argument)
cathal66 0:fb0c24069b95 71 {
cathal66 0:fb0c24069b95 72
cathal66 0:fb0c24069b95 73 while (true)
cathal66 0:fb0c24069b95 74 {
cathal66 0:fb0c24069b95 75 Distance_Mutex.lock();
cathal66 0:fb0c24069b95 76 Distance = ping_sensor.read_m();
cathal66 0:fb0c24069b95 77 Distance_Mutex.unlock();
cathal66 1:6ce5517b28b8 78 Thread::wait(100);
cathal66 0:fb0c24069b95 79 }
cathal66 0:fb0c24069b95 80 }
cathal66 0:fb0c24069b95 81
cathal66 0:fb0c24069b95 82 void too_close_event_thread(void const *argument)
cathal66 0:fb0c24069b95 83 {
cathal66 0:fb0c24069b95 84 float dist_thread = 0;
cathal66 0:fb0c24069b95 85 while (true)
cathal66 0:fb0c24069b95 86 {
cathal66 0:fb0c24069b95 87 Distance_Mutex.lock();
cathal66 0:fb0c24069b95 88 dist_thread = Distance;
cathal66 0:fb0c24069b95 89 Distance_Mutex.unlock();
cathal66 0:fb0c24069b95 90
cathal66 0:fb0c24069b95 91 if (dist_thread < 0.25)
cathal66 0:fb0c24069b95 92 {
cathal66 0:fb0c24069b95 93 //event via a message queue
cathal66 0:fb0c24069b95 94 message_t *message = mpool.alloc();
cathal66 0:fb0c24069b95 95 message->event = TOO_CLOSE;
cathal66 0:fb0c24069b95 96 queue.put(message);
cathal66 0:fb0c24069b95 97
cathal66 0:fb0c24069b95 98 led2 = !led2;
cathal66 0:fb0c24069b95 99 }
cathal66 0:fb0c24069b95 100 Thread::wait(250);
cathal66 0:fb0c24069b95 101
cathal66 0:fb0c24069b95 102 }
cathal66 0:fb0c24069b95 103 }
cathal66 0:fb0c24069b95 104
cathal66 0:fb0c24069b95 105 void detect_event_thread(void const *argument)
cathal66 0:fb0c24069b95 106 {
cathal66 0:fb0c24069b95 107 float dist_thread = 0;
cathal66 0:fb0c24069b95 108 while (true)
cathal66 0:fb0c24069b95 109 {
cathal66 0:fb0c24069b95 110 Distance_Mutex.lock();
cathal66 0:fb0c24069b95 111 dist_thread = Distance;
cathal66 0:fb0c24069b95 112 Distance_Mutex.unlock();
cathal66 0:fb0c24069b95 113
cathal66 0:fb0c24069b95 114 if(dist_thread > 0.25 & dist_thread < 2)
cathal66 0:fb0c24069b95 115 {
cathal66 0:fb0c24069b95 116 //event via a message queue
cathal66 0:fb0c24069b95 117 message_t *message = mpool.alloc();
cathal66 0:fb0c24069b95 118 message->event = DETECT;
cathal66 0:fb0c24069b95 119 queue.put(message);
cathal66 0:fb0c24069b95 120
cathal66 0:fb0c24069b95 121 led3= !led3;
cathal66 0:fb0c24069b95 122 }
cathal66 0:fb0c24069b95 123 Thread::wait(250);
cathal66 0:fb0c24069b95 124 }
cathal66 0:fb0c24069b95 125 }
cathal66 0:fb0c24069b95 126
cathal66 0:fb0c24069b95 127 void too_far_event_thread(void const *argument)
cathal66 0:fb0c24069b95 128 {
cathal66 0:fb0c24069b95 129 float dist_thread = 0;
cathal66 0:fb0c24069b95 130 while (true)
cathal66 0:fb0c24069b95 131 {
cathal66 0:fb0c24069b95 132 Distance_Mutex.lock();
cathal66 0:fb0c24069b95 133 dist_thread = Distance;
cathal66 0:fb0c24069b95 134 Distance_Mutex.unlock();
cathal66 0:fb0c24069b95 135
cathal66 0:fb0c24069b95 136 if (dist_thread > 3)
cathal66 0:fb0c24069b95 137 {
cathal66 0:fb0c24069b95 138 //event via a message queue
cathal66 0:fb0c24069b95 139 message_t *message = mpool.alloc();
cathal66 0:fb0c24069b95 140 message->event = TOO_FAR;
cathal66 0:fb0c24069b95 141 queue.put(message);
cathal66 0:fb0c24069b95 142
cathal66 0:fb0c24069b95 143 led4 = !led4;
cathal66 0:fb0c24069b95 144 }
cathal66 0:fb0c24069b95 145 Thread::wait(250);
cathal66 0:fb0c24069b95 146 }
cathal66 0:fb0c24069b95 147 }
cathal66 0:fb0c24069b95 148
cathal66 0:fb0c24069b95 149
cathal66 0:fb0c24069b95 150
cathal66 0:fb0c24069b95 151 void Servo_Z_Thread(void const *args) {
cathal66 0:fb0c24069b95 152 float Sevro_Pos;
cathal66 0:fb0c24069b95 153 float Sevro_Pos_New = 0.1;
cathal66 0:fb0c24069b95 154 float Sevro_Pos_Old = 0.01;
cathal66 0:fb0c24069b95 155
cathal66 0:fb0c24069b95 156 while (true) {
cathal66 0:fb0c24069b95 157
cathal66 0:fb0c24069b95 158 Sevro_Pos_New = Servo_z_smooth;
cathal66 0:fb0c24069b95 159
cathal66 0:fb0c24069b95 160 if(Sevro_Pos_Old <= Sevro_Pos_New)
cathal66 0:fb0c24069b95 161 {
cathal66 0:fb0c24069b95 162 for(float i=Sevro_Pos_Old*10000; i<=Sevro_Pos_New*10000; i=i+25)
cathal66 0:fb0c24069b95 163 {
cathal66 0:fb0c24069b95 164 Sevro_Pos = i/10000;
cathal66 0:fb0c24069b95 165 Servo_Z = Sevro_Pos;
cathal66 0:fb0c24069b95 166 printf("Servo_Z1: %1.6f %1.6f %1.6f\n\r ",Sevro_Pos, i,Sevro_Pos_Old);
cathal66 0:fb0c24069b95 167 //Thread::wait(50);
cathal66 0:fb0c24069b95 168 }
cathal66 0:fb0c24069b95 169 }
cathal66 0:fb0c24069b95 170 else
cathal66 0:fb0c24069b95 171 {
cathal66 0:fb0c24069b95 172 for(float i=Sevro_Pos_Old*10000; i>=Sevro_Pos_New*10000; i=i-25)
cathal66 0:fb0c24069b95 173 {
cathal66 0:fb0c24069b95 174 Sevro_Pos = i/10000;
cathal66 0:fb0c24069b95 175 Servo_Z = Sevro_Pos;
cathal66 0:fb0c24069b95 176 printf("Servo_Z2: %1.6f \n\r",Sevro_Pos);
cathal66 0:fb0c24069b95 177 //Thread::wait(50);
cathal66 0:fb0c24069b95 178 }
cathal66 0:fb0c24069b95 179 }
cathal66 0:fb0c24069b95 180 Sevro_Pos_Old = Sevro_Pos_New;
cathal66 0:fb0c24069b95 181 Thread::wait(500);
cathal66 0:fb0c24069b95 182 }
cathal66 0:fb0c24069b95 183 }
cathal66 0:fb0c24069b95 184
cathal66 0:fb0c24069b95 185
cathal66 0:fb0c24069b95 186
cathal66 0:fb0c24069b95 187 void LED_All_Colour(int red , int green ,int blue , int bright)
cathal66 0:fb0c24069b95 188 {
cathal66 0:fb0c24069b95 189 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
cathal66 0:fb0c24069b95 190
cathal66 0:fb0c24069b95 191 int colorbuf[NUM_COLORS] = {0x000000};
cathal66 0:fb0c24069b95 192 //int colorbuf[NUM_COLORS] = {0x000000,0x00f0ff,0x00ff00,0x00ffff,0xffffff,0x00ff00,0x00ffff,0x0000ff,0xff00ff};
cathal66 0:fb0c24069b95 193
cathal66 0:fb0c24069b95 194 red <<= 16;
cathal66 0:fb0c24069b95 195 green <<= 8;
cathal66 0:fb0c24069b95 196
cathal66 0:fb0c24069b95 197 colorbuf[0] = red + green + blue;
cathal66 0:fb0c24069b95 198
cathal66 0:fb0c24069b95 199 printf("Colour: %6x red:%6x Green:%6x Blue:%6x Bright:%x \n\r",colorbuf[0], red , green , blue, bright);
cathal66 0:fb0c24069b95 200 // for each of the colours (j) write out 10 of them
cathal66 0:fb0c24069b95 201 // the pixels are written at the colour*10, plus the colour position
cathal66 0:fb0c24069b95 202 // all modulus 60 so it wraps around
cathal66 0:fb0c24069b95 203 for (int i = 0; i < WS2812_BUF; i++) {
cathal66 0:fb0c24069b95 204 px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
cathal66 0:fb0c24069b95 205 }
cathal66 0:fb0c24069b95 206
cathal66 0:fb0c24069b95 207 // now all the colours are computed, add a fade effect using intensity scaling
cathal66 0:fb0c24069b95 208 // compute and write the II value for each pixel
cathal66 0:fb0c24069b95 209 for (int j=0; j<WS2812_BUF; j++) {
cathal66 0:fb0c24069b95 210 // px.SetI(pixel position, II value)
cathal66 0:fb0c24069b95 211 //px.SetI(j%WS2812_BUF, 0xff+(0xf*(j%NUM_LEDS_PER_COLOR))); //full brightness
cathal66 0:fb0c24069b95 212 px.SetI(j%WS2812_BUF, bright +(bright*(j%NUM_LEDS_PER_COLOR))); //control brightness
cathal66 0:fb0c24069b95 213 }
cathal66 0:fb0c24069b95 214 //set the colour of the LEDs
cathal66 0:fb0c24069b95 215 for (int z=WS2812_BUF; z >= 0 ; z--) {
cathal66 0:fb0c24069b95 216 ws.write_offsets(px.getBuf(),z,z,z);
cathal66 0:fb0c24069b95 217
cathal66 0:fb0c24069b95 218
cathal66 0:fb0c24069b95 219 }
cathal66 0:fb0c24069b95 220 }
cathal66 0:fb0c24069b95 221
cathal66 0:fb0c24069b95 222
cathal66 0:fb0c24069b95 223 void LED_Colour_Pos(int position, int red , int green ,int blue , int bright)
cathal66 0:fb0c24069b95 224 {
cathal66 0:fb0c24069b95 225 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
cathal66 0:fb0c24069b95 226
cathal66 0:fb0c24069b95 227 int colorbuf = 0x000000;
cathal66 0:fb0c24069b95 228
cathal66 0:fb0c24069b95 229 red <<= 16;
cathal66 0:fb0c24069b95 230 green <<= 8;
cathal66 0:fb0c24069b95 231
cathal66 0:fb0c24069b95 232 colorbuf = red + green + blue;
cathal66 0:fb0c24069b95 233
cathal66 0:fb0c24069b95 234 printf("Colour: %6x red:%6x Green:%6x Blue:%6x Bright:%x \n\r",colorbuf, red , green , blue, bright);
cathal66 0:fb0c24069b95 235 // for each of the colours (j) write out 10 of them
cathal66 0:fb0c24069b95 236 // the pixels are written at the colour*10, plus the colour position
cathal66 0:fb0c24069b95 237 // all modulus 60 so it wraps around
cathal66 0:fb0c24069b95 238 px.Set(position, colorbuf);
cathal66 0:fb0c24069b95 239
cathal66 0:fb0c24069b95 240 // now all the colours are computed, add a fade effect using intensity scaling
cathal66 0:fb0c24069b95 241 // compute and write the II value for each pixel
cathal66 0:fb0c24069b95 242 //px.SetI(pixel position, II value)
cathal66 0:fb0c24069b95 243 px.SetI(position, bright); //control brightness
cathal66 0:fb0c24069b95 244 }
cathal66 0:fb0c24069b95 245
cathal66 0:fb0c24069b95 246 void LED_Colour_Pos_Set()
cathal66 0:fb0c24069b95 247 {
cathal66 0:fb0c24069b95 248 //set the colour of the LEDs
cathal66 0:fb0c24069b95 249 for (int z=WS2812_BUF-1; z >= 0 ; z--)
cathal66 0:fb0c24069b95 250 {
cathal66 0:fb0c24069b95 251 ws.write_offsets(px.getBuf(),z,z,z);
cathal66 0:fb0c24069b95 252 }
cathal66 0:fb0c24069b95 253 }
cathal66 0:fb0c24069b95 254
cathal66 1:6ce5517b28b8 255 void Smooth_Fade_Thread(void const *args)
cathal66 1:6ce5517b28b8 256 {
cathal66 1:6ce5517b28b8 257 int speed = 10;
cathal66 1:6ce5517b28b8 258 int h = 1;
cathal66 1:6ce5517b28b8 259 while(1)
cathal66 1:6ce5517b28b8 260 {
cathal66 1:6ce5517b28b8 261 Thread::signal_wait(0x2); //wait for flag to be set
cathal66 1:6ce5517b28b8 262 for(int j = 0; j<=h;j++)
cathal66 1:6ce5517b28b8 263 {
cathal66 1:6ce5517b28b8 264 for(int i = 0;i<=255;i=i+speed)
cathal66 1:6ce5517b28b8 265 {
cathal66 1:6ce5517b28b8 266 LED_All_Colour(i, 0 ,255-i , 255);
cathal66 1:6ce5517b28b8 267 //wait(0.01);
cathal66 1:6ce5517b28b8 268 }
cathal66 1:6ce5517b28b8 269 for(int i = 0;i<=255;i=i+speed)
cathal66 1:6ce5517b28b8 270 {
cathal66 1:6ce5517b28b8 271 LED_All_Colour(255-i, i ,0 , 255);
cathal66 1:6ce5517b28b8 272 //wait(0.01);
cathal66 1:6ce5517b28b8 273 }
cathal66 1:6ce5517b28b8 274 for(int i = 0;i<=255;i=i+speed)
cathal66 1:6ce5517b28b8 275 {
cathal66 1:6ce5517b28b8 276 LED_All_Colour(0, 255-i ,i , 255);
cathal66 1:6ce5517b28b8 277 //wait(0.01);
cathal66 1:6ce5517b28b8 278 }
cathal66 1:6ce5517b28b8 279 }
cathal66 1:6ce5517b28b8 280 }
cathal66 1:6ce5517b28b8 281 }
cathal66 0:fb0c24069b95 282
cathal66 0:fb0c24069b95 283 void Smooth_Fade(int h , int speed)
cathal66 0:fb0c24069b95 284 {
cathal66 1:6ce5517b28b8 285
cathal66 0:fb0c24069b95 286 for(int j = 0; j<=h;j++)
cathal66 0:fb0c24069b95 287 {
cathal66 0:fb0c24069b95 288 for(int i = 0;i<=255;i=i+speed)
cathal66 0:fb0c24069b95 289 {
cathal66 0:fb0c24069b95 290 LED_All_Colour(i, 0 ,255-i , 255);
cathal66 0:fb0c24069b95 291 //wait(0.01);
cathal66 0:fb0c24069b95 292 }
cathal66 0:fb0c24069b95 293 for(int i = 0;i<=255;i=i+speed)
cathal66 0:fb0c24069b95 294 {
cathal66 0:fb0c24069b95 295 LED_All_Colour(255-i, i ,0 , 255);
cathal66 0:fb0c24069b95 296 //wait(0.01);
cathal66 0:fb0c24069b95 297 }
cathal66 0:fb0c24069b95 298 for(int i = 0;i<=255;i=i+speed)
cathal66 0:fb0c24069b95 299 {
cathal66 0:fb0c24069b95 300 LED_All_Colour(0, 255-i ,i , 255);
cathal66 0:fb0c24069b95 301 //wait(0.01);
cathal66 0:fb0c24069b95 302 }
cathal66 1:6ce5517b28b8 303 }
cathal66 1:6ce5517b28b8 304
cathal66 0:fb0c24069b95 305 }
cathal66 0:fb0c24069b95 306
cathal66 0:fb0c24069b95 307 void Demo_thread(void const *argument) {
cathal66 1:6ce5517b28b8 308
cathal66 1:6ce5517b28b8 309 Thread thread_smooth_fade(Smooth_Fade_Thread);
cathal66 1:6ce5517b28b8 310
cathal66 0:fb0c24069b95 311 while (true)
cathal66 0:fb0c24069b95 312 {
cathal66 0:fb0c24069b95 313 Thread::signal_wait(0x1); //wait for flag to be set
cathal66 0:fb0c24069b95 314
cathal66 0:fb0c24069b95 315 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 316 //////////////////////////////////// X //////////////////////////////////////////
cathal66 0:fb0c24069b95 317 /////////////////////////////////////////////////////////////////////////////////////
cathal66 1:6ce5517b28b8 318 Servo_Y = 0.06;
cathal66 1:6ce5517b28b8 319 Servo_X = 0.075;
cathal66 1:6ce5517b28b8 320 Servo_z_smooth = 0.09;
cathal66 0:fb0c24069b95 321 LED_Colour_Pos(0,255,0,0,128);
cathal66 0:fb0c24069b95 322 LED_Colour_Pos(1,0,255,0,128);
cathal66 0:fb0c24069b95 323 LED_Colour_Pos(2,0,0,255,128);
cathal66 0:fb0c24069b95 324 LED_Colour_Pos(3,255,255,0,128);
cathal66 0:fb0c24069b95 325 LED_Colour_Pos(4,255,0,255,128);
cathal66 0:fb0c24069b95 326 LED_Colour_Pos(5,128,255,0,128);
cathal66 0:fb0c24069b95 327 LED_Colour_Pos(6,128,50,0,255);
cathal66 0:fb0c24069b95 328 LED_Colour_Pos(7,0,255,128,128);
cathal66 0:fb0c24069b95 329 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:fb0c24069b95 330 LED_Colour_Pos(9,128,128,255,128);
cathal66 0:fb0c24069b95 331 LED_Colour_Pos_Set();
cathal66 0:fb0c24069b95 332 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);
cathal66 1:6ce5517b28b8 333 Thread::wait(500);
cathal66 1:6ce5517b28b8 334 Servo_Y = 0.06;
cathal66 1:6ce5517b28b8 335 Servo_X = 0.075;
cathal66 0:fb0c24069b95 336 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 337 LED_Colour_Pos(0,128,255,0,128);
cathal66 0:fb0c24069b95 338 LED_Colour_Pos(1,128,50,0,255);
cathal66 0:fb0c24069b95 339 LED_Colour_Pos(2,0,255,128,128);
cathal66 0:fb0c24069b95 340 LED_Colour_Pos(3,128,0,128,255);
cathal66 0:fb0c24069b95 341 LED_Colour_Pos(4,128,128,255,128);
cathal66 0:fb0c24069b95 342 LED_Colour_Pos(5,255,255,0,255);
cathal66 0:fb0c24069b95 343 LED_Colour_Pos(6,128,50,127,255);
cathal66 0:fb0c24069b95 344 LED_Colour_Pos(7,255,255,0,128);
cathal66 0:fb0c24069b95 345 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:fb0c24069b95 346 LED_Colour_Pos(9,0,128,255,128);
cathal66 0:fb0c24069b95 347 LED_Colour_Pos_Set();
cathal66 0:fb0c24069b95 348 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);
cathal66 0:fb0c24069b95 349 Thread::wait(500);
cathal66 1:6ce5517b28b8 350 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 351 Servo_X = 0.08;
cathal66 0:fb0c24069b95 352 Servo_z_smooth = 0.08;
cathal66 0:fb0c24069b95 353 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);
cathal66 0:fb0c24069b95 354 Thread::wait(500);
cathal66 0:fb0c24069b95 355 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 356 Servo_X = 0.05;
cathal66 0:fb0c24069b95 357 Servo_z_smooth = 0.1;
cathal66 0:fb0c24069b95 358 LED_All_Colour(255, 0 , 0 , 255);
cathal66 0:fb0c24069b95 359 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);
cathal66 0:fb0c24069b95 360 Thread::wait(500);
cathal66 1:6ce5517b28b8 361 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 362 Servo_X = 0.05;
cathal66 0:fb0c24069b95 363 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 364 LED_All_Colour(255, 255 , 0 , 255);
cathal66 0:fb0c24069b95 365 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);
cathal66 0:fb0c24069b95 366 Thread::wait(500);
cathal66 1:6ce5517b28b8 367 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 368 Servo_X = 0.09;
cathal66 0:fb0c24069b95 369 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 370 LED_All_Colour(0, 0 , 255 , 255);
cathal66 0:fb0c24069b95 371 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);
cathal66 0:fb0c24069b95 372 Thread::wait(500);
cathal66 1:6ce5517b28b8 373 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 374 Servo_X = 0.05;
cathal66 0:fb0c24069b95 375 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 376 LED_All_Colour(0, 255 , 0 , 255);
cathal66 0:fb0c24069b95 377 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);
cathal66 0:fb0c24069b95 378 Thread::wait(500);
cathal66 1:6ce5517b28b8 379 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 380 Servo_X = 0.09;
cathal66 0:fb0c24069b95 381 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 382 LED_Colour_Pos(0,255,0,0,128);
cathal66 0:fb0c24069b95 383 LED_Colour_Pos(1,0,255,0,128);
cathal66 0:fb0c24069b95 384 LED_Colour_Pos(2,0,0,255,128);
cathal66 0:fb0c24069b95 385 LED_Colour_Pos(3,255,255,0,128);
cathal66 0:fb0c24069b95 386 LED_Colour_Pos(4,255,0,255,128);
cathal66 0:fb0c24069b95 387 LED_Colour_Pos(5,128,255,0,128);
cathal66 0:fb0c24069b95 388 LED_Colour_Pos(6,128,50,0,255);
cathal66 0:fb0c24069b95 389 LED_Colour_Pos(7,0,255,128,128);
cathal66 0:fb0c24069b95 390 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:fb0c24069b95 391 LED_Colour_Pos(9,128,128,255,128);
cathal66 0:fb0c24069b95 392 LED_Colour_Pos_Set();
cathal66 0:fb0c24069b95 393 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);
cathal66 0:fb0c24069b95 394 Thread::wait(500);
cathal66 1:6ce5517b28b8 395 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 396 Servo_X = 0.05;
cathal66 0:fb0c24069b95 397 Servo_z_smooth = 0.05;
cathal66 1:6ce5517b28b8 398 thread_smooth_fade.signal_set(0x2);
cathal66 0:fb0c24069b95 399 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);
cathal66 0:fb0c24069b95 400 Thread::wait(500);
cathal66 1:6ce5517b28b8 401 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 402 Servo_X = 0.08;
cathal66 0:fb0c24069b95 403 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 404 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);
cathal66 0:fb0c24069b95 405 Thread::wait(500);
cathal66 0:fb0c24069b95 406
cathal66 0:fb0c24069b95 407 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 408 //////////////////////////////////// y //////////////////////////////////////////
cathal66 0:fb0c24069b95 409 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 410 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 411 Servo_X = 0.08;
cathal66 0:fb0c24069b95 412 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 413 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);
cathal66 0:fb0c24069b95 414 Thread::wait(500);
cathal66 0:fb0c24069b95 415 Servo_Y = 0.08;
cathal66 1:6ce5517b28b8 416 Servo_X = 0.04;
cathal66 0:fb0c24069b95 417 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 418 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);
cathal66 0:fb0c24069b95 419 Thread::wait(500);
cathal66 0:fb0c24069b95 420 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 421 Servo_X = 0.075;
cathal66 0:fb0c24069b95 422 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 423 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);
cathal66 0:fb0c24069b95 424 Thread::wait(500);
cathal66 0:fb0c24069b95 425 Servo_Y = 0.065;
cathal66 1:6ce5517b28b8 426 Servo_X = 0.09;
cathal66 0:fb0c24069b95 427 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 428 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);
cathal66 0:fb0c24069b95 429 Thread::wait(1000);
cathal66 0:fb0c24069b95 430 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 431 Servo_X = 0.075;
cathal66 0:fb0c24069b95 432 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 433 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);
cathal66 0:fb0c24069b95 434 Thread::wait(500);
cathal66 0:fb0c24069b95 435 Servo_Y = 0.065;
cathal66 0:fb0c24069b95 436 Servo_X = 0.075;
cathal66 0:fb0c24069b95 437 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 438 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);
cathal66 0:fb0c24069b95 439 Thread::wait(500);
cathal66 0:fb0c24069b95 440 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 441 Servo_X = 0.075;
cathal66 0:fb0c24069b95 442 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 443 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);
cathal66 0:fb0c24069b95 444 Thread::wait(500);
cathal66 0:fb0c24069b95 445 Servo_Y = 0.065;
cathal66 1:6ce5517b28b8 446 Servo_X = 0.04;
cathal66 0:fb0c24069b95 447 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 448 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);
cathal66 0:fb0c24069b95 449 Thread::wait(500);
cathal66 0:fb0c24069b95 450 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 451 Servo_X = 0.075;
cathal66 0:fb0c24069b95 452 Servo_z_smooth = 0.09;
cathal66 0:fb0c24069b95 453 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);
cathal66 0:fb0c24069b95 454 Thread::wait(750);
cathal66 0:fb0c24069b95 455
cathal66 0:fb0c24069b95 456 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 457 //////////////////////////////////// Z //////////////////////////////////////////
cathal66 0:fb0c24069b95 458 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 459 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 460 Servo_X = 0.075;
cathal66 0:fb0c24069b95 461 Servo_z_smooth = 0.08;
cathal66 0:fb0c24069b95 462 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);
cathal66 0:fb0c24069b95 463 Thread::wait(500);
cathal66 0:fb0c24069b95 464 Servo_Y = 0.06;
cathal66 1:6ce5517b28b8 465 Servo_X = 0.9;
cathal66 1:6ce5517b28b8 466 Servo_z_smooth = 0.03;
cathal66 0:fb0c24069b95 467 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);
cathal66 1:6ce5517b28b8 468 Thread::wait(1000);
cathal66 0:fb0c24069b95 469 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 470 Servo_X = 0.075;
cathal66 0:fb0c24069b95 471 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 472 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);
cathal66 0:fb0c24069b95 473 Thread::wait(500);
cathal66 0:fb0c24069b95 474 Servo_Y = 0.06;
cathal66 1:6ce5517b28b8 475 Servo_X = 0.04;
cathal66 0:fb0c24069b95 476 Servo_z_smooth = 0.11;
cathal66 0:fb0c24069b95 477 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);
cathal66 0:fb0c24069b95 478 Thread::wait(750);
cathal66 0:fb0c24069b95 479 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 480 Servo_X = 0.075;
cathal66 0:fb0c24069b95 481 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 482 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);
cathal66 0:fb0c24069b95 483 Thread::wait(750);
cathal66 0:fb0c24069b95 484 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 485 Servo_X = 0.075;
cathal66 0:fb0c24069b95 486 Servo_z_smooth = 0.11;
cathal66 0:fb0c24069b95 487 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);
cathal66 0:fb0c24069b95 488 Thread::wait(750);
cathal66 0:fb0c24069b95 489 Servo_Y = 0.06;
cathal66 1:6ce5517b28b8 490 Servo_X = 0.08;
cathal66 0:fb0c24069b95 491 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 492 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);
cathal66 0:fb0c24069b95 493 Thread::wait(750);
cathal66 0:fb0c24069b95 494 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 495 Servo_X = 0.075;
cathal66 0:fb0c24069b95 496 Servo_z_smooth = 0.11;
cathal66 0:fb0c24069b95 497 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);
cathal66 0:fb0c24069b95 498 Thread::wait(750);
cathal66 0:fb0c24069b95 499 Servo_Y = 0.06;
cathal66 1:6ce5517b28b8 500 Servo_X = 0.04;
cathal66 0:fb0c24069b95 501 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 502 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);
cathal66 1:6ce5517b28b8 503 Thread::wait(1000);
cathal66 1:6ce5517b28b8 504 Servo_Y = 0.09;
cathal66 1:6ce5517b28b8 505 Servo_X = 0.075;
cathal66 0:fb0c24069b95 506 Servo_z_smooth = 0.03;
cathal66 1:6ce5517b28b8 507 LED_All_Colour(0, 0 , 0 , 255);
cathal66 0:fb0c24069b95 508 printf("Servo X: %f1.4 Servo Y: %f1.4 Servo Z: %f1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 1:6ce5517b28b8 509 Thread::wait(30000);
cathal66 1:6ce5517b28b8 510 Thread::signal_wait(0x1); //wait for flag to be set
cathal66 1:6ce5517b28b8 511 Thread::wait(1000);
cathal66 0:fb0c24069b95 512 }
cathal66 0:fb0c24069b95 513
cathal66 0:fb0c24069b95 514 }
cathal66 0:fb0c24069b95 515
cathal66 0:fb0c24069b95 516 void Red_Blue_Flash(int h)
cathal66 0:fb0c24069b95 517 {
cathal66 0:fb0c24069b95 518 for(int i = 0;i<=h;i++)
cathal66 0:fb0c24069b95 519 {
cathal66 0:fb0c24069b95 520
cathal66 0:fb0c24069b95 521 LED_All_Colour(255, 0 ,0 , 50);
cathal66 0:fb0c24069b95 522 wait(0.02);
cathal66 0:fb0c24069b95 523 LED_All_Colour(255, 0 ,0 , 255);
cathal66 0:fb0c24069b95 524 wait(0.02);
cathal66 0:fb0c24069b95 525 LED_All_Colour(0, 0 , 255 , 50);
cathal66 0:fb0c24069b95 526 wait(0.02);
cathal66 0:fb0c24069b95 527 LED_All_Colour(0, 0 , 255 , 255);
cathal66 0:fb0c24069b95 528 wait(0.02);
cathal66 0:fb0c24069b95 529 }
cathal66 0:fb0c24069b95 530 }
cathal66 0:fb0c24069b95 531
cathal66 1:6ce5517b28b8 532
cathal66 1:6ce5517b28b8 533
cathal66 0:fb0c24069b95 534 int main (void)
cathal66 0:fb0c24069b95 535 {
cathal66 0:fb0c24069b95 536
cathal66 0:fb0c24069b95 537 //Thread fsm(fsm_thread);
cathal66 0:fb0c24069b95 538 Thread too_far_event(too_far_event_thread);
cathal66 0:fb0c24069b95 539 Thread too_close_event(too_close_event_thread);
cathal66 0:fb0c24069b95 540 Thread detect_event(detect_event_thread);
cathal66 0:fb0c24069b95 541 Thread thread_Servo_Z(Servo_Z_Thread );
cathal66 0:fb0c24069b95 542 Thread thread_Demo(Demo_thread );
cathal66 0:fb0c24069b95 543 Thread thread_Sonar_Read(read_sonar_thread );
cathal66 1:6ce5517b28b8 544
cathal66 0:fb0c24069b95 545 //RtosTimer timer(timeout_event, osTimerPeriodic, (void *)0);
cathal66 0:fb0c24069b95 546
cathal66 0:fb0c24069b95 547 int state = PAN_TILT;
cathal66 0:fb0c24069b95 548
cathal66 0:fb0c24069b95 549 //start timer with a 2 sec timeout
cathal66 0:fb0c24069b95 550 //timer.start(2000);
cathal66 0:fb0c24069b95 551
cathal66 0:fb0c24069b95 552 while(0)
cathal66 0:fb0c24069b95 553 {
cathal66 0:fb0c24069b95 554 osEvent evt = queue.get();
cathal66 0:fb0c24069b95 555 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 556 {
cathal66 0:fb0c24069b95 557 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 558 mpool.free(message);
cathal66 0:fb0c24069b95 559 }
cathal66 0:fb0c24069b95 560 }
cathal66 0:fb0c24069b95 561
cathal66 0:fb0c24069b95 562 while (true)
cathal66 0:fb0c24069b95 563 {
cathal66 0:fb0c24069b95 564
cathal66 0:fb0c24069b95 565 switch(state) // locked
cathal66 0:fb0c24069b95 566 {
cathal66 0:fb0c24069b95 567
cathal66 0:fb0c24069b95 568 case SCAN_FACE:
cathal66 0:fb0c24069b95 569
cathal66 0:fb0c24069b95 570 osEvent evt = queue.get();
cathal66 0:fb0c24069b95 571 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 572 {
cathal66 0:fb0c24069b95 573 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 574
cathal66 0:fb0c24069b95 575 if(message->event == DETECT)
cathal66 0:fb0c24069b95 576 {
cathal66 0:fb0c24069b95 577 //next state
cathal66 0:fb0c24069b95 578 state = SCAN_FACE;
cathal66 0:fb0c24069b95 579 lcd.cls();
cathal66 0:fb0c24069b95 580 lcd.locate(0,2);
cathal66 0:fb0c24069b95 581 lcd.printf("state scan face - detect");
cathal66 0:fb0c24069b95 582
cathal66 0:fb0c24069b95 583 }
cathal66 0:fb0c24069b95 584
cathal66 0:fb0c24069b95 585
cathal66 0:fb0c24069b95 586 if(message->event == TOO_CLOSE)
cathal66 0:fb0c24069b95 587 {
cathal66 0:fb0c24069b95 588 //next state
cathal66 0:fb0c24069b95 589 state = LOCK_DOWN;
cathal66 0:fb0c24069b95 590 lcd.cls();
cathal66 0:fb0c24069b95 591 lcd.locate(0,2);
cathal66 0:fb0c24069b95 592 lcd.printf("state scan face - too close");
cathal66 0:fb0c24069b95 593
cathal66 0:fb0c24069b95 594 }
cathal66 0:fb0c24069b95 595
cathal66 0:fb0c24069b95 596
cathal66 0:fb0c24069b95 597 if(message->event == TOO_FAR)
cathal66 0:fb0c24069b95 598 {
cathal66 0:fb0c24069b95 599 state = PAN_TILT;
cathal66 0:fb0c24069b95 600 lcd.cls();
cathal66 0:fb0c24069b95 601 lcd.locate(0,2);
cathal66 0:fb0c24069b95 602 lcd.printf("state scan face - too-far");
cathal66 0:fb0c24069b95 603
cathal66 0:fb0c24069b95 604 }
cathal66 0:fb0c24069b95 605
cathal66 0:fb0c24069b95 606 mpool.free(message);
cathal66 0:fb0c24069b95 607
cathal66 0:fb0c24069b95 608 }
cathal66 0:fb0c24069b95 609
cathal66 0:fb0c24069b95 610 //timer.start(2000);
cathal66 0:fb0c24069b95 611
cathal66 0:fb0c24069b95 612 break;
cathal66 0:fb0c24069b95 613
cathal66 0:fb0c24069b95 614 case PAN_TILT:
cathal66 0:fb0c24069b95 615
cathal66 0:fb0c24069b95 616 //osEvent
cathal66 0:fb0c24069b95 617 evt = queue.get();
cathal66 0:fb0c24069b95 618 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 619 {
cathal66 0:fb0c24069b95 620 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 621
cathal66 0:fb0c24069b95 622 if(message->event == TOO_FAR)
cathal66 0:fb0c24069b95 623 {
cathal66 0:fb0c24069b95 624 //next state
cathal66 0:fb0c24069b95 625 state = PAN_TILT;
cathal66 0:fb0c24069b95 626 lcd.cls();
cathal66 0:fb0c24069b95 627 lcd.locate(0,2);
cathal66 0:fb0c24069b95 628 lcd.printf("state pan tilt - too-far");
cathal66 0:fb0c24069b95 629
cathal66 0:fb0c24069b95 630 }
cathal66 0:fb0c24069b95 631
cathal66 0:fb0c24069b95 632 if(message->event == DETECT)
cathal66 0:fb0c24069b95 633 {
cathal66 0:fb0c24069b95 634 //next state
cathal66 0:fb0c24069b95 635 state = PAN_TILT;
cathal66 0:fb0c24069b95 636 thread_Demo.signal_set(0x1);
cathal66 0:fb0c24069b95 637 lcd.cls();
cathal66 0:fb0c24069b95 638 lcd.locate(0,2);
cathal66 0:fb0c24069b95 639 lcd.printf("state pan tilt - detect");
cathal66 0:fb0c24069b95 640
cathal66 0:fb0c24069b95 641 }
cathal66 0:fb0c24069b95 642
cathal66 0:fb0c24069b95 643 if(message->event == TOO_CLOSE)
cathal66 0:fb0c24069b95 644 {
cathal66 0:fb0c24069b95 645 state = PAN_TILT;
cathal66 0:fb0c24069b95 646
cathal66 0:fb0c24069b95 647 lcd.cls();
cathal66 0:fb0c24069b95 648 lcd.locate(0,2);
cathal66 0:fb0c24069b95 649 lcd.printf("state pan tilt - too close");
cathal66 0:fb0c24069b95 650
cathal66 0:fb0c24069b95 651 }
cathal66 0:fb0c24069b95 652
cathal66 0:fb0c24069b95 653 mpool.free(message);
cathal66 0:fb0c24069b95 654 }
cathal66 0:fb0c24069b95 655
cathal66 0:fb0c24069b95 656 //timer.start(2000);
cathal66 0:fb0c24069b95 657
cathal66 0:fb0c24069b95 658 break;
cathal66 0:fb0c24069b95 659
cathal66 0:fb0c24069b95 660 case LOCK_DOWN:
cathal66 0:fb0c24069b95 661
cathal66 0:fb0c24069b95 662 evt = queue.get();
cathal66 0:fb0c24069b95 663 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 664 {
cathal66 0:fb0c24069b95 665 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 666
cathal66 0:fb0c24069b95 667 if(message->event == TOO_CLOSE)
cathal66 0:fb0c24069b95 668 {
cathal66 0:fb0c24069b95 669 state = LOCK_DOWN;
cathal66 0:fb0c24069b95 670
cathal66 0:fb0c24069b95 671 lcd.cls();
cathal66 0:fb0c24069b95 672 lcd.locate(0,2);
cathal66 0:fb0c24069b95 673 lcd.printf("state lock down - too close");
cathal66 0:fb0c24069b95 674 }
cathal66 0:fb0c24069b95 675
cathal66 0:fb0c24069b95 676 if(message->event == TIME_OUT)
cathal66 0:fb0c24069b95 677 {
cathal66 0:fb0c24069b95 678 state = PAN_TILT;
cathal66 0:fb0c24069b95 679
cathal66 0:fb0c24069b95 680 lcd.cls();
cathal66 0:fb0c24069b95 681 lcd.locate(0,2);
cathal66 0:fb0c24069b95 682 lcd.printf("state lock down - time out");
cathal66 0:fb0c24069b95 683 }
cathal66 0:fb0c24069b95 684
cathal66 0:fb0c24069b95 685 mpool.free(message);
cathal66 0:fb0c24069b95 686
cathal66 0:fb0c24069b95 687 }
cathal66 0:fb0c24069b95 688
cathal66 0:fb0c24069b95 689 //timer.start(2000);
cathal66 0:fb0c24069b95 690
cathal66 0:fb0c24069b95 691 break;
cathal66 0:fb0c24069b95 692
cathal66 0:fb0c24069b95 693 } //End of switch
cathal66 0:fb0c24069b95 694
cathal66 0:fb0c24069b95 695 //toggle led for local testing
cathal66 0:fb0c24069b95 696
cathal66 0:fb0c24069b95 697
cathal66 0:fb0c24069b95 698 } //end of while(1)
cathal66 0:fb0c24069b95 699
cathal66 0:fb0c24069b95 700 }