File content as of revision 0:9a36dbb02bc8:

/*
 * 4WD_OMNI  by Tomoki Hokida
 * Every direction Type
 *
 */

#include "mbed.h"
#include <math.h>

#define PI 3.141592

Serial pc(USBTX,USBRX);
DigitalOut check(LED1);
PwmOut wh[4] = {p21,p22,p23,p24};

void moveOmni(int ox,int oy)
{
    double trans[4] = {0};

    trans[0] = ox*(cos(0.75*PI)) + oy*(sin(0.75*PI));
    trans[1] = ox*(cos(-0.75*PI)) + oy*(sin(-0.75*PI));
    trans[2] = ox*(cos(-0.25*PI)) + oy*(sin(-0.25*PI));
    trans[3] = ox*(cos(0.25*PI)) + oy*(sin(0.25*PI));

    for(int i=0;i<4;i++){

        if(trans[i]>100){
            trans[i]=100; 
        }else if(trans[i]<-100){
            trans[i]=-100;
        }

        wh[i] = (trans[i]/100.0);        
    }

    pc.printf("motor:%f, motor:%f, motor:%f, motor:%f\n",trans[0],trans[1],trans[2],trans[3]);
}

int main()
{
    int ix,iy;

    for(;;){
        ix=0;
        iy=0;

        //ex value 
        switch(pc.getc()){
            case '1': ix = iy = -100; break;
            case '2': ix = iy = 0; break;
            case '3': ix = iy = 100; break;        
        }    
        moveOmni(ix,iy);  
        
        check = !check; 
    }

}