File content as of revision 0:4ed9b2004dd0:

#include "mbed.h"

#define PI 3.141592

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

const int  addr[4] = { 0x10, 0x23, 0x56, 0x76 };
int     i,dig,i2cVAL,error_val=0;
//double    rad, prePwmDuty[4], PwmDuty[4];

int main()
{
    char          data[5] = { 0 },MDFL[2],MDFR[2],MDRL[2],MDRR[2];
    int     i,dig,i2cVAL,error=0;
    double    rad, prePwmDuty[4], PwmDuty[4];
    i2c.frequency(300000);  // I2Cの通信速度設定
    FEP02.baud(19200);// シリアルのボーレートを19200に設定
    while(1) {
        if(FEP02.getc() == 255) {
            for(i = 0; i < 5; i++) {
                data[i] = FEP02.getc();
                error_val = 0;
            }
        }  else {
            error_val++;
        }
        dig = data[0] + data[1] * 128;// 受信データから元の角度に変換
        rad = (dig / 180.0) * PI;   // radianに変換
        if(error > 10) {
            dig = 360;
            pc.printf("error\r\n");
        }
        if(dig >= 360) {
            prePwmDuty[0] = 0;
            prePwmDuty[1] = 0;
            prePwmDuty[2] = 0;
            prePwmDuty[3] = 0;
        } else {
            // 各モータへの出力を算出
            prePwmDuty[0] = 1.41421356 * sin(rad - ((5 / 4) * PI));
            prePwmDuty[1] = 1.41421356 * sin(rad - (PI / 4));
            prePwmDuty[2] = prePwmDuty[0];
            prePwmDuty[3] = prePwmDuty[1];
        }
        for(i = 0; i < 4; i++) {
            // 値を-1.0~1.0の範囲だからその範囲に収める
            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;
        }
        PwmDuty[2] = 1 - PwmDuty[2];
        PwmDuty[3] = 1 - PwmDuty[3];
        MDFR[0] = PwmDuty[0]*255;
        MDRR[0] = PwmDuty[1]*255;
        MDRL[0] = PwmDuty[2]*255;
        MDFL[0] = PwmDuty[3]*255;
        i2cVAL   += i2c.write(addr[0], MDFR, 2, false);
        i2cVAL   += i2c.write(addr[1], MDRR, 2, false);
        i2cVAL   += i2c.write(addr[2], MDRL, 2, false);
        i2cVAL   += i2c.write(addr[3], MDFL, 2, false);
        if(i2cVAL) {
            leds[2] = 1;
        } else {
            leds[2] = 0;
        }
        printf("%d,%d,%d,%d,%d\r\n", dig, error_val, MDFR[0], MDRL[0], data[4]);
    }
}