File content as of revision 4:ff05e1fe33ce:

#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
            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(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
        int value_to_steer_UCL = 1; 
        int value_to_steer_LCL = -1; 
        int val1=0;
        int val2=0;
        int val3=0;
        int val4=0;
        int val5=0;
        val1 = mean_measured1[1] - mean_measured1[0];
        val2 = mean_measured2[1] - mean_measured2[0];
        val3 = mean_measured3[1] - mean_measured3[0];
        val4 = mean_measured4[1] - mean_measured4[0];
        val5 = mean_measured5[1] - mean_measured5[0];
        
        if((val2 < value_to_steer_LCL) && (val4 > value_to_steer_UCL)){
            //turn right
            }
        else if((val2 > value_to_steer_UCL) && (val4 < value_to_steer_LCL)){
            //turn left
            }
        else {
            // drive
            }
        
        //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 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;
    }