File content as of revision 2:22d86efbbada:

#include "mbed.h"


#define MAX 0x0A
#define HI 0x01
#define LO 0x00

DigitalOut led_1(LED1);         //program running.
DigitalOut led_2(LED2);         //sensors operating.
DigitalOut led_3(LED3);         //is moving.
DigitalOut led_4(LED4);         //is complete.
DigitalOut Bit1(p25);
DigitalOut Bit2(p24);
DigitalOut Bit3(p23);
DigitalOut Trig(p6);
DigitalIn Echo(p7);
PwmOut steering(p21);
PwmOut velocity(p22);
 

Timer US;


//global variable dec
int rawUS_data[5]={0,0,0,0,0};  //raw data{chan1,chan2,chan3,chan4,chan5}
int US1_mean[MAX]={0,0,0,0,0,0,0,0,0,0};  
int US2_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
int US3_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
int US4_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
int US5_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
int turn_rate[2]={0,0};
float vo = 0;

//function constructs
void setActiveUS(int chan);             //select sensor
int getPing(void);                      //get measurement
void turn(float s);      //turn car -1 left 0 centre 1 right
void drive(float v);   //drive -1 reverse 1 forward
void stop(void);
int measurement_mean(int *Array);


int main() {
    int iCount = 0;
    int measured = 0;
    int iMean = 0;
    double mean_measured1[2] = {0,0};
    double mean_measured2[2] = {0,0};
    double mean_measured3[2] = {0,0};
    double mean_measured4[2] = {0,0};
    double mean_measured5[2] = {0,0};
    
    while(iMean < 3){
        if(iMean == 2){
            iMean = 0;
            }
        while(iCount <= 5){
            setActiveUS(iCount);            //set mux address
            measured = getPing();           //get measurement
            rawUS_data[iCount] = measured;  //get raw measurement
            iCount +=1;
            wait(0.4);                      //will need calibrating
            }
        US1_mean[0]=rawUS_data[0];          //assign new value
        US2_mean[0]=rawUS_data[1];          //to respective sensor
        US3_mean[0]=rawUS_data[2];
        US4_mean[0]=rawUS_data[3];
        US5_mean[0]=rawUS_data[4];
    
        for(int i=MAX;i>0;i--){
            US1_mean[i] = US1_mean[i-1];    //index data along window
            US2_mean[i] = US1_mean[i-1];    //wrt size of window
            US3_mean[i] = US1_mean[i-1];
            US4_mean[i] = US1_mean[i-1];
            US5_mean[i] = US1_mean[i-1];
            }
        mean_measured1[0] = measurement_mean(&US1_mean);
        mean_measured2[0] = measurement_mean(&US2_mean);
        mean_measured3[0] = measurement_mean(&US3_mean);
        mean_measured4[0] = measurement_mean(&US4_mean);
        mean_measured5[0] = measurement_mean(&US5_mean);
        
        if(iMean == 1){
            mean_measured1[1] = mean_measured1[0];
            mean_measured2[1] = mean_measured2[0];
            mean_measured3[1] = mean_measured3[0];
            mean_measured4[1] = mean_measured4[0];
            mean_measured5[1] = mean_measured5[0];
            }
            
        //if the second value is smaller than the first value
        //steer right
        
        //if the second value is bigger than first value
        //steer left
        
        // with respect to a deadband (error)
        
        //drive at 1 speed
        iMean +=1;
        }   
}

void setActiveUS(int chan){
    switch(chan){
        case 0:
            //ultrasonic 1
            Bit1 = 0;
            Bit2 = 0;
            Bit3 = 0;
            return;
        
        case 1:
            //ultrasonic 2
            Bit1 = 1;
            Bit2 = 0;
            Bit3 = 0;
            return;
            
        case 2:
            //ultrasonic 3
            Bit1 = 0;
            Bit2 = 1;
            Bit3 = 0;
            return;
        
        case 3:
            //ultrasonic 4
            Bit1 = 1;
            Bit2 = 1;
            Bit3 = 0;
            return;
        
        case 4:
            //ultrasonic 5
            Bit1 = 0;
            Bit2 = 0;
            Bit3 = 1;
            return;
        
        }
    }

int getPing(void){
    int result=0;   
                        //start positive edge of trigger
    Trig = HI;
    US.reset();
    wait_us(10.0);      //hold it high for 10us
    Trig = LO;          //set negative going edge
    while(Echo == 0){}; //while receive is LO
    US.start();         //start counting received pulse
    while(Echo == 1){};
    US.stop();          
    result = ((US.read_us()*10)/58);    //pulse duration = distance set to cm
    return result;    

}

void turn(float s){
    s=s+1;
    if (s>=0 && s<=2) {
        steering.pulsewidth(s/2000+0.001);
    }

    return;
    }
    
void drive(float v){
     v=v+1;
    if (v>=0 && v<=2) {
        //no reverse code
        velocity.pulsewidth(v/2000+0.001);
        vo=v;
        }
    return;
    }
    
void stop(void){
    drive(0.0);
    
    return;
    }

int measurement_mean(int *Array){
    int value=0;
    int sum = 0;
    for(int i=0;i<=MAX;i++){
        sum +=Array[i];
        }
    value = sum/MAX;
    return (int)value;
    }