Diff: main.cpp

Revision:

0:dc587be179e9

Child:

1:a3bfd422ea9e

diff -r 000000000000 -r dc587be179e9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jul 20 08:57:05 2015 +0000
@@ -0,0 +1,191 @@
+#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);//コントローラー
+RawSerial Mechanum(PA_11,PA_12);//メカナム
+I2C i2c(D14,D15);//sda,scl
+const int airAddr=0xA0;
+
+int Bflag = 0;
+DigitalOut myled(LED1);// ,test1(PB_7) ,test2(PA_15) ,test3(PC_13) ,test4(PC_12);
+#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;
+    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;
+}
+
+
+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 N;
+}
+
+int main() {
+    pc.baud(230400);
+    Dev.baud(921600);
+    Mechanum.baud(230400);
+    char shotL[]={0xC0,0};
+    char shotR[]={0xE0,0};
+    Dev.attach(num,Serial::RxIrq);//受信割り込み設定
+
+    //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){
+            i2cS=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;
+        }
+        else if(!getBt(btL))
+            shotLf=true;
+        
+        if(getBt(btR)&shotRf){
+            i2cS=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;
+        }
+        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]&12,getBt(btSqa),val,i2cS);
+    }
+}
\ No newline at end of file