File content as of revision 24:ee0943b12feb:

#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 btSel 0
#define presUp 4
#define presRight 5
#define presDown 6
#define presLeft 7
#define devideNum 32
#define ED 132
#define TD 144//208
#define EL 134
#define TL 200
#define TNR 138//204
#define TNL 150
#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=0xd0;
const int airReg=10;
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変換
}

int 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;
}

bool detectPole(int scVector)
{
    int vector=TD,laser=0;
    bool btFlag=false,canFlag=false,breakFlag=false,stopFlag=false;
    Blue=0.0f;Green=0.0f,Red=0.8f;
    while(true)
    {    
        if((getBt(btTri)&&getBt(btSph)&&canFlag)||breakFlag)
            return breakFlag;
        else if(!(getBt(btTri)&&getBt(btSph)))
            canFlag=true;
            
        //スイッチが押された時
        if(((int)DATA[1]&12)==12&&!btFlag){
            wait(0.001f);
            Mechanum.putc(64);
            btFlag=true;
            Blue=0;Green=0.8;Red=0;
            vector=scVector;
        }
        
        //片方のレーザーが反応したとき
        if(btFlag&&!laser){
            if(!(((int)DATA[1]&2)==2)){
                laser=1;
                Red=0.8;
                vector=0;
            }
            else if(!(((int)DATA[1]&1)==1)){
                laser=2;
                Red=0.8;
                vector=0;
            }
        }
        
        //もう片方のレーザーが反応した時
        else if(!(((int)DATA[1]&laser)==laser)&&laser&&!stopFlag){
            Green=0;Red=0.8;Blue=0.8;
            vector=64;
            stopFlag=true;
            wait(0.05);
        }
        //ストップ後
        else if(stopFlag)
        {
            vector=162;
            breakFlag=true;
        }
        
        //pc.printf("%d\r\n",DATA[1]);
        //deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
        //pc.printf("%d\r\n",scVector);
        Mechanum.putc(vector);
        wait(0.0126f);
    }
    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=32;
    int val;
    i2c.frequency(400000); 
    setFCLED(0,0.8,0.8);
    bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,detFinF=false;

    while(true)
    {           
        //ポール検出モード
        if(getBt(btTri)&&getBt(btSph)){
            if(deg>devideNum-1);
            else if(deg<17){
                //pc.printf("R\r\n");
                detFinF=detectPole(TNR);
            }
            else{
                //pc.printf("L\r\n");
                detFinF=detectPole(TNL);
            }
        }
        
        //射出
        if(getBt(btL)&&obonFlag){
            Mechanum.putc(162);
            wait(0.5f);
            Mechanum.putc(163);
            kRed=0;
        }
        
        //射出
        if(getBt(btR)&&obonFlag){
            kRed=shotToDenziben(airReg,2,1,airAddr);
            wait(0.5f);
            kRed=shotToDenziben(airReg,0,1,airAddr);
            //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
            //i2c.write(airAddr,cmd,2);
            kRed=0;
        }
        
        //射出
        if(getBt(btTri)&&obonFlag){
            kRed=shotToDenziben(airReg,4,1,airAddr);
            wait(0.5f);
            kRed=shotToDenziben(airReg,0,1,airAddr);
            kRed=0;
        }

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