Diff: main.cpp

Revision:

0:3bdd926bec58

Child:

1:1a692f5a040d

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)
+    
+}