File content as of revision 10:6059097698f7:

#include "mbed.h"
#include "rtos.h"
#include "C12832_lcd.h"

//File Access
LocalFileSystem local("local");

//Timer
Timer t;

//Varible
float sonarDistance;
float servoPosition;

//Mutex
Mutex sonarDistance_mutex;                                      //Init Mutex for sensor value
Mutex servoPosition_mutex;                                      //Init Mutex for servo value

//I/O    
AnalogIn sonarPin(p17);                                         //Assign pin to read sonar sensor
PwmOut servoPin(p21);                                           //Assign pin for servo output
PwmOut myled1(LED1);                                        //Assign pin for LED1
PwmOut myled2(LED2);                                        //Assign pin for LED2
PwmOut myled3(LED3);                                        //Assign pin for LED3
PwmOut myled4(LED4);                                        //Assign pin for LED4

//LCD Setup
C12832_LCD lcd;                                                 //setup LCD screen

void sonarSensor(void const *args){
    while(true){
        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();                             //Mutex lock for the Servo value
            servoPin.write(servoPosition/2);                    //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)
    {
    int Size=16;
    float Average_4[16];                                        //number of value in the array
    float Average_Sum=0;
    bool exportDebugData = true;
    float debugDataTimeLimit = 60.0f;
    FILE *graphcsv = fopen("/local/graph.csv","w");
  
    
    while(true)
        {
        for(int i=0;i<Size;i++)
            {
             if (exportDebugData){
                fprintf(graphcsv, "%f,",t.read());              // Print time to file;
             }
             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
                 if (exportDebugData){
                    fprintf(graphcsv, "%f,",sonarDistance);     // Write sonarDistance to file
                 }
             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_mutex.lock();                        //Mutex lock for the servo value             
                 servoPosition = Average_Sum/Size;              //Divide the array by the number of element in the array
                 if (exportDebugData){
                    fprintf(graphcsv, "%f\n",servoPosition);    // Write servoPosition to file
                 }
             servoPosition_mutex.unlock();                      //Mutex unlock for the servo value
            
            if(t.read() <= debugDataTimeLimit){
                fclose(graphcsv);
                exportDebugData = false;
            }
            }//end for loop
        }//end of while loop
    }//End of thread

void display(void const *args){
    while(true){
        sonarDistance_mutex.lock();                             //Mutex lock for the sonar value
        servoPosition_mutex.lock();                             //Mutex lock for the servo value
            lcd.cls();                                          //Clear the lcd
            lcd.locate(0,0);                                    //Cursor postion on the lcd
            lcd.printf("Sonar : %3.6f \nServo : %3.6f \nSCIENCE!",sonarDistance,servoPosition);
        sonarDistance_mutex.unlock();                           //Mutex unlock for the sonar value
        servoPosition_mutex.unlock();                           //Mutex unlock for the servo value
        Thread::wait(200);                                      //Thread Wait
            
    }
}

void LED_meter(void const *args) {
    
    float LED_distance = 0;                                     //Create var to store sonar distance
    
    while(true) {
        sonarDistance_mutex.lock();                             //Mutex lock for the sonar value
        LED_distance=sonarDistance;                             //Store value of sonar sensor in LED_distance
        sonarDistance_mutex.unlock();                           //Mutex unlock for the sonar value
        
        if(LED_distance<=0.15)
            {
            myled1.write(((LED_distance-0.15)*-1)*20);          //PWM the LED from 0% to 100%
            }
        else
            {
            myled1 = 0;                                         //Turn off LED
            }
            
        if(LED_distance<=0.10)
            {
            myled2.write(((LED_distance-0.10)*-1)*20);          //PWM the LED from 0% to 100%
            }
        else
            {
            myled2 = 0;                                         //Turn off LED
            }
 
        if(LED_distance<=0.05)
            {
            myled3.write(((LED_distance-0.05)*-1)*40);          //PWM the LED from 0% to 100%
            }
        else
            {
            myled3 = 0;                                         //Turn off LED
            }       
       
        if(LED_distance<=0.025)
            {
            myled4.write(((LED_distance-0.025)*-1)*40);         //PWM the LED from 0% to 100%
            //s2=0.1;  
            }
        else
            {
            myled4 = 0;                                         //Turn off LED
            //s2=1;  
            }         
    }
}

int main(){
    sonarDistance = 0.0f;
    servoPosition = 0.0f;
    
    t.start();
    
    Thread sonarSensor_thread(sonarSensor);
    Thread servoControl_thread(servoControl);
    Thread logic_thread(logic);
    Thread display_thread(display);
    Thread LED_meter_thread(LED_meter);
    
    while(true)
        {
        }    
    }