File content as of revision 0:dc7bdd0a1b32:

#include "mbed.h"
#include <algorithm>

double voltageToDistance(uint16_t);
void setPwmBoth(float);

double FilterRechts(double);
double FilterVoor(double);
double FilterBeneden(double);
double FilterLinks(double);

AnalogIn sensorA1(A2); //Sensor 1
AnalogIn sensorA2(A3); //Sensor 2
AnalogIn sensorA3(A4); //Sensor 3
AnalogIn sensorA4(A5); //Sensor 4

//H-brug poorten (pwm)
PwmOut pwmL1(PC_8);
PwmOut pwmL3(PB_4);

//Duty cycle
float dutyCycle1 = 0.0f;
float dutyCycle2 = 0.0f;

void setPwmBoth(float pwmFloat) {
    dutyCycle1 = pwmFloat;
    dutyCycle2 = pwmFloat;
    pwmL1 = dutyCycle1;
    pwmL3 = dutyCycle2;
}

double FilterRechts(double sensorValue) {
    static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
    static int arrayPosition = 0;
    double average = 0;
    sensorArray[arrayPosition] = sensorValue;
    arrayPosition ++;
    for(int i = 0; i < 10; i++) {
        average += sensorArray[i];
    }
    average /= 10;
    if(arrayPosition == 10) {
        arrayPosition = 0;
    }
    printf("%f\n", average);
    return average;
}
double FilterVoor(double sensorValue) {
    static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
    static int arrayPosition = 0;
    double average = 0;
    sensorArray[arrayPosition] = sensorValue;
    arrayPosition ++;
    for(int i = 0; i < 10; i++) 
    {
        average += sensorArray[i];
    }
    average /= 10;
    if(arrayPosition == 10) 
    {
    arrayPosition = 0;
    }
    printf("%f\n", average);
    return average;
}
double FilterBeneden(double sensorValue) {
    static double sensorArray[10] = {14, 14, 14, 14, 14, 14, 14, 14, 14, 14};
    static int arrayPosition = 0;
    double average = 0;
    sensorArray[arrayPosition] = sensorValue;
    arrayPosition ++;
    for(int i = 0; i < 10; i++) 
    {
        average += sensorArray[i];
    }
    average /= 10;
    if(arrayPosition == 10) 
    {
        arrayPosition = 0;
    }
    printf("%f\n", average);
    return average;
}
double FilterLinks(double sensorValue) {
    static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
    static int arrayPosition = 0;
    double average = 0;
    sensorArray[arrayPosition] = sensorValue;
    arrayPosition ++;
    for(int i = 0; i < 10; i++) 
    {
        average += sensorArray[i];
    }
    average /= 10;
    if(arrayPosition == 10) 
    {
        arrayPosition = 0;
    }
    printf("%f\n", average);
    return average;
}

double voltageToDistance(uint16_t voltage) {
    if (voltage < 16) 
    {
        voltage = 16;
    }
    double distance = 0;
    distance = 2076.0 / (voltage - 11.0);
    return distance * 100;
}

int main(){
    enum State
    {
        initialize,
        wait,
        autonomous,
        HmoveForward,
        HturnLeft,
        HturnRight,
        Hreverse,
        neutral,
        AmoveForward,
        Aleft,
        Aright,
        AturnLeft,
        AturnRight
    };
    
    //Digitale H-brug poorten
    DigitalOut L2(PB_8);
    DigitalOut E1(PC_9);
    DigitalOut L4(PB_14);
    DigitalOut E2(PB_15);
    
    //Statemachine
    State stateMachine;
    stateMachine = initialize;
    
    //Serieele poorten
    Serial pc(USBTX, USBRX);
    Serial bluetooth(PA_11, PA_12); // TX, RX
    
    //Booleans om afgelopen statussen te onthouden
    bool fromForward = false;
    bool fromReverse = false;
    bool status = false; //status false = autonoom, true = handmatig
    
    //Timer voor het uitrijden van de bluetooth
    Timer bluetoothTimer;
    Timer autoTimer;
    
    int movingDelay = 25;
    int cutoff = 30;
    int detection = 25;
    
    while (1)
    {
        switch(stateMachine)
        {
            case initialize:
                pwmL1.period_ms(10);
                pwmL3.period_ms(10);
                pwmL1 = dutyCycle1;
                pwmL3 = dutyCycle2;
                bluetooth.baud(115200);
                pc.baud(115200);
                pc.printf("Bluetooth Start\r\n");
                stateMachine = wait;
                break;
                
            case wait:
                if (bluetooth.readable())
                {
                    switch(bluetooth.getc()) 
                    {
                        case 30:
                            status = true;
                            bluetoothTimer.start();
                            break;
                            
                        case 40:
                            status = false;
                            stateMachine = autonomous;
                            break;
                            
                        case 49:
                            bluetoothTimer.reset();
                            if(status == true) 
                            {
                                stateMachine = HmoveForward;
                            }
                            break;
                            
                        case 47:
                            bluetoothTimer.reset();
                            if(status == true) 
                            {
                                stateMachine = HturnLeft;
                            }
                            break;
                            
                        case 51:
                            bluetoothTimer.reset();
                            if(status == true) 
                            {
                                stateMachine = HturnRight;
                            }
                            break;
                            
                        case 55:
                            bluetoothTimer.reset();
                            if(status == true) {
                                stateMachine = Hreverse;
                            }
                            break;
                    }
                } 
                else if (status == false) 
                {
                    stateMachine = autonomous;
                } 
                else if (status == true) 
                {
                    stateMachine = neutral;
                }
                break;
                
            case autonomous:
                double SensorRechts  = voltageToDistance(sensorA1.read_u16());
                double SensorVoor    = voltageToDistance(sensorA2.read_u16());
                double SensorBeneden = voltageToDistance(sensorA3.read_u16());
                double SensorLinks   = voltageToDistance(sensorA4.read_u16());
                
                if(SensorRechts  > cutoff) {SensorRechts  = cutoff;}
                if(SensorVoor    > cutoff) {SensorVoor    = cutoff;}
                if(SensorBeneden > cutoff) {SensorBeneden = cutoff;}
                if(SensorLinks   > cutoff) {SensorLinks   = cutoff;}
                
                SensorRechts  = FilterRechts(SensorRechts);
                SensorVoor    = FilterVoor(SensorVoor);
                SensorBeneden = FilterBeneden(SensorBeneden);
                SensorLinks   = FilterLinks(SensorLinks);
                
                if(SensorBeneden > 17 || SensorBeneden < 11) 
                {
                    stateMachine = AturnLeft;                                                           //Draai naar links als er een afgrond is of een obstakel ligt
                } 
                else if(SensorRechts > detection && SensorVoor > detection && SensorLinks > detection) 
                {
                    stateMachine = AmoveForward;                                                        //Naar voren als sensoren niks zien.
                } 
                else if(SensorRechts < detection && SensorVoor > detection && SensorLinks > detection) 
                {
                    stateMachine = Aleft;                                                               //Naar links afbuigen als een schuine sensor een object ziet.
                } 
                else if(SensorRechts > detection && SensorVoor > detection && SensorLinks < detection) 
                {
                    stateMachine = Aright;                                                              //Naar rechts afbuigen als een schuine sensor een object ziet.
                } 
                else if(SensorVoor < detection && SensorLinks > detection) 
                {
                    stateMachine = AturnLeft;                                                           //Naar links draaien als de sensor naar voren iets ziet (rechts maakt niet uit)
                } 
                else if(SensorVoor < detection && SensorLinks < detection) 
                {
                    stateMachine = AturnRight;                                                          //Naar rechts draaien als de sensoren naar voor en links iets zien
                }
                break;
                
            case HmoveForward:
                fromForward = true;
                if(fromReverse == true) 
                {
                    setPwmBoth(0.0f);
                    fromReverse = false;
                }
                if(dutyCycle1 <= 0.99f && status == true) 
                {
                    E2 = true;
                    L4 = false;
                    E1 = true;
                    L2 = false;
                    dutyCycle1 += 0.05f;
                    dutyCycle2 += 0.05f;
                    pwmL1 = dutyCycle1;
                    pwmL3 = dutyCycle2;
                }
                stateMachine = wait;
                break;
                
            case HturnLeft:
                E1 = true;
                L2 = true;
                E2 = true;
                L4 = false;
                dutyCycle1 = dutyCycle2 = 0.5f;
                pwmL1 = pwmL3 = dutyCycle1;
                stateMachine = wait;
                break;
                
            case HturnRight:
                E1 = true;
                L2 = false;
                E2 = true;
                L4 = true;
                dutyCycle1 = dutyCycle2 = 0.5f;
                pwmL1 = pwmL3 = dutyCycle1;
                stateMachine = wait;
                break;
                
            case Hreverse:
                fromReverse = true;
                E2 = true;
                L4 = true;
                E1 = true;
                L2 = true;
                if(fromForward == true) 
                {
                    setPwmBoth(1.0f);
                    fromForward = false;
                }
                dutyCycle1 -= 0.05f;
                dutyCycle2 -= 0.05f;
                pwmL1 = dutyCycle1;
                pwmL3 = dutyCycle2;
                stateMachine = wait;
                break;
                
            case neutral:
                if(bluetoothTimer.read_ms() > 100) {
                    E2 = false;
                    L4 = false;
                    pwmL1 = false;
                    pwmL3 = false;
                    E1 = false;
                    L2 = false;
                    if     (fromForward == true){setPwmBoth(0.0f);} 
                    else if(fromReverse == true){setPwmBoth(1.0f);}
                }
                stateMachine = wait;
                break;
                
            case AmoveForward:
                autoTimer.start();
                while(autoTimer.read_ms() < movingDelay) 
                {
                    E1 = true;
                    L2 = false;
                    E2 = true;
                    L4 = false;
                    setPwmBoth(0.60f);
                }
                autoTimer.stop();
                autoTimer.reset();
                stateMachine = wait;
                break;
                
            case Aright:
                autoTimer.start();
                while(autoTimer.read_ms() < movingDelay) 
                {
                    E1 = true;
                    L2 = false;
                    E2 = false;
                    L4 = false;
                    pwmL3 = false;
                    dutyCycle1 = 0.60f;
                    pwmL1 = dutyCycle1;
                }
                autoTimer.stop();
                autoTimer.reset();
                stateMachine = wait;
                break;
                
            case Aleft:
                autoTimer.start();
                while(autoTimer.read_ms() < movingDelay) 
                {
                    E1 = false;
                    L2 = false;
                    pwmL1 = false;
                    E2 = true;
                    L4 = false;
                    dutyCycle2 = 0.60f;
                    pwmL3 = dutyCycle2;
                }
                autoTimer.stop();
                autoTimer.reset();
                stateMachine = wait;
                break;
                
            case AturnRight:
                autoTimer.start();
                while(autoTimer.read_ms() < movingDelay) 
                {
                    E1 = true;
                    L2 = false;
                    E2 = true;
                    L4 = true;
                    setPwmBoth(0.50f);
                }
                autoTimer.stop();
                autoTimer.reset();
                stateMachine = wait;
                break;
                
            case AturnLeft:
                autoTimer.start();
                while(autoTimer.read_ms() < movingDelay) 
                {
                    E1 = true;
                    L2 = true;
                    E2 = true;
                    L4 = false;
                    setPwmBoth(0.50f);
                }
                autoTimer.stop();
                autoTimer.reset();
                stateMachine = wait;
                break;
                
            default:
                stateMachine = wait;
                break;
        }
    }
}