Diff: main.cpp

Revision:

5:e82e00f12634

Parent:

4:429d9a63eb79

Child:

6:4c62f9c91b1d

--- a/main.cpp	Thu Jan 15 16:32:56 2015 +0000
+++ b/main.cpp	Mon Jan 19 15:28:35 2015 +0000
@@ -18,6 +18,7 @@
         sonarDistance_mutex.lock();
             sonarDistance = sonarPin.read();
         sonarDistance_mutex.unlock();
+        Thread::wait(10);
     }
 }
 
@@ -31,9 +32,21 @@
 
 void logic(void const *args){
     while(true){
+        float Average_4[]={0,0,0,0,0,0,0};
+        float Average_Sum=0;
+        
         sonarDistance_mutex.lock();
         servoPosition_mutex.lock();
-            servoPosition = (sonarDistance - servoPosition) / 2;
+            //servoPosition = (sonarDistance - servoPosition)/2 ;
+            for(int i=0;i<6;i++)
+                {
+                 Average_Sum = Average_Sum-Average_4[i];
+                 Average_4[i]= sonarDistance;
+                 Average_Sum = Average_Sum + Average_4[i];
+                 servoPosition = Average_Sum/6;
+                // Thread::wait(50);
+                }
+            
         sonarDistance_mutex.unlock();
         servoPosition_mutex.unlock();
     }