File content as of revision 11:f4639eb91e5f:

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




//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;
    
  
    
    while(true)
        {
        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_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
        }//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;  
            }         
    }
}


void msd_task(void const *) {
    
    USBHostMSD msd("usb2");
    
  
        
        // try to connect a MSD device
        while(!msd.connect()) {
            Thread::wait(500);
        }
         // append a file
            FILE * fp = fopen("/usb2/test.csv", "w");
            for (int i=0; i<2000; i++)  {
                    
                    
                    sonarDistance_mutex.lock();                             //Mutex lock for the sonar value
                    fprintf(fp,"%.2f\n",sonarDistance);
                    sonarDistance_mutex.unlock();                           //Mutex unlock for the sonar value
                    
            }
            fclose(fp);

}

int main(){
    sonarDistance = 0.0f;
    servoPosition = 0.0f;
    
    
    Thread sonarSensor_thread(sonarSensor);
    Thread servoControl_thread(servoControl);
    Thread logic_thread(logic);
    Thread display_thread(display);
    Thread LED_meter_thread(LED_meter);
    Thread msdTask(msd_task, NULL, osPriorityNormal, 1024 * 4);
    
    while(true)
        {
        }    
    }