File content as of revision 1:a3bfd422ea9e:

#include "mbed.h"
#include "math.h"
#define M_PI 3.1415926535897932384626433832795
#define trigL 0
#define trigR 1
#define btL 2
#define btR 3
#define btTri 4
#define btSph 5
#define btCro 6
#define btSqa 7

Serial pc(SERIAL_TX, SERIAL_RX);
//Serial Dev(D8,D2);//コントローラー
Serial Dev(PC_6,PA_12);//コンßトローラー
RawSerial Mechanum(D8,D2);//メカナム
I2C i2c(D14,D15);//sda,scl
const int airAddr=0xA0;
BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ

int Bflag = 0;
DigitalOut myled(LED1);// ,test1(PB_7) ,test2(PA_15) ,test3(PC_13) ,test4(PC_12);
PwmOut Green(D5),Blue(D4),Red(D3);
#define N 6
char DATA[N];
void rotate(double setAngle);
void speed();

void num()//割り込まれたら６回受信
{
    if(Dev.getc()==114){
        //printf("ReceiveCommand\r\n");
        for(int i = 0 ;i<N ;i++ )
        {
            DATA[i]=Dev.getc();
            //printf("%d",DATA[i]);
        }
    }
   //printf("\r\n");
}

bool getBt(int num)
{
    return (DATA[0]>>num)%2;
}

double pointToDeg(double y,double x,int threshold)//座標から角度を求める関数thresholdはしきい値
{
    if(abs((int)(y-128))<threshold)
        y=128;
    if(abs((int)(x-128))<threshold)
        x=128;
    return atan2(-(y-128),x-128)/M_PI*180;//y軸反転、/PI*180でRadからDeg変換
}

double devidePoint(double y,double x,int threshold,float devide)
{
    double tempX,tempY,dev;//仮のX,Yと分けた角度
    tempX=(x-128);tempY=(y-128);//それぞれ-128
    if(abs((int)tempY)<threshold)//しきい値以下なら０にする
        tempY=0;
    if(abs((int)tempX)<threshold)
        tempX=0;
    if(tempX==0&&tempY==0)
        dev=360;
    //printf("X:%lfY:%lfDev:%lf\r\n",tempX,tempY,dev); 
    else
        dev=atan2(tempX,-tempY)/M_PI*180;//軸を逆にして代入、Y軸反転
    return ((dev<0)?(dev+360):(dev))/devide;//分割
}

int valueForMechanum(int deg,bool triL,bool triR,bool btSpd)
{   
    bool stpFlag=false,spnFlag=false;//ストップフラグ、スピンフラグ
    int val=0,i;//値、For用
    for(i=0;i<6;i++){
        switch(i){
            case 0:
                if(deg!=8||(triL||triR))//スピンかニュートラル以外なら
                    val=1;
                else 
                    stpFlag=true;//ストップ
                break;
                
            case 1:
                if(stpFlag)
                    val+=1;
                else
                    val+=btSpd;//スピード
                break;
                
            case 2:
                if(triL||triR){//トリガーのどちらかがONなら
                    val++;
                    spnFlag=true;//スピンフラグON
                }
                break;
                
            case 5:
                val=val<<2;
                if(spnFlag)//トリガーのどちらかの値
                    val+=(triL)?1:0;
                else//ジョイスティックの値
                    val+=deg;
                break;
        }
        if(i<5)
            val=val<<1;
    }
    //printf("val:%ddeg:%dtrigL:%dtrigR:%dbtSp:%d\r\n",val,deg,triL,triR,btSpd);
    return val;
}

void detectPole()
{
    bool btFlag=true,lFlag=false;
    int vector=130;
    Blue=0.8f;Green=0;
    while(true)
    {
        if((((int)DATA[1]&12)==12)&&btFlag){
            vector=128;
            lFlag=true;
            btFlag=false;
        }
        
        else if(((!(int)DATA[1]&1==1)&&(!(int)DATA[1]&2==2))&&lFlag){
            vector=72;
            break;
        }
        //printf("Vector:%dDATA[1]%d\r\n",vector,(int)DATA[1]);
        Mechanum.putc(vector);
        wait(0.001f);
    }
    btFlag=true;lFlag=false;
    Blue=0;Green=0;
}


bool shot(char reg, char *data ,int size)
{
    char DATA[2] = {reg,size};
    bool A = i2c.write(airAddr,DATA,2);
    A|= i2c.write(airAddr,data,size);
    return A;
}

int main() {
    pc.baud(230400);
    Dev.baud(921600);
    Mechanum.baud(230400);
    char shotL[]={0xC0,0};
    char shotR[]={0xE0,0};
    Dev.attach(num,Serial::RxIrq);//受信割り込み設定
    Green=0;Blue=0;Red=0;
    //pc.printf("Hello World !\n");
    double deg;
    int val;
    bool shotLf=true,shotRf=true;
    bool i2cS=true;
    i2c.frequency(400000); 

    while(true)
    {
        if(getBt(btTri)&&getBt(btSph)){
            detectPole();
            //printf("DetectMode\r\n");
        }
        if(getBt(btL)&shotLf){
            dI2c=shot(0x02,shotL,1);
            wait(0.3f);
            //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
            //i2c.write(airAddr,cmd,2);
            shotLf=false;
            dI2c=0;
        }
        else if(!getBt(btL))
            shotLf=true;
        
        if(getBt(btR)&shotRf){
            dI2c=shot(0x02,shotR,1);
            wait(0.3f);
            //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
            //i2c.write(airAddr,cmd,2);
            shotRf=false;
            dI2c=0;
        }
        else if(!getBt(btR))
            shotRf=true;
        //deg=pointToDeg((double)DATA[3],(double)DATA[2],30);
        deg=devidePoint((double)DATA[3],(double)DATA[2],15,11.25);
        //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
        val=valueForMechanum(deg,getBt(trigL),getBt(trigR),getBt(btSph));
        //printf("val:%d\r\n",val);
        Mechanum.putc(val);
        wait(0.03f);
        printf("X:%dX1:%d:bt%dval:%di2cS:%d\r\n",(int)DATA[0],(int)DATA[1],getBt(btSqa),val,i2cS);
    }
}