File content as of revision 20:dfda3dbb9007:

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

JY901 jy(PB_9, PB_8); //sda, scl

PID angle(2.0,0,0,0.01);
Serial pc(USBTX, USBRX, 115200);
SerialMultiByte arduino(PC_12,PD_2);

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(3);

void setup(){
        arduino.setHeaders(127,127);
        arduino.startReceive(5);
        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,now_angle;
    jy.calibrateAll(50);
    uint8_t weight[5];
    angle.setInputLimits(-180.0,180.0);
    angle.setOutputLimits(-1,1);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    while (true) {
        led2=1;
        arduino.getData(weight);
        for (uint8_t i = 0; i < 4; i++) {
            pc.printf("%d",weight[i]);
            pc.printf("\t");
        }

        y = weight[0] + weight[2] - weight[1] - weight[3];
        x = weight[0] + weight[1] - weight[2] - weight[3];
        if(y < -50){
            y = -0.5;
        }else if(y > 50){
            y = 0.5;
        }else{
            y = 0;
        }
        if(x < -50){
            x = -0.5;
        }else if(x > 50){
            x = 0.5;
        }else{
            x = 0;
        }
        now_angle = jy.getZaxisAngle();
        
        if(now_angle>180 && now_angle<360){
            now_angle = now_angle-360;
        }
        angle.setProcessValue(now_angle);
        omni.computeXY(
            x,
            y,
            0,
            0,
            angle.compute()
            
        );
        for(int i=0; i<3; i++){
            motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
        }
        //pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
    }
}