File content as of revision 1:02b46ad7dcac:

#include "mbed.h"

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

DigitalOut m1a(p29), m1b(p30), m2a(p27), m2b(p28);
PwmOut m1p(p26), m2p(p25);
PwmOut m3p(p23), m4p(p24);

void motor (int num, float speed, int brake) {
    if (num == 1) {
        if (brake) {
            m1p = 1;
            m1a = 1;
            m1b = 1;
        } else
        if (speed > 0) {
            m1p = speed;
            m1a = 1;
            m1b = 0;
        } else
        if (speed < 0) {
            m1p = - speed;
            m1a = 0;
            m1b = 1;
        } else {
            m1p = 1;
            m1a = 0;
            m1b = 0;
        }
    } else
    if (num == 2) {
        if (brake) {
            m2p = 1;
            m2a = 1;
            m2b = 1;
        } else
        if (speed > 0) {
            m2p = speed;
            m2a = 1;
            m2b = 0;
        } else
        if (speed < 0) {
            m2p = - speed;
            m2a = 0;
            m2b = 1;
        } else {
            m2p = 1;
            m2a = 0;
            m2b = 0;
        }
    }
}

void servo (int num, float deg) {
    if (num == 3) {
        m3p.period_ms(20);
        m3p.pulsewidth_us(1500 + (int)(500.0 * deg));
    } else
    if (num == 4) {
        m4p.period_ms(20);
        m4p.pulsewidth_us(1500 + (int)(500.0 * deg));
    }
}

int main() {
    float f;

    while(1) {
        led1 = 1;
        for (f = 0; f < 1; f += 0.02) {
            motor(1, f, 0);
            motor(2, f, 0);
            servo(3, f);
            servo(4, f);
            wait(0.1);
        }
        led1 = 0;
        led2 = 1;
        for (f = 1; f > -1; f -= 0.02) {
            motor(1, f, 0);
            motor(2, f, 0);
            servo(3, f);
            servo(4, f);
            wait(0.1);
        }
        led2 = 0;
        led3 = 1;
        for (f = -1; f < 0; f += 0.02) {
            motor(1, f, 0);
            motor(2, f, 0);
            servo(3, f);
            servo(4, f);
            wait(0.1);
        }
        led3 = 0;
        led4 = 1;
        motor(1, 0, 1);
        motor(2, 0, 1);
        wait(1);
        led4 = 0;
    }
}