Embedded Team
/
lampbot_sonar_state_machine_test
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: C12832 RangeFinder mbed-rtos mbed
Revision 0:3bdd926bec58, committed 2015-04-23
- Comitter:
- donaldo1991
- Date:
- Thu Apr 23 15:05:11 2015 +0000
- Commit message:
- Initial commit
Changed in this revision
diff -r 000000000000 -r 3bdd926bec58 C12832.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/C12832.lib Thu Apr 23 15:05:11 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/askksa12543/code/C12832/#990d5eec2ef6
diff -r 000000000000 -r 3bdd926bec58 RangeFinder.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RangeFinder.lib Thu Apr 23 15:05:11 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/NickRyder/code/RangeFinder/#cf496fb70317
diff -r 000000000000 -r 3bdd926bec58 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 23 15:05:11 2015 +0000
@@ -0,0 +1,261 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "C12832.h"
+#include "RangeFinder.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
+
+//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;
+
+//SONAR_SENSOR INPUT
+RangeFinder ping_sensor(p21, 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
+
+
+void timeout_event(void const *n)
+{
+ message_t *message = mpool.alloc();
+ message->event = TIME_OUT;
+ queue.put(message);
+
+ led = !led;
+}
+
+
+void too_close_event_thread(void const *argument)
+ {
+ while (true)
+ {
+ float Distance = ping_sensor.read_m();
+ if (Distance < 0)
+ {
+ //event via a message queue
+ message_t *message = mpool.alloc();
+ message->event = TOO_CLOSE;
+ queue.put(message);
+
+ led2 = !led2;
+ }
+
+ }
+}
+
+void detect_event_thread(void const *argument)
+ {
+ while (true)
+ {
+ float Distance = ping_sensor.read_m();
+ if(Distance > 1 & Distance < 2)
+ {
+ //event via a message queue
+ message_t *message = mpool.alloc();
+ message->event = DETECT;
+ queue.put(message);
+
+ led3= !led3;
+ }
+ }
+ }
+
+void too_far_event_thread(void const *argument)
+{
+ while (true)
+ {
+ float Distance = ping_sensor.read_m();
+ if (Distance > 3)
+ {
+ //event via a message queue
+ message_t *message = mpool.alloc();
+ message->event = TOO_FAR;
+ queue.put(message);
+
+ led4 = !led4;
+ }
+ }
+}
+
+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);
+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 = SCAN_FACE;
+ lcd.cls();
+ lcd.locate(0,2);
+ lcd.printf("state pan tilt - detect");
+
+ }
+
+ if(message->event == TOO_CLOSE)
+ {
+ state = LOCK_DOWN;
+
+ 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)
+
+}
diff -r 000000000000 -r 3bdd926bec58 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Apr 23 15:05:11 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#899aee34da6a
diff -r 000000000000 -r 3bdd926bec58 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 23 15:05:11 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/433970e64889 \ No newline at end of file