File content as of revision 13:5bccc2d600eb:

#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
#define presUp 0
#define presRight 1
#define presDown 2
#define presLeft 3
#define devideNum 32
#define ED 132
#define TD 144//208
#define EL 134
#define TL 200
#define TNR 138//204
#define TNL 135
#define debugFlag 1
#define stopCount 33

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=0x20;
const int airAddr=0xA0;
const int valveAddr=0xC0;
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 Blue(D5),Green(D4),Red(D3);
//PwmOut kRed(A3),kBlue(A1),kGreen(A0);
PwmOut kRed(A3),kBlue(PA_15),kGreen(PB_7);
#define N 7
char DATA[N];
void rotate(double setAngle);
void speed();
int stopCounter=0;

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

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

bool getPress(int num)
{
    return (DATA[6]>>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,int devide,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!=devide||(triL||triR))//スピンかニュートラル以外なら
                    val=1;
                else 
                    stpFlag=true;//ストップ
                break;
                
            case 1:
                if(stpFlag)
                    val++;
                else
                    val+=btSpd;//スピード
                break;
                
            case 2:
                if(triL||triR){//トリガーのどちらかがONなら
                    val++;
                    spnFlag=true;//スピンフラグON
                }
                break;
                
            case 5:
                val=val<<2;//5bit目から7bit目へ
                if(spnFlag)//トリガーのどちらかの値
                    val+=(triL)?1:0;
                else if(!stpFlag)//ジョイスティックの値
                    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=false,scFlag=false;
    int vector=TD;
    Blue=0.0f;Green=0.0f,Red=0.8f;
    double deg=32;
    while(true)
    {
        /*//スイッチが押された時
        if(((int)DATA[1]&12)==12&&!btFlag){
            wait(0.001f);
            Mechanum.putc(64);
            btFlag=true;
            Blue=0.0f;Green=0.8f;Red=0;
            vector=64;
        }*/
        
        //方向選択
        if(!btFlag)
        {
            deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
            pc.printf("deg%lf\r\n",deg);
            if((int)deg!=devideNum){
                vector=(deg<17)?(TNL):(TNR);
                pc.printf("vector%d\r\n",vector);
                scFlag=true;
            }
        }
        //レーザーが反応した時
        else if(!(int)(DATA[1]&0x02)&&btFlag&&scFlag/*!((int)DATA[1]&2==2)&&btFlag*/){
            vector=64;
            break;
        }
        
        Mechanum.putc(vector);
        wait(0.001f);
    }
    btFlag=false;
    Blue=0;Green=0;Red=0;   
}


bool shotToDenziben(char reg, char data ,int size,int addr)
{
    char REG[2] = {reg,size};
    char DATA[2]={data,0};
    bool A = i2c.write(addr,REG,2);
    A|= i2c.write(addr,DATA,size);
    return A;
}

char getshot(char reg)
{
    char input=0;
    char DATA[2] = {reg,1};
    i2c.write(airAddr,DATA,2);
    wait(0.005f);
    i2c.read(airAddr,&input,1);
    return input;
}

void setFCLED(double RED,double GREEN,double BLUE)
{
    Red=RED;
    Green=GREEN;
    Blue=BLUE;
}

int main() { 
    pc.baud(230400);
    Dev.baud(921600);
    Mechanum.baud(230400);
    DigitalIn bt(USER_BUTTON);
    Dev.attach(num,Serial::RxIrq);//受信割り込み設定
    Green=0;Blue=0;Red=0;
    double gValue=0,bValue=0,rValue=0;
    DigitalOut l(PB_2);
    int sqf=0;
    double deg;
    int val;
    i2c.frequency(400000); 
    setFCLED(0,0.8,0.8);
    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false;

    while(true)
    {           
        //ポール検出モード
        if(getBt(btTri)&&getBt(btSph)){
            detectPole();
            //printf("DetectMode\r\n");
        }
        
        //射出
        if(getBt(btL)&&obonFlag){
            kRed=shotToDenziben(0x02,39,1,airAddr);
            wait(0.1f);
            kRed=0;
        }
        
        //射出
        if(getBt(btR)&&obonFlag){
            kRed=shotToDenziben(0x02,23,1,airAddr);
            wait(0.1f);
            //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
            //i2c.write(airAddr,cmd,2);
            kRed=0;
        }
        
        //射出
        if(getBt(btTri)&&obonFlag){
            kRed=shotToDenziben(0x02,15,1,airAddr);
            wait(0.2f);
            kRed=shotToDenziben(0x02,0,1,airAddr);
            kRed=0;
        }
        
        //バルブ開放
        if(getPress(presLeft)&&pLeftF){
            pLeftF=false;
            kRed=shotToDenziben(7,1,1,valveAddr);
        }
        else if(!getPress(presLeft))
            pLeftF=true;
            
        //バルブ開放
        if(getPress(presRight)&&pRightF){
            pRightF=false;
            kRed=shotToDenziben(8,1,1,valveAddr);
        }
        else if(!getPress(presRight))
            pRightF=true;
        
        //バルブ開放
        if(getBt(btCro)&&btCrF){
            btCrF=false;
            kRed=shotToDenziben(9,1,1,valveAddr);
        }
        else if(!getBt(btCro))
            btCrF=true;


        //レーザーポインタ
        if(getBt(btSqa)&&sqf){
            sqf=0;
            l=!l;
            kBlue=!kBlue;
        }
        else if(!getBt(btSqa))
            sqf=1;
            
        //タッチセンサ
        if(((int)DATA[1]&12)==12){
            bValue=0.0f;gValue=0.8f;rValue=0;
        }
        else{
            bValue=0.8f;gValue=0;rValue=0;
        }
        
        //お盆回し
        if(getPress(presUp)){
            kRed=shotToDenziben(0x02,7,1,airAddr);
            wait(0.1);
            obonFlag=true;
            rValue=0.8;gValue=0;bValue=0.8;
            kRed=0;
        }
        else if(getPress(presDown)){
            kRed=shotToDenziben(0x02,0,1,airAddr);
            wait(0.1);
            bValue=0.8f;gValue=0;rValue=0;
            obonFlag=false;
            kRed=0;
        }
            
        deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
        val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
        
        //一定時間通信してないと停止
        if(stopCounter>stopCount)
            val=64;
        else
            stopCounter++;
        Mechanum.putc(val);
        setFCLED(rValue,gValue,bValue);
        wait(0.03f);
        do{
            if(!debugFlag)
                break;
            pc.printf("val:%d\r\n",val);
            //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
            //printf("%d\r\n",(int)DATA[0]);
            //pc.printf("DATA[1]%d\r\n",(int)(DATA[1]&0x02));
            //printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val);
        }while(false);
    }
}