Diff: main.cpp

Revision:

6:4c62f9c91b1d

Parent:

5:e82e00f12634

Child:

7:0fa7430ab812

--- a/main.cpp	Mon Jan 19 15:28:35 2015 +0000
+++ b/main.cpp	Sun Jan 25 22:26:35 2015 +0000
@@ -5,52 +5,53 @@
 float sonarDistance;
 float servoPosition;
 
-Mutex sonarDistance_mutex;
-Mutex servoPosition_mutex;
+Mutex sonarDistance_mutex;                                  //Init Mutex for sensor value
+Mutex servoPosition_mutex;                                  //Init Mutex for servo value
+    
+AnalogIn sonarPin(p17);                                     //Assign pin to read sonar sensor
+PwmOut servoPin(p21);                                       //Assign pin for servo output
 
-AnalogIn sonarPin(p17);
-PwmOut servoPin(p21);
-
-C12832_LCD lcd;
+C12832_LCD lcd;                                             //setup LCD screen
 
 void sonarSensor(void const *args){
     while(true){
-        sonarDistance_mutex.lock();
-            sonarDistance = sonarPin.read();
-        sonarDistance_mutex.unlock();
-        Thread::wait(10);
-    }
-}
+        sonarDistance_mutex.lock();                         //Mutex lock for the Servo value
+            sonarDistance = sonarPin.read();                //Read analog voltage and store in variable
+        sonarDistance_mutex.unlock();                       //Mutex unlock for the Servo value
+        Thread::wait(10);                                   //Pause thread for 10msec
+    }  //End of While loop
+} //End of Thread
 
 void servoControl(void const *args){
     while(true){
-        servoPosition_mutex.lock();
-            servoPin.write(servoPosition);
-        servoPosition_mutex.unlock();
-    }
-}
+        servoPosition_mutex.lock();                         //Mutex lock for the Servo value
+            servoPin.write(servoPosition);                  //Write servo value to servo pin
+        servoPosition_mutex.unlock();                       //Mutex unlock for the Servo value
+    }  //end of while loop
+} //end of thread
 
-void logic(void const *args){
-    while(true){
-        float Average_4[]={0,0,0,0,0,0,0};
+void logic(void const *args)
+    {
+    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();
-        servoPosition_mutex.lock();
-            //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.lock();                         //Mutex lock for the Sonar value
+        servoPosition_mutex.lock();                         //Mutex lock for the servo value
             
-        sonarDistance_mutex.unlock();
-        servoPosition_mutex.unlock();
-    }
-}
+        for(int i=0;i<6;i++)
+            {
+             Average_Sum = Average_Sum-Average_4[i];        //Remove the 4th oldest value from the average sum
+             Average_4[i]= sonarDistance;                   //Add the new value to the array
+             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
+            }//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
 
 void display(void const *args){
     while(true){
@@ -76,6 +77,7 @@
     Thread logic_thread(logic);
     Thread display_thread(display);
     
-    while(true){
+    while(true)
+        {
         }    
     }
\ No newline at end of file