File content as of revision 4:076faa2578a4:

/*
 * 4WD_OMNI  by Tomoki Hokida
 * Every direction Type
 * Fujisawaリスペクトの4輪オムニ用ラフコード
 */

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

#define PI 3.141592

Serial pc(USBTX,USBRX);
DigitalOut check(LED1);
DigitalOut motor[8] = {p13,p14,p15,p16,p17,p18,p19,p20};

BusInOut sw(p5, p6, p7, p8);

PwmOut pwm[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]>0){
                motor[i*2] = 1;
                motor[i*2+1] = 0;                               
                }else{
                motor[i*2+1] = 1;
                motor[i*2] = 0;        
            }          
            trans[i] = fabs(trans[i]);
            if(trans[i]>100){
            trans[i]=100;}
            pwm[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;
        
        sw.input();
        sw.mode(PullUp);
        
        //ex value 
        switch(sw){
            case 0xF: ix = iy = 0; break;
            case 0xE: ix = iy = 100; break;
            case 0xD: ix = iy = 50; break;
            case 0xB: ix = iy = -100; break;        
            case 0x7: ix = iy = -50; break;
            }            
        moveOmni(ix,iy);          
        check = !check; 
    }

}