File content as of revision 6:6ce8adb328fa:

#define BEAT 140
#include "mbed.h"
#include "math.h"
#include "omni_wheel.h"
#include "motorsmlap.h"
#include "SerialMultiByte.h"

Serial pc(USBTX, USBRX, 115200);
SerialMultiByte arduinocontroller(PC_10,PC_11);

PwmOut beep(PA_0);
DigitalOut led1(PA_11);
DigitalOut led2(LED2);

motorSmLap motor[] = {
    motorSmLap(PC_6, PC_9, PC_8),
    motorSmLap(PB_6, PA_9, PC_7),
    motorSmLap(PA_8, PB_4, PB_7)
    };
    
const double PII = 3.14159265358979;
OmniWheel omni(4);

void setup(){
        arduinocontroller.setHeaders(127,127);
        arduinocontroller.startReceive(13);
        
        omni.wheel[0].setRadian(PII/2.0);
        omni.wheel[1].setRadian(7.0/6.0*PII);
        omni.wheel[2].setRadian(11.0/6.0*PII);
        
        beep.period(1.0/3136);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/2960);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/2489);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/1760);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/1661);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/2637);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/3322);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.period(1.0/4186);
        beep.write(0.4f);
        wait_ms(BEAT);
        
        beep.write(0.0f);
        wait_ms(BEAT);  
}

int main() {
    setup();
    float x, y;
    
    uint8_t button[13];
    while (true) {
        led2=1;
        arduinocontroller.getData(button);
        for (uint8_t i = 0; i < 13; i++) {
            pc.printf("%d",button[i]);
            pc.printf("\t");
        }
        
        if(button[0]==1){
            y = -0.5;
        }
        
        if(button[1]==1){
            y = 0.5;
        }
        
        if(button[2]==1){
            x = 0.5;
        }
        
        if(button[3]==1){
            x = -0.5;
        }
         
        omni.computeXY(
            x,
            y,
            0,
            0,
            0
        );
        for(int i=0; i<3; i++){
            motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
        }
        pc.printf("X = <%f>, Y = <%f>\r\n",x,y);
    }
}