Diff: main.cpp.orig

Revision:

22:ab619079fb8d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Sat Oct 17 05:44:35 2015 +0000
@@ -0,0 +1,345 @@
+#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);
+    }
+}
\ No newline at end of file