File content as of revision 0:f2830b957dea:

#include "mbed.h"

/*
    serialピン配置
    {TX(motherマイコン側はRX),RX,VIN,GND}
*/
#define PI 3.14159265359
#define dataNum 5
#define SOLADDR 0xc1

class Motor
{
    private:
        I2C i2c;
        char addr;
    public:
        void setAddr(int addr_){
            addr = addr_;
        }
        void init(char addr,int freq = 100000){
            setAddr(addr);
            i2c.frequency(freq);
            char d = 0;
            i2c.write((addr+1) << 1,&d,1);
            wait(0.01);
        }
        Motor(PinName sda,PinName scl,int addr):i2c(sda,scl){
            init(addr);
        }
        Motor(I2C& i2c_,int addr):i2c(i2c_){
            init(addr);
        }
        Motor& operator=(float fval) {
            if(fabs(fval) < 0.1f)   free();
            else{
                if(fval < 0)        run(1,char(-1*fval*255));
                else if(fval > 0)   run(0,char(fval*255));
            }
            return *this;
        }
        bool run(char mode,char speed){
            bool f = 0;char d = speed;
            if(mode == 0 || mode == 1)  f = i2c.write((addr+mode) << 1,&d,1);
            wait(0.01);
            return f;
        }
        bool stop(){
            return run(1,0);
        }
        bool free(){
            return run(0,0);
    }
};

class omuni{
    private:
        I2C i2c;
        Motor m1,m2,m3;
        char RXData[dataNum];
        int8_t map(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax) {
            // check it's within the range
            if (inMin<inMax) {
                if (in <= inMin)
                    return outMin;
                if (in >= inMax)
                    return outMax;
            } else { // cope with input range being backwards.
                if (in >= inMin)
                    return outMin;
                if (in <= inMax)
                    return outMax;
            }
            // calculate how far into the range we are
            float scale = float(in-inMin)/float(inMax-inMin);
            // calculate the output.
            return int8_t(outMin + scale*float(outMax-outMin));
        }
    public:
        omuni(I2C& i2c_,int8_t addr1,int8_t addr2,int8_t addr3)
        :i2c(i2c_),m1(i2c_,addr1),m2(i2c_,addr2),m3(i2c_,addr3)
        {
            RXData[0] = 'H';
            RXData[1] =  0 ;
            RXData[2] =  0 ;
            RXData[3] =  0 ;
            RXData[4] =  0 ;
        }
        void out(char rxdata[dataNum]){
            signed char x1 = map(rxdata[1],-90,90,-100,100);
            signed char y1 = map(rxdata[2],-90,90,-100,100);
            signed char x2 = map(rxdata[3],-90,90,-100,100);
            signed char y2 = map(rxdata[4],-90,90,-100,100);
            float r1 = pow(x1*x1+y1*y1,0.5)/168;
            float r2 = pow(x2*x2+y2*y2,0.5)/168;
            float sieta1 = atan2(double(x1),double(y1));
            float sieta2 = atan2(double(x2),double(y2));
            if(r2 < 0.1F)r2 = 0;
            float alpha = PI/2;
            float x_2 = cos(sieta2);
            float y_2 = sin(sieta2);
            m1 = float(sin(alpha)*x2*0.01);
            m2 = float(sin(alpha+4.0/3*PI)*x2 - cos(alpha+4.0/3*PI)*y2)*0.01;
            m3 = float(sin(alpha+2.0/3*PI)*x2 - cos(alpha+2.0/3*PI)*y2)*0.01;
        }
};



//上は後にライブラリ化

I2C i2c(p28, p27);
omuni omu(i2c, 0x10, 0x12, 0x14);

int ConData[2][12];

BusOut led(LED1,LED2,LED3,LED4);

Serial con(p9, p10);
Serial master(p13,p14);
Serial pc(USBTX, USBRX);

void GetData()
{
    if ( con.getc() == 'H' ) { // ヘッダ文字を見つけたところから読み取る
         ConData[0][0] = 'H';
         for (int i = 1; i < 12; i++)
         {
             char t = (char)con.getc();
             ConData[0][i] = t;
         }
         for (int i = 0; i < 12; i++)
         {
             char t = (char)con.getc();
             ConData[1][i] = t;
         }
    }
}


int main() {
        con.baud(115200);
        master.baud(115200);
        pc.baud(115200);
        con.attach(&GetData,Serial::RxIrq);
        
        char soldata = 0;
        
        while(1){
            
            char MotorData[] = {'H', ConData[0][1]-1, ConData[0][2]-10, ConData[1][1]+1, ConData[1][2]-8};
            omu.out(MotorData);
            
            for(int i = 0; i < 12; i++)
            {
                    master.putc(ConData[0][i]);
            }
            for(int i = 0; i < 12; i++)
            {
                    master.putc(ConData[1][i]);
            }
            
            soldata = (ConData[0][3] << 2) + ConData[1][3];
            i2c.write(SOLADDR, &soldata, 1);
        }   
}