Embedded Team / Mbed 2 deprecated lampbot_sonar_state_machine_test

State machine test

Dependencies:   C12832 RangeFinder mbed-rtos mbed

main.cpp@0:3bdd926bec58, 2015-04-23 (annotated)

Committer:
donaldo1991
Date:
Thu Apr 23 15:05:11 2015 +0000
Revision:
0:3bdd926bec58
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
donaldo1991 0:3bdd926bec58 1 #include "mbed.h"
donaldo1991 0:3bdd926bec58 2 #include "rtos.h"
donaldo1991 0:3bdd926bec58 3 #include "C12832.h"
donaldo1991 0:3bdd926bec58 4 #include "RangeFinder.h"
donaldo1991 0:3bdd926bec58 5
donaldo1991 0:3bdd926bec58 6 //FINITE STATE MACHINE EVENTS
donaldo1991 0:3bdd926bec58 7 #define TIME_OUT 0
donaldo1991 0:3bdd926bec58 8 #define TOO_CLOSE 1
donaldo1991 0:3bdd926bec58 9 #define TOO_FAR 2
donaldo1991 0:3bdd926bec58 10 #define DETECT 3
donaldo1991 0:3bdd926bec58 11
donaldo1991 0:3bdd926bec58 12 //STATES
donaldo1991 0:3bdd926bec58 13 #define SCAN_FACE 0
donaldo1991 0:3bdd926bec58 14 #define PAN_TILT 1
donaldo1991 0:3bdd926bec58 15 #define LOCK_DOWN 2
donaldo1991 0:3bdd926bec58 16
donaldo1991 0:3bdd926bec58 17 //pass event via message queue
donaldo1991 0:3bdd926bec58 18 typedef struct {
donaldo1991 0:3bdd926bec58 19 int event; /* AD result of measured voltage */
donaldo1991 0:3bdd926bec58 20 } message_t;
donaldo1991 0:3bdd926bec58 21
donaldo1991 0:3bdd926bec58 22 MemoryPool<message_t, 16> mpool;
donaldo1991 0:3bdd926bec58 23 Queue<message_t, 16> queue;
donaldo1991 0:3bdd926bec58 24
donaldo1991 0:3bdd926bec58 25 //SONAR_SENSOR INPUT
donaldo1991 0:3bdd926bec58 26 RangeFinder ping_sensor(p21, 5, 5800.0, 100000);
donaldo1991 0:3bdd926bec58 27
donaldo1991 0:3bdd926bec58 28 //local display]
donaldo1991 0:3bdd926bec58 29 C12832 lcd(p5, p7, p6, p8, p11);
donaldo1991 0:3bdd926bec58 30
donaldo1991 0:3bdd926bec58 31 //leds for debug
donaldo1991 0:3bdd926bec58 32 DigitalOut led(LED1); //button press
donaldo1991 0:3bdd926bec58 33 DigitalOut led2(LED2); //fsm thread
donaldo1991 0:3bdd926bec58 34 DigitalOut led3(LED3);
donaldo1991 0:3bdd926bec58 35 DigitalOut led4(LED4); //timeout thread
donaldo1991 0:3bdd926bec58 36
donaldo1991 0:3bdd926bec58 37
donaldo1991 0:3bdd926bec58 38 void timeout_event(void const *n)
donaldo1991 0:3bdd926bec58 39 {
donaldo1991 0:3bdd926bec58 40 message_t *message = mpool.alloc();
donaldo1991 0:3bdd926bec58 41 message->event = TIME_OUT;
donaldo1991 0:3bdd926bec58 42 queue.put(message);
donaldo1991 0:3bdd926bec58 43
donaldo1991 0:3bdd926bec58 44 led = !led;
donaldo1991 0:3bdd926bec58 45 }
donaldo1991 0:3bdd926bec58 46
donaldo1991 0:3bdd926bec58 47
donaldo1991 0:3bdd926bec58 48 void too_close_event_thread(void const *argument)
donaldo1991 0:3bdd926bec58 49 {
donaldo1991 0:3bdd926bec58 50 while (true)
donaldo1991 0:3bdd926bec58 51 {
donaldo1991 0:3bdd926bec58 52 float Distance = ping_sensor.read_m();
donaldo1991 0:3bdd926bec58 53 if (Distance < 0)
donaldo1991 0:3bdd926bec58 54 {
donaldo1991 0:3bdd926bec58 55 //event via a message queue
donaldo1991 0:3bdd926bec58 56 message_t *message = mpool.alloc();
donaldo1991 0:3bdd926bec58 57 message->event = TOO_CLOSE;
donaldo1991 0:3bdd926bec58 58 queue.put(message);
donaldo1991 0:3bdd926bec58 59
donaldo1991 0:3bdd926bec58 60 led2 = !led2;
donaldo1991 0:3bdd926bec58 61 }
donaldo1991 0:3bdd926bec58 62
donaldo1991 0:3bdd926bec58 63 }
donaldo1991 0:3bdd926bec58 64 }
donaldo1991 0:3bdd926bec58 65
donaldo1991 0:3bdd926bec58 66 void detect_event_thread(void const *argument)
donaldo1991 0:3bdd926bec58 67 {
donaldo1991 0:3bdd926bec58 68 while (true)
donaldo1991 0:3bdd926bec58 69 {
donaldo1991 0:3bdd926bec58 70 float Distance = ping_sensor.read_m();
donaldo1991 0:3bdd926bec58 71 if(Distance > 1 & Distance < 2)
donaldo1991 0:3bdd926bec58 72 {
donaldo1991 0:3bdd926bec58 73 //event via a message queue
donaldo1991 0:3bdd926bec58 74 message_t *message = mpool.alloc();
donaldo1991 0:3bdd926bec58 75 message->event = DETECT;
donaldo1991 0:3bdd926bec58 76 queue.put(message);
donaldo1991 0:3bdd926bec58 77
donaldo1991 0:3bdd926bec58 78 led3= !led3;
donaldo1991 0:3bdd926bec58 79 }
donaldo1991 0:3bdd926bec58 80 }
donaldo1991 0:3bdd926bec58 81 }
donaldo1991 0:3bdd926bec58 82
donaldo1991 0:3bdd926bec58 83 void too_far_event_thread(void const *argument)
donaldo1991 0:3bdd926bec58 84 {
donaldo1991 0:3bdd926bec58 85 while (true)
donaldo1991 0:3bdd926bec58 86 {
donaldo1991 0:3bdd926bec58 87 float Distance = ping_sensor.read_m();
donaldo1991 0:3bdd926bec58 88 if (Distance > 3)
donaldo1991 0:3bdd926bec58 89 {
donaldo1991 0:3bdd926bec58 90 //event via a message queue
donaldo1991 0:3bdd926bec58 91 message_t *message = mpool.alloc();
donaldo1991 0:3bdd926bec58 92 message->event = TOO_FAR;
donaldo1991 0:3bdd926bec58 93 queue.put(message);
donaldo1991 0:3bdd926bec58 94
donaldo1991 0:3bdd926bec58 95 led4 = !led4;
donaldo1991 0:3bdd926bec58 96 }
donaldo1991 0:3bdd926bec58 97 }
donaldo1991 0:3bdd926bec58 98 }
donaldo1991 0:3bdd926bec58 99
donaldo1991 0:3bdd926bec58 100 int main (void)
donaldo1991 0:3bdd926bec58 101 {
donaldo1991 0:3bdd926bec58 102
donaldo1991 0:3bdd926bec58 103 //Thread fsm(fsm_thread);
donaldo1991 0:3bdd926bec58 104 Thread too_far_event(too_far_event_thread);
donaldo1991 0:3bdd926bec58 105 Thread too_close_event(too_close_event_thread);
donaldo1991 0:3bdd926bec58 106 Thread detect_event(detect_event_thread);
donaldo1991 0:3bdd926bec58 107 RtosTimer timer(timeout_event, osTimerPeriodic, (void *)0);
donaldo1991 0:3bdd926bec58 108
donaldo1991 0:3bdd926bec58 109 int state = PAN_TILT;
donaldo1991 0:3bdd926bec58 110
donaldo1991 0:3bdd926bec58 111 //start timer with a 2 sec timeout
donaldo1991 0:3bdd926bec58 112 timer.start(2000);
donaldo1991 0:3bdd926bec58 113
donaldo1991 0:3bdd926bec58 114 while(0)
donaldo1991 0:3bdd926bec58 115 {
donaldo1991 0:3bdd926bec58 116 osEvent evt = queue.get();
donaldo1991 0:3bdd926bec58 117 if (evt.status == osEventMessage)
donaldo1991 0:3bdd926bec58 118 {
donaldo1991 0:3bdd926bec58 119 message_t *message = (message_t*)evt.value.p;
donaldo1991 0:3bdd926bec58 120 mpool.free(message);
donaldo1991 0:3bdd926bec58 121 }
donaldo1991 0:3bdd926bec58 122 }
donaldo1991 0:3bdd926bec58 123
donaldo1991 0:3bdd926bec58 124 while (true)
donaldo1991 0:3bdd926bec58 125 {
donaldo1991 0:3bdd926bec58 126
donaldo1991 0:3bdd926bec58 127 switch(state) // locked
donaldo1991 0:3bdd926bec58 128 {
donaldo1991 0:3bdd926bec58 129
donaldo1991 0:3bdd926bec58 130 case SCAN_FACE:
donaldo1991 0:3bdd926bec58 131
donaldo1991 0:3bdd926bec58 132 osEvent evt = queue.get();
donaldo1991 0:3bdd926bec58 133 if (evt.status == osEventMessage)
donaldo1991 0:3bdd926bec58 134 {
donaldo1991 0:3bdd926bec58 135 message_t *message = (message_t*)evt.value.p;
donaldo1991 0:3bdd926bec58 136
donaldo1991 0:3bdd926bec58 137 if(message->event == DETECT)
donaldo1991 0:3bdd926bec58 138 {
donaldo1991 0:3bdd926bec58 139 //next state
donaldo1991 0:3bdd926bec58 140 state = SCAN_FACE;
donaldo1991 0:3bdd926bec58 141 lcd.cls();
donaldo1991 0:3bdd926bec58 142 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 143 lcd.printf("state scan face - detect");
donaldo1991 0:3bdd926bec58 144
donaldo1991 0:3bdd926bec58 145 }
donaldo1991 0:3bdd926bec58 146
donaldo1991 0:3bdd926bec58 147
donaldo1991 0:3bdd926bec58 148 if(message->event == TOO_CLOSE)
donaldo1991 0:3bdd926bec58 149 {
donaldo1991 0:3bdd926bec58 150 //next state
donaldo1991 0:3bdd926bec58 151 state = LOCK_DOWN;
donaldo1991 0:3bdd926bec58 152 lcd.cls();
donaldo1991 0:3bdd926bec58 153 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 154 lcd.printf("state scan face - too close");
donaldo1991 0:3bdd926bec58 155
donaldo1991 0:3bdd926bec58 156 }
donaldo1991 0:3bdd926bec58 157
donaldo1991 0:3bdd926bec58 158
donaldo1991 0:3bdd926bec58 159 if(message->event == TOO_FAR)
donaldo1991 0:3bdd926bec58 160 {
donaldo1991 0:3bdd926bec58 161 state = PAN_TILT;
donaldo1991 0:3bdd926bec58 162 lcd.cls();
donaldo1991 0:3bdd926bec58 163 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 164 lcd.printf("state scan face - too-far");
donaldo1991 0:3bdd926bec58 165
donaldo1991 0:3bdd926bec58 166 }
donaldo1991 0:3bdd926bec58 167
donaldo1991 0:3bdd926bec58 168 mpool.free(message);
donaldo1991 0:3bdd926bec58 169
donaldo1991 0:3bdd926bec58 170 }
donaldo1991 0:3bdd926bec58 171
donaldo1991 0:3bdd926bec58 172 timer.start(2000);
donaldo1991 0:3bdd926bec58 173
donaldo1991 0:3bdd926bec58 174 break;
donaldo1991 0:3bdd926bec58 175
donaldo1991 0:3bdd926bec58 176 case PAN_TILT:
donaldo1991 0:3bdd926bec58 177
donaldo1991 0:3bdd926bec58 178 //osEvent
donaldo1991 0:3bdd926bec58 179 evt = queue.get();
donaldo1991 0:3bdd926bec58 180 if (evt.status == osEventMessage)
donaldo1991 0:3bdd926bec58 181 {
donaldo1991 0:3bdd926bec58 182 message_t *message = (message_t*)evt.value.p;
donaldo1991 0:3bdd926bec58 183
donaldo1991 0:3bdd926bec58 184 if(message->event == TOO_FAR)
donaldo1991 0:3bdd926bec58 185 {
donaldo1991 0:3bdd926bec58 186 //next state
donaldo1991 0:3bdd926bec58 187 state = PAN_TILT;
donaldo1991 0:3bdd926bec58 188 lcd.cls();
donaldo1991 0:3bdd926bec58 189 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 190 lcd.printf("state pan tilt - too-far");
donaldo1991 0:3bdd926bec58 191
donaldo1991 0:3bdd926bec58 192 }
donaldo1991 0:3bdd926bec58 193
donaldo1991 0:3bdd926bec58 194 if(message->event == DETECT)
donaldo1991 0:3bdd926bec58 195 {
donaldo1991 0:3bdd926bec58 196 //next state
donaldo1991 0:3bdd926bec58 197 state = SCAN_FACE;
donaldo1991 0:3bdd926bec58 198 lcd.cls();
donaldo1991 0:3bdd926bec58 199 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 200 lcd.printf("state pan tilt - detect");
donaldo1991 0:3bdd926bec58 201
donaldo1991 0:3bdd926bec58 202 }
donaldo1991 0:3bdd926bec58 203
donaldo1991 0:3bdd926bec58 204 if(message->event == TOO_CLOSE)
donaldo1991 0:3bdd926bec58 205 {
donaldo1991 0:3bdd926bec58 206 state = LOCK_DOWN;
donaldo1991 0:3bdd926bec58 207
donaldo1991 0:3bdd926bec58 208 lcd.cls();
donaldo1991 0:3bdd926bec58 209 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 210 lcd.printf("state pan tilt - too close");
donaldo1991 0:3bdd926bec58 211
donaldo1991 0:3bdd926bec58 212 }
donaldo1991 0:3bdd926bec58 213
donaldo1991 0:3bdd926bec58 214 mpool.free(message);
donaldo1991 0:3bdd926bec58 215 }
donaldo1991 0:3bdd926bec58 216
donaldo1991 0:3bdd926bec58 217 timer.start(2000);
donaldo1991 0:3bdd926bec58 218
donaldo1991 0:3bdd926bec58 219 break;
donaldo1991 0:3bdd926bec58 220
donaldo1991 0:3bdd926bec58 221 case LOCK_DOWN:
donaldo1991 0:3bdd926bec58 222
donaldo1991 0:3bdd926bec58 223 evt = queue.get();
donaldo1991 0:3bdd926bec58 224 if (evt.status == osEventMessage)
donaldo1991 0:3bdd926bec58 225 {
donaldo1991 0:3bdd926bec58 226 message_t *message = (message_t*)evt.value.p;
donaldo1991 0:3bdd926bec58 227
donaldo1991 0:3bdd926bec58 228 if(message->event == TOO_CLOSE)
donaldo1991 0:3bdd926bec58 229 {
donaldo1991 0:3bdd926bec58 230 state = LOCK_DOWN;
donaldo1991 0:3bdd926bec58 231
donaldo1991 0:3bdd926bec58 232 lcd.cls();
donaldo1991 0:3bdd926bec58 233 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 234 lcd.printf("state lock down - too close");
donaldo1991 0:3bdd926bec58 235 }
donaldo1991 0:3bdd926bec58 236
donaldo1991 0:3bdd926bec58 237 if(message->event == TIME_OUT)
donaldo1991 0:3bdd926bec58 238 {
donaldo1991 0:3bdd926bec58 239 state = PAN_TILT;
donaldo1991 0:3bdd926bec58 240
donaldo1991 0:3bdd926bec58 241 lcd.cls();
donaldo1991 0:3bdd926bec58 242 lcd.locate(0,2);
donaldo1991 0:3bdd926bec58 243 lcd.printf("state lock down - time out");
donaldo1991 0:3bdd926bec58 244 }
donaldo1991 0:3bdd926bec58 245
donaldo1991 0:3bdd926bec58 246 mpool.free(message);
donaldo1991 0:3bdd926bec58 247
donaldo1991 0:3bdd926bec58 248 }
donaldo1991 0:3bdd926bec58 249
donaldo1991 0:3bdd926bec58 250 timer.start(2000);
donaldo1991 0:3bdd926bec58 251
donaldo1991 0:3bdd926bec58 252 break;
donaldo1991 0:3bdd926bec58 253
donaldo1991 0:3bdd926bec58 254 } //End of switch
donaldo1991 0:3bdd926bec58 255
donaldo1991 0:3bdd926bec58 256 //toggle led for local testing
donaldo1991 0:3bdd926bec58 257
donaldo1991 0:3bdd926bec58 258
donaldo1991 0:3bdd926bec58 259 } //end of while(1)
donaldo1991 0:3bdd926bec58 260
donaldo1991 0:3bdd926bec58 261 }