File content as of revision 5:3eed67b60cd2:

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

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

Serial pc(USBTX, USBRX, 115200);
SerialMultiByte arduino(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(){
        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;
    //jy.calibrateAll(50);
    uint8_t weight[5];
    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;
        }
         
        omni.computeXY(
            x,
            y,
            0,
            0,
            0
            //(pad.getTrigger(0) - pad.getTrigger(1)) / 255.0 * 0.8
        );
        for(int i=0; i<3; i++){
            motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
        }
        if(weight[0]>40 && weight[3]>40){
            for(int i=0; i<3; i++){
                motor[i].setMotorSpeed(0.2);
            }
        }
        
        if(weight[1]>40 && weight[2]>40){
            for(int i=0; i<3; i++){
                motor[i].setMotorSpeed(-0.2);
            }
        }
        pc.printf("X = <%f>, Y = <%f>\r\n",x,y);
    }
}