Donal / Mbed 2 deprecated Lamp_04

thread working with sonar and demo

Dependencies:   C12832 PixelArray RangeFinder WS2812 mbed-rtos mbed

main.cpp@0:fb0c24069b95, 2015-04-27 (annotated)

Committer:
cathal66
Date:
Mon Apr 27 12:13:07 2015 +0000
Revision:
0:fb0c24069b95
Child:
1:6ce5517b28b8
Working with sonar and thread

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 0:fb0c24069b95 78 Thread::wait(250);
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 0:fb0c24069b95 255
cathal66 0:fb0c24069b95 256 void Smooth_Fade(int h , int speed)
cathal66 0:fb0c24069b95 257 {
cathal66 0:fb0c24069b95 258 for(int j = 0; j<=h;j++)
cathal66 0:fb0c24069b95 259 {
cathal66 0:fb0c24069b95 260 for(int i = 0;i<=255;i=i+speed)
cathal66 0:fb0c24069b95 261 {
cathal66 0:fb0c24069b95 262 LED_All_Colour(i, 0 ,255-i , 255);
cathal66 0:fb0c24069b95 263 //wait(0.01);
cathal66 0:fb0c24069b95 264 }
cathal66 0:fb0c24069b95 265 for(int i = 0;i<=255;i=i+speed)
cathal66 0:fb0c24069b95 266 {
cathal66 0:fb0c24069b95 267 LED_All_Colour(255-i, i ,0 , 255);
cathal66 0:fb0c24069b95 268 //wait(0.01);
cathal66 0:fb0c24069b95 269 }
cathal66 0:fb0c24069b95 270 for(int i = 0;i<=255;i=i+speed)
cathal66 0:fb0c24069b95 271 {
cathal66 0:fb0c24069b95 272 LED_All_Colour(0, 255-i ,i , 255);
cathal66 0:fb0c24069b95 273 //wait(0.01);
cathal66 0:fb0c24069b95 274 }
cathal66 0:fb0c24069b95 275 }
cathal66 0:fb0c24069b95 276 }
cathal66 0:fb0c24069b95 277
cathal66 0:fb0c24069b95 278 void Demo_thread(void const *argument) {
cathal66 0:fb0c24069b95 279 while (true)
cathal66 0:fb0c24069b95 280 {
cathal66 0:fb0c24069b95 281 Thread::signal_wait(0x1); //wait for flag to be set
cathal66 0:fb0c24069b95 282
cathal66 0:fb0c24069b95 283 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 284 //////////////////////////////////// X //////////////////////////////////////////
cathal66 0:fb0c24069b95 285 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 286 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 287 Servo_X = 0.06;
cathal66 0:fb0c24069b95 288 Servo_z_smooth = 0.01;
cathal66 0:fb0c24069b95 289 LED_Colour_Pos(0,255,0,0,128);
cathal66 0:fb0c24069b95 290 LED_Colour_Pos(1,0,255,0,128);
cathal66 0:fb0c24069b95 291 LED_Colour_Pos(2,0,0,255,128);
cathal66 0:fb0c24069b95 292 LED_Colour_Pos(3,255,255,0,128);
cathal66 0:fb0c24069b95 293 LED_Colour_Pos(4,255,0,255,128);
cathal66 0:fb0c24069b95 294 LED_Colour_Pos(5,128,255,0,128);
cathal66 0:fb0c24069b95 295 LED_Colour_Pos(6,128,50,0,255);
cathal66 0:fb0c24069b95 296 LED_Colour_Pos(7,0,255,128,128);
cathal66 0:fb0c24069b95 297 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:fb0c24069b95 298 LED_Colour_Pos(9,128,128,255,128);
cathal66 0:fb0c24069b95 299 LED_Colour_Pos_Set();
cathal66 0:fb0c24069b95 300 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 301 Thread::wait(1000);
cathal66 0:fb0c24069b95 302 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 303 Servo_X = 0.06;
cathal66 0:fb0c24069b95 304 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 305 LED_Colour_Pos(0,128,255,0,128);
cathal66 0:fb0c24069b95 306 LED_Colour_Pos(1,128,50,0,255);
cathal66 0:fb0c24069b95 307 LED_Colour_Pos(2,0,255,128,128);
cathal66 0:fb0c24069b95 308 LED_Colour_Pos(3,128,0,128,255);
cathal66 0:fb0c24069b95 309 LED_Colour_Pos(4,128,128,255,128);
cathal66 0:fb0c24069b95 310 LED_Colour_Pos(5,255,255,0,255);
cathal66 0:fb0c24069b95 311 LED_Colour_Pos(6,128,50,127,255);
cathal66 0:fb0c24069b95 312 LED_Colour_Pos(7,255,255,0,128);
cathal66 0:fb0c24069b95 313 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:fb0c24069b95 314 LED_Colour_Pos(9,0,128,255,128);
cathal66 0:fb0c24069b95 315 LED_Colour_Pos_Set();
cathal66 0:fb0c24069b95 316 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 317 Thread::wait(500);
cathal66 0:fb0c24069b95 318 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 319 Servo_X = 0.08;
cathal66 0:fb0c24069b95 320 Servo_z_smooth = 0.08;
cathal66 0:fb0c24069b95 321 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 322 Thread::wait(500);
cathal66 0:fb0c24069b95 323 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 324 Servo_X = 0.05;
cathal66 0:fb0c24069b95 325 Servo_z_smooth = 0.1;
cathal66 0:fb0c24069b95 326 LED_All_Colour(255, 0 , 0 , 255);
cathal66 0:fb0c24069b95 327 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 328 Thread::wait(500);
cathal66 0:fb0c24069b95 329 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 330 Servo_X = 0.05;
cathal66 0:fb0c24069b95 331 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 332 LED_All_Colour(255, 255 , 0 , 255);
cathal66 0:fb0c24069b95 333 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 334 Thread::wait(500);
cathal66 0:fb0c24069b95 335 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 336 Servo_X = 0.09;
cathal66 0:fb0c24069b95 337 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 338 LED_All_Colour(0, 0 , 255 , 255);
cathal66 0:fb0c24069b95 339 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 340 Thread::wait(500);
cathal66 0:fb0c24069b95 341 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 342 Servo_X = 0.05;
cathal66 0:fb0c24069b95 343 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 344 LED_All_Colour(0, 255 , 0 , 255);
cathal66 0:fb0c24069b95 345 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 346 Thread::wait(500);
cathal66 0:fb0c24069b95 347 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 348 Servo_X = 0.09;
cathal66 0:fb0c24069b95 349 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 350 LED_Colour_Pos(0,255,0,0,128);
cathal66 0:fb0c24069b95 351 LED_Colour_Pos(1,0,255,0,128);
cathal66 0:fb0c24069b95 352 LED_Colour_Pos(2,0,0,255,128);
cathal66 0:fb0c24069b95 353 LED_Colour_Pos(3,255,255,0,128);
cathal66 0:fb0c24069b95 354 LED_Colour_Pos(4,255,0,255,128);
cathal66 0:fb0c24069b95 355 LED_Colour_Pos(5,128,255,0,128);
cathal66 0:fb0c24069b95 356 LED_Colour_Pos(6,128,50,0,255);
cathal66 0:fb0c24069b95 357 LED_Colour_Pos(7,0,255,128,128);
cathal66 0:fb0c24069b95 358 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:fb0c24069b95 359 LED_Colour_Pos(9,128,128,255,128);
cathal66 0:fb0c24069b95 360 LED_Colour_Pos_Set();
cathal66 0:fb0c24069b95 361 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 362 Thread::wait(500);
cathal66 0:fb0c24069b95 363 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 364 Servo_X = 0.05;
cathal66 0:fb0c24069b95 365 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 366 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 367 Thread::wait(500);
cathal66 0:fb0c24069b95 368 Servo_Y = 0.05;
cathal66 0:fb0c24069b95 369 Servo_X = 0.08;
cathal66 0:fb0c24069b95 370 Servo_z_smooth = 0.05;
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 0:fb0c24069b95 373
cathal66 0:fb0c24069b95 374 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 375 //////////////////////////////////// y //////////////////////////////////////////
cathal66 0:fb0c24069b95 376 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 377 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 378 Servo_X = 0.08;
cathal66 0:fb0c24069b95 379 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 380 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 381 Thread::wait(500);
cathal66 0:fb0c24069b95 382 Servo_Y = 0.08;
cathal66 0:fb0c24069b95 383 Servo_X = 0.075;
cathal66 0:fb0c24069b95 384 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 385 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 386 Thread::wait(500);
cathal66 0:fb0c24069b95 387 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 388 Servo_X = 0.075;
cathal66 0:fb0c24069b95 389 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 390 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 391 Thread::wait(500);
cathal66 0:fb0c24069b95 392 Servo_Y = 0.065;
cathal66 0:fb0c24069b95 393 Servo_X = 0.075;
cathal66 0:fb0c24069b95 394 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 395 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 396 Thread::wait(1000);
cathal66 0:fb0c24069b95 397 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 398 Servo_X = 0.075;
cathal66 0:fb0c24069b95 399 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 400 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 401 Thread::wait(500);
cathal66 0:fb0c24069b95 402 Servo_Y = 0.065;
cathal66 0:fb0c24069b95 403 Servo_X = 0.075;
cathal66 0:fb0c24069b95 404 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 405 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 406 Thread::wait(500);
cathal66 0:fb0c24069b95 407 Servo_Y = 0.055;
cathal66 0:fb0c24069b95 408 Servo_X = 0.075;
cathal66 0:fb0c24069b95 409 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 410 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 411 Thread::wait(500);
cathal66 0:fb0c24069b95 412 Servo_Y = 0.065;
cathal66 0:fb0c24069b95 413 Servo_X = 0.075;
cathal66 0:fb0c24069b95 414 Servo_z_smooth = 0.05;
cathal66 0:fb0c24069b95 415 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 416 Thread::wait(500);
cathal66 0:fb0c24069b95 417 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 418 Servo_X = 0.075;
cathal66 0:fb0c24069b95 419 Servo_z_smooth = 0.09;
cathal66 0:fb0c24069b95 420 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 421 Thread::wait(750);
cathal66 0:fb0c24069b95 422
cathal66 0:fb0c24069b95 423 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 424 //////////////////////////////////// Z //////////////////////////////////////////
cathal66 0:fb0c24069b95 425 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:fb0c24069b95 426 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 427 Servo_X = 0.075;
cathal66 0:fb0c24069b95 428 Servo_z_smooth = 0.08;
cathal66 0:fb0c24069b95 429 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 430 Thread::wait(500);
cathal66 0:fb0c24069b95 431 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 432 Servo_X = 0.075;
cathal66 0:fb0c24069b95 433 Servo_z_smooth = 0.1;
cathal66 0:fb0c24069b95 434 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 435 Thread::wait(500);
cathal66 0:fb0c24069b95 436 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 437 Servo_X = 0.075;
cathal66 0:fb0c24069b95 438 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 439 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 440 Thread::wait(500);
cathal66 0:fb0c24069b95 441 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 442 Servo_X = 0.075;
cathal66 0:fb0c24069b95 443 Servo_z_smooth = 0.11;
cathal66 0:fb0c24069b95 444 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 445 Thread::wait(750);
cathal66 0:fb0c24069b95 446 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 447 Servo_X = 0.075;
cathal66 0:fb0c24069b95 448 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 449 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 450 Thread::wait(750);
cathal66 0:fb0c24069b95 451 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 452 Servo_X = 0.075;
cathal66 0:fb0c24069b95 453 Servo_z_smooth = 0.11;
cathal66 0:fb0c24069b95 454 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 455 Thread::wait(750);
cathal66 0:fb0c24069b95 456 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 457 Servo_X = 0.075;
cathal66 0:fb0c24069b95 458 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 459 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 460 Thread::wait(750);
cathal66 0:fb0c24069b95 461 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 462 Servo_X = 0.075;
cathal66 0:fb0c24069b95 463 Servo_z_smooth = 0.11;
cathal66 0:fb0c24069b95 464 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 465 Thread::wait(750);
cathal66 0:fb0c24069b95 466 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 467 Servo_X = 0.075;
cathal66 0:fb0c24069b95 468 Servo_z_smooth = 0.07;
cathal66 0:fb0c24069b95 469 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 470 Thread::wait(1500);
cathal66 0:fb0c24069b95 471 Servo_Y = 0.06;
cathal66 0:fb0c24069b95 472 Servo_X = 0.75;
cathal66 0:fb0c24069b95 473 Servo_z_smooth = 0.03;
cathal66 0:fb0c24069b95 474 printf("Servo X: %f1.4 Servo Y: %f1.4 Servo Z: %f1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:fb0c24069b95 475 Thread::wait(1500);
cathal66 0:fb0c24069b95 476 }
cathal66 0:fb0c24069b95 477
cathal66 0:fb0c24069b95 478 }
cathal66 0:fb0c24069b95 479
cathal66 0:fb0c24069b95 480 void Red_Blue_Flash(int h)
cathal66 0:fb0c24069b95 481 {
cathal66 0:fb0c24069b95 482 for(int i = 0;i<=h;i++)
cathal66 0:fb0c24069b95 483 {
cathal66 0:fb0c24069b95 484
cathal66 0:fb0c24069b95 485 LED_All_Colour(255, 0 ,0 , 50);
cathal66 0:fb0c24069b95 486 wait(0.02);
cathal66 0:fb0c24069b95 487 LED_All_Colour(255, 0 ,0 , 255);
cathal66 0:fb0c24069b95 488 wait(0.02);
cathal66 0:fb0c24069b95 489 LED_All_Colour(0, 0 , 255 , 50);
cathal66 0:fb0c24069b95 490 wait(0.02);
cathal66 0:fb0c24069b95 491 LED_All_Colour(0, 0 , 255 , 255);
cathal66 0:fb0c24069b95 492 wait(0.02);
cathal66 0:fb0c24069b95 493 }
cathal66 0:fb0c24069b95 494 }
cathal66 0:fb0c24069b95 495
cathal66 0:fb0c24069b95 496 void Demo()
cathal66 0:fb0c24069b95 497 {
cathal66 0:fb0c24069b95 498
cathal66 0:fb0c24069b95 499 }
cathal66 0:fb0c24069b95 500
cathal66 0:fb0c24069b95 501 int main (void)
cathal66 0:fb0c24069b95 502 {
cathal66 0:fb0c24069b95 503
cathal66 0:fb0c24069b95 504 //Thread fsm(fsm_thread);
cathal66 0:fb0c24069b95 505 Thread too_far_event(too_far_event_thread);
cathal66 0:fb0c24069b95 506 Thread too_close_event(too_close_event_thread);
cathal66 0:fb0c24069b95 507 Thread detect_event(detect_event_thread);
cathal66 0:fb0c24069b95 508 Thread thread_Servo_Z(Servo_Z_Thread );
cathal66 0:fb0c24069b95 509 Thread thread_Demo(Demo_thread );
cathal66 0:fb0c24069b95 510 Thread thread_Sonar_Read(read_sonar_thread );
cathal66 0:fb0c24069b95 511 //RtosTimer timer(timeout_event, osTimerPeriodic, (void *)0);
cathal66 0:fb0c24069b95 512
cathal66 0:fb0c24069b95 513 int state = PAN_TILT;
cathal66 0:fb0c24069b95 514
cathal66 0:fb0c24069b95 515 //start timer with a 2 sec timeout
cathal66 0:fb0c24069b95 516 //timer.start(2000);
cathal66 0:fb0c24069b95 517
cathal66 0:fb0c24069b95 518 while(0)
cathal66 0:fb0c24069b95 519 {
cathal66 0:fb0c24069b95 520 osEvent evt = queue.get();
cathal66 0:fb0c24069b95 521 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 522 {
cathal66 0:fb0c24069b95 523 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 524 mpool.free(message);
cathal66 0:fb0c24069b95 525 }
cathal66 0:fb0c24069b95 526 }
cathal66 0:fb0c24069b95 527
cathal66 0:fb0c24069b95 528 while (true)
cathal66 0:fb0c24069b95 529 {
cathal66 0:fb0c24069b95 530
cathal66 0:fb0c24069b95 531 switch(state) // locked
cathal66 0:fb0c24069b95 532 {
cathal66 0:fb0c24069b95 533
cathal66 0:fb0c24069b95 534 case SCAN_FACE:
cathal66 0:fb0c24069b95 535
cathal66 0:fb0c24069b95 536 osEvent evt = queue.get();
cathal66 0:fb0c24069b95 537 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 538 {
cathal66 0:fb0c24069b95 539 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 540
cathal66 0:fb0c24069b95 541 if(message->event == DETECT)
cathal66 0:fb0c24069b95 542 {
cathal66 0:fb0c24069b95 543 //next state
cathal66 0:fb0c24069b95 544 state = SCAN_FACE;
cathal66 0:fb0c24069b95 545 lcd.cls();
cathal66 0:fb0c24069b95 546 lcd.locate(0,2);
cathal66 0:fb0c24069b95 547 lcd.printf("state scan face - detect");
cathal66 0:fb0c24069b95 548
cathal66 0:fb0c24069b95 549 }
cathal66 0:fb0c24069b95 550
cathal66 0:fb0c24069b95 551
cathal66 0:fb0c24069b95 552 if(message->event == TOO_CLOSE)
cathal66 0:fb0c24069b95 553 {
cathal66 0:fb0c24069b95 554 //next state
cathal66 0:fb0c24069b95 555 state = LOCK_DOWN;
cathal66 0:fb0c24069b95 556 lcd.cls();
cathal66 0:fb0c24069b95 557 lcd.locate(0,2);
cathal66 0:fb0c24069b95 558 lcd.printf("state scan face - too close");
cathal66 0:fb0c24069b95 559
cathal66 0:fb0c24069b95 560 }
cathal66 0:fb0c24069b95 561
cathal66 0:fb0c24069b95 562
cathal66 0:fb0c24069b95 563 if(message->event == TOO_FAR)
cathal66 0:fb0c24069b95 564 {
cathal66 0:fb0c24069b95 565 state = PAN_TILT;
cathal66 0:fb0c24069b95 566 lcd.cls();
cathal66 0:fb0c24069b95 567 lcd.locate(0,2);
cathal66 0:fb0c24069b95 568 lcd.printf("state scan face - too-far");
cathal66 0:fb0c24069b95 569
cathal66 0:fb0c24069b95 570 }
cathal66 0:fb0c24069b95 571
cathal66 0:fb0c24069b95 572 mpool.free(message);
cathal66 0:fb0c24069b95 573
cathal66 0:fb0c24069b95 574 }
cathal66 0:fb0c24069b95 575
cathal66 0:fb0c24069b95 576 //timer.start(2000);
cathal66 0:fb0c24069b95 577
cathal66 0:fb0c24069b95 578 break;
cathal66 0:fb0c24069b95 579
cathal66 0:fb0c24069b95 580 case PAN_TILT:
cathal66 0:fb0c24069b95 581
cathal66 0:fb0c24069b95 582 //osEvent
cathal66 0:fb0c24069b95 583 evt = queue.get();
cathal66 0:fb0c24069b95 584 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 585 {
cathal66 0:fb0c24069b95 586 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 587
cathal66 0:fb0c24069b95 588 if(message->event == TOO_FAR)
cathal66 0:fb0c24069b95 589 {
cathal66 0:fb0c24069b95 590 //next state
cathal66 0:fb0c24069b95 591 state = PAN_TILT;
cathal66 0:fb0c24069b95 592 lcd.cls();
cathal66 0:fb0c24069b95 593 lcd.locate(0,2);
cathal66 0:fb0c24069b95 594 lcd.printf("state pan tilt - too-far");
cathal66 0:fb0c24069b95 595
cathal66 0:fb0c24069b95 596 }
cathal66 0:fb0c24069b95 597
cathal66 0:fb0c24069b95 598 if(message->event == DETECT)
cathal66 0:fb0c24069b95 599 {
cathal66 0:fb0c24069b95 600 //next state
cathal66 0:fb0c24069b95 601 state = PAN_TILT;
cathal66 0:fb0c24069b95 602 thread_Demo.signal_set(0x1);
cathal66 0:fb0c24069b95 603 lcd.cls();
cathal66 0:fb0c24069b95 604 lcd.locate(0,2);
cathal66 0:fb0c24069b95 605 lcd.printf("state pan tilt - detect");
cathal66 0:fb0c24069b95 606
cathal66 0:fb0c24069b95 607 }
cathal66 0:fb0c24069b95 608
cathal66 0:fb0c24069b95 609 if(message->event == TOO_CLOSE)
cathal66 0:fb0c24069b95 610 {
cathal66 0:fb0c24069b95 611 state = PAN_TILT;
cathal66 0:fb0c24069b95 612
cathal66 0:fb0c24069b95 613 lcd.cls();
cathal66 0:fb0c24069b95 614 lcd.locate(0,2);
cathal66 0:fb0c24069b95 615 lcd.printf("state pan tilt - too close");
cathal66 0:fb0c24069b95 616
cathal66 0:fb0c24069b95 617 }
cathal66 0:fb0c24069b95 618
cathal66 0:fb0c24069b95 619 mpool.free(message);
cathal66 0:fb0c24069b95 620 }
cathal66 0:fb0c24069b95 621
cathal66 0:fb0c24069b95 622 //timer.start(2000);
cathal66 0:fb0c24069b95 623
cathal66 0:fb0c24069b95 624 break;
cathal66 0:fb0c24069b95 625
cathal66 0:fb0c24069b95 626 case LOCK_DOWN:
cathal66 0:fb0c24069b95 627
cathal66 0:fb0c24069b95 628 evt = queue.get();
cathal66 0:fb0c24069b95 629 if (evt.status == osEventMessage)
cathal66 0:fb0c24069b95 630 {
cathal66 0:fb0c24069b95 631 message_t *message = (message_t*)evt.value.p;
cathal66 0:fb0c24069b95 632
cathal66 0:fb0c24069b95 633 if(message->event == TOO_CLOSE)
cathal66 0:fb0c24069b95 634 {
cathal66 0:fb0c24069b95 635 state = LOCK_DOWN;
cathal66 0:fb0c24069b95 636
cathal66 0:fb0c24069b95 637 lcd.cls();
cathal66 0:fb0c24069b95 638 lcd.locate(0,2);
cathal66 0:fb0c24069b95 639 lcd.printf("state lock down - too close");
cathal66 0:fb0c24069b95 640 }
cathal66 0:fb0c24069b95 641
cathal66 0:fb0c24069b95 642 if(message->event == TIME_OUT)
cathal66 0:fb0c24069b95 643 {
cathal66 0:fb0c24069b95 644 state = PAN_TILT;
cathal66 0:fb0c24069b95 645
cathal66 0:fb0c24069b95 646 lcd.cls();
cathal66 0:fb0c24069b95 647 lcd.locate(0,2);
cathal66 0:fb0c24069b95 648 lcd.printf("state lock down - time out");
cathal66 0:fb0c24069b95 649 }
cathal66 0:fb0c24069b95 650
cathal66 0:fb0c24069b95 651 mpool.free(message);
cathal66 0:fb0c24069b95 652
cathal66 0:fb0c24069b95 653 }
cathal66 0:fb0c24069b95 654
cathal66 0:fb0c24069b95 655 //timer.start(2000);
cathal66 0:fb0c24069b95 656
cathal66 0:fb0c24069b95 657 break;
cathal66 0:fb0c24069b95 658
cathal66 0:fb0c24069b95 659 } //End of switch
cathal66 0:fb0c24069b95 660
cathal66 0:fb0c24069b95 661 //toggle led for local testing
cathal66 0:fb0c24069b95 662
cathal66 0:fb0c24069b95 663
cathal66 0:fb0c24069b95 664 } //end of while(1)
cathal66 0:fb0c24069b95 665
cathal66 0:fb0c24069b95 666 }