File content as of revision 5:0382ed1bf13d:

#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 chan);

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
            rawUS_data[iCount] = getPing();  //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(1);    //calc mean 1
        mean_measured2[0] = measurement_mean(2);    //calc mean 2
        mean_measured3[0] = measurement_mean(3);    //calc mean 3
        mean_measured4[0] = measurement_mean(4);    //calc mean 4
        mean_measured5[0] = measurement_mean(5);    //calc mean 5
        
        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;
    
    Trig = HI;          //start positive edge of trigger
    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 t){
    if (t++>=0&&s<=2) {steering.pulsewidth(t/2000+0.001);}                      // calculate steering angle to pulsewidth conversion
    telemUpdate();                                                              // save telem file after angle change
    return;    
}
    
void drive(float v){
    if (v++>=0&&s<=2) {velocity.pulsewidth(v/2000+0.001);}                      // calculate velocity to pulsewidth conversion
    telemUpdate(0.0);                                                           // save telem file after speed change
    return;
}

void stop(void){
    drive(0.0);                                                                 // Reduce speed to 0
    return;
}

int measurement_mean(int chan){
    int value=0;
    int sum = 0;
    switch (chan){
        case 1:
            for(int i=0;i<=MAX;i++){
                sum += US1_mean[i];         //sum all elements of US1 window
                }
            value = sum/MAX;                //calculate mean for US1
            return (int)value;              //return using type casting
        case 2:
            for(int i=0;i<=MAX;i++){
                sum +=US2_mean[i];          //sum all elements of US2 window
                }
            value = sum/MAX;                //calculate mean for US2
            return (int)value;              //return using type casting
        case 3:
            for(int i=0;i<=MAX;i++){
                sum +=US3_mean[i];          //sum all elements of US3 window
                }
            value = sum/MAX;                //calculate mean for US3
            return (int)value;              //return using type casting
        case 4:
            for(int i=0;i<=MAX;i++){
                sum +=US4_mean[i];          //sum all elements of US4 window
                }
            value = sum/MAX;                //calculate mean for US4
            return (int)value;              //return using type casting
        case 5:
            for(int i=0;i<=MAX;i++){
                sum +=US5_mean[i];          //sum all elements of US5 window
                }
            value = sum/MAX;                //calculate mean for US5
            return (int)value;              //return using type casting
        }
    
    return 0;
    }