Diff: main.cpp

Revision:

7:0fa7430ab812

Parent:

6:4c62f9c91b1d

Child:

8:b22ef9f44549

--- a/main.cpp	Sun Jan 25 22:26:35 2015 +0000
+++ b/main.cpp	Mon Jan 26 14:58:21 2015 +0000
@@ -2,15 +2,24 @@
 #include "rtos.h"
 #include "C12832_lcd.h"
 
+//Varible
 float sonarDistance;
 float servoPosition;
 
+//Mutex
 Mutex sonarDistance_mutex;                                  //Init Mutex for sensor value
 Mutex servoPosition_mutex;                                  //Init Mutex for servo value
-    
+Mutex LED_mutex;
+
+//I/O    
 AnalogIn sonarPin(p17);                                     //Assign pin to read sonar sensor
 PwmOut servoPin(p21);                                       //Assign pin for servo output
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
 
+//LCD Setup
 C12832_LCD lcd;                                             //setup LCD screen
 
 void sonarSensor(void const *args){
@@ -32,24 +41,28 @@
 
 void logic(void const *args)
     {
+    int Size=12;
+    float Average_4[12];                  //number of value in the array
+    float Average_Sum=0;
     while(true)
         {
-        float Average_4[]={0,0,0,0,0,0,0};                  //number of value in the array
-        float Average_Sum=0;
-        
-        sonarDistance_mutex.lock();                         //Mutex lock for the Sonar value
-        servoPosition_mutex.lock();                         //Mutex lock for the servo value
-            
-        for(int i=0;i<6;i++)
+
+        for(int i=0;i<Size;i++)
             {
              Average_Sum = Average_Sum-Average_4[i];        //Remove the 4th oldest value from the average sum
+
+             sonarDistance_mutex.lock();                         //Mutex lock for the Sonar value
              Average_4[i]= sonarDistance;                   //Add the new value to the array
+             sonarDistance_mutex.unlock();                       //Mutex unlock for the Sonar value
+             
              Average_Sum = Average_Sum + Average_4[i];      //Add the new array value to the sum
-             servoPosition = Average_Sum/6;                 //Divide the array by the number of element in the array
+             
+             servoPosition_mutex.lock();                         //Mutex lock for the servo value             
+             servoPosition = Average_Sum/Size;                      //Divide the array by the number of element in the array
+             servoPosition_mutex.unlock();                       //Mutex unlock for the servo value
+            
             }//end for loop
             
-        sonarDistance_mutex.unlock();                       //Mutex unlock for the Sonar value
-        servoPosition_mutex.unlock();                       //Mutex unlock for the servo value
         }//end of while loop
     }//End of thread
 
@@ -67,6 +80,55 @@
     }
 }
 
+void LED_meter(void const *args) {
+    
+    float LED_distance = 0;
+    
+    while(true) {
+        sonarDistance_mutex.lock();
+        LED_distance=sonarDistance;
+        sonarDistance_mutex.unlock();
+        
+        if(LED_distance<=0.2)
+            {
+            myled1 = 1;
+            }
+        else
+            {
+            myled1 = 0;
+            }
+            
+        if(LED_distance<=0.15)
+            {
+            myled2 = 1;
+            }
+        else
+            {
+            myled2 = 0;
+            }
+ 
+        if(LED_distance<=0.1)
+            {
+            myled3 = 1;
+            }
+        else
+            {
+            myled3 = 0;
+            }       
+       
+        if(LED_distance<=0.05)
+            {
+            myled4 = 1;
+            //s2=0.1;  
+            }
+        else
+            {
+            myled4 = 0;
+            //s2=1;  
+            }         
+    }
+}
+
 int main(){
     sonarDistance = 0.0f;
     servoPosition = 0.0f;
@@ -76,6 +138,7 @@
     Thread servoControl_thread(servoControl);
     Thread logic_thread(logic);
     Thread display_thread(display);
+    Thread LED_meter_thread(LED_meter);
     
     while(true)
         {