File content as of revision 1:4f5a22371fff:

#include "mbed.h"

#define ratio 0.3
#define conratio (1.0-ratio)

#define PI 3.141592
#define qPI 0.785398

DigitalOut leds[8]={PA_11,PB_12,PB_2,PB_1,PB_15,PB_14,PB_13,PC_4};
I2C i2c(I2C_SDA,I2C_SCL);
Serial FEP02(PC_6,PA_12);
Serial pc(USBTX,USBRX);

char data[5]={0};
bool recFrag=0;
const int addr[4]={0x10,0x23,0x56,0x76};

void ledsReset(){
    int i=0;
    while(i<8){
        leds[i]=0;
        i++;
    }
}

void getConData(){
    int i=0;
    if(FEP02.getc()==255){
        recFrag=1;
        while(i<5){
            data[i]=FEP02.getc();
            i++;
        }
    }
    //pc.printf("Recieved\r\n");
}

void MotorOut(char MDFL[],char MDFR[],char MDRL[],char MDRR[]){
    int dig;
    double rad,turn;
    double prePwmDuty[4],PwmDuty[4];
    int i=0,n=0;
    
    dig = data[0]+data[1]*128;
    rad = (dig/180.0)*PI;
    if(dig==360){
        while(n<4){
            prePwmDuty[n]=0;
            n++;
        }
    }else{
        prePwmDuty[0]=1.41421356*sin(rad+qPI);//*conratio;
        prePwmDuty[1]=1.41421356*sin(rad-qPI);//*conratio;
        prePwmDuty[2]=prePwmDuty[1];
        prePwmDuty[3]=prePwmDuty[0];
    }
    
    turn = (data[2]-63)/64.0;
    //turn = 0;
    
    prePwmDuty[0]+=turn;//*conratio;//*ratio;
    prePwmDuty[1]-=turn;//*conratio;//*ratio;
    prePwmDuty[2]+=turn;//*conratio;//*ratio;
    prePwmDuty[3]-=turn;//*conratio;//*ratio;
    
    while(i<4){
        if(prePwmDuty[i]<-1.0) prePwmDuty[i]=-1.0;
        else if(prePwmDuty[i]>1.0) prePwmDuty[i]=1.0;
        PwmDuty[i]=(fabs(prePwmDuty[i]+1.0))/2;
        i++;
    }
    
    MDFL[0] = PwmDuty[0]*255;
    MDFR[0] = PwmDuty[1]*255;
    MDRL[0] = PwmDuty[2]*255;
    MDRR[0] = PwmDuty[3]*255;
    
    //printf("%f,%f,%f,%f\r\n",PwmDuty[0],PwmDuty[1],PwmDuty[2],PwmDuty[3]);
}

void MotorReset(){
    char data[2]={127,0};
    int i=0;
    while(i<4){
        i2c.write(addr[i],data,2,false);
        i++;
    }
}

void MotorRun(){
    char data[2]={255,0};
    int i=0;
    while(i<4){
        i2c.write(addr[i],data,2,false);
        i++;
    }
}

int main() {
    char MDFL[2],MDFR[2],MDRL[2],MDRR[2];
    int i2cVAL=0;
    int i=0;
    
    i2c.frequency(300000);
    ledsReset();
    FEP02.baud(19200);
    /*
    MotorRun();
    wait(3);
    */
    MotorReset();
    //FEP02.attach(&getConData);
    leds[0]=1;
    
    
    while(1) {
        i2cVAL=0;
        i=0;
        if(FEP02.getc()==255){
            while(i<5){
                data[i]=FEP02.getc();
                i++;
            }
            MotorOut(MDFL,MDFR,MDRL,MDRR);
            
            i2cVAL += i2c.write(addr[0],MDFL,2,false);
            i2cVAL += i2c.write(addr[1],MDFR,2,false);
            i2cVAL += i2c.write(addr[2],MDRL,2,false);
            i2cVAL += i2c.write(addr[3],MDRR,2,false);
            
            printf("%d\r\n",i2cVAL);
            printf("%d,%d,%d,%d\r\n",MDFL[0],MDFR[0],MDRL[0],MDRR[0]);
            leds[0]=!leds[0];
            leds[1]=1;
            if(i2cVAL) leds[2]=1;
            else leds[2]=0;
        }
        leds[1]=0;            
    }
}