File content as of revision 4:17d1343916ec:

#include "mbed.h"
/*
MOTOR A
ENA - p21
IN1 - p5    --> OUT1 - MOTORA+
IN2 - p6    --> OUT2 - MOTORA-
MOTOR B
ENB - p22
IN3 - p26   --> OUT3 - MOTORB+
IN4 - p25   --> OUT4 - MOTORB-
*/

Timeout systicker;

PwmOut in1(p5);
PwmOut in2(p6);

PwmOut in3(p26);
PwmOut in4(p25);
PwmOut block(p24);

DigitalOut en_a(p21);
DigitalOut en_b(p22);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

AnalogIn collision(p15);

DigitalOut sirene(p19);
DigitalOut lights(p20);

Serial bluetooth(p9, p10);
//Serial pc(USBTX, USBRX);

static int control_state = 0; // When 0, allow bluetooth control, When 1 disable bluetooth control

void reset_LED()
{
    led1.write(0);
    led2.write(0);
    led3.write(0);
    led4.write(0);
}

void TimeOut_handler()
{
    in1.write(0.0);
    in2.write(0.0);
    in3.write(0.0);
    in4.write(0.0);
    reset_LED();
}

/* Initialize timers for the H-bridge*/
int init_drive()
{
// Set PWM frequency to 10 kHz
    in1.period_us(100);
    in2.period_us(100);
    in3.period_us(100);
    in4.period_us(100);
    
// Set Block frequency of Siren to 2 Hz with DC of 50%
    block.period(1.0f);
    block.write(0.50f);

    in1.write(0.0);
    in2.write(0.0);
    in3.write(0.0);
    in4.write(0.0);
    /* Set enable pin high. NEVER set this pins low during driving.
    There are no protection diodes and with EN low, H-bridge is in Hi-Z mode.
    This can generate a high voltage that breaks the H-bridge.*/
    en_a.write(1);
    en_b.write(1);

    return 1;
}

void rxCallback()
{
    static int sirene_state = 0;
    static int lights_state = 0;

    if (control_state == 1)
    { 
        return;
    }
    
    float time_out_sec = 0.1;
    char c;
    c = bluetooth.getc();
    switch (c) {
            // Forward direction
        case 'W':
            reset_LED();
            led1.write(1);
            in1.write(1.0);
            in2.write(0.0);
            in3.write(0.0);
            in4.write(1.0);
            systicker.attach(&TimeOut_handler, time_out_sec);
            break;
            // Backward direction
        case 'S':
            reset_LED();
            led2.write(1);
            in1.write(0.0);
            in2.write(1.0);
            in3.write(1.0);
            in4.write(0.0);
            systicker.attach(&TimeOut_handler, time_out_sec);
            break;
            // Left direction
        case 'A':
            reset_LED();
            led3.write(1);
            in1.write(1.0);
            in2.write(0.0);
            in3.write(0.0);
            in4.write(0.0);
            systicker.attach(&TimeOut_handler, time_out_sec);
            break;
        case 'D':
            reset_LED();
            led4.write(1);
            in1.write(0.0);
            in2.write(0.0);
            in3.write(0.0);
            in4.write(1.0);
            systicker.attach(&TimeOut_handler, time_out_sec);
            break;
        case 'g':
            if (sirene_state == 0) {
                sirene.write(0);
                sirene_state = 1;
            } else {
                sirene.write(1);
                sirene_state = 0;
            }
            break;
        case 'k':
            if (lights_state == 0) {
                lights.write(1);
                lights_state = 1;
            } else {
                lights.write(0);
                lights_state = 0;
            }
            break;
    }
}

/* Initialize the bluetooth module*/
int init_bluetooth()
{
    bluetooth.baud(9600);
    bluetooth.attach(&rxCallback, Serial::RxIrq);
    return 1;
}

int main()
{
    float value=0.0;

    sirene.write(1); // Is switched with PNP --> High is off
    lights.write(0); // Is directly connected to IO-pin --> Low is off

    init_drive();
    init_bluetooth();

    /** Collision Low == 1.2 V
        Collision High == 1.7V
        Trigger on 1.5V -> 0.45
    */
    while(1) {
        value = collision.read();
        if (value >= 0.45) {
            control_state = 1; // Disable bluetooth control
            // Drive backwards for a second
            led2.write(1);
            led3.write(1);
            led4.write(1);
            led1.write(1);
            in1.write(0.0);
            in2.write(1.0);
            in3.write(1.0);
            in4.write(0.0);
            wait (1);
            reset_LED(); 
            control_state = 0; // Allow bluetooth control
        } else {
            // do nothing
        }

    }
}