main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #define M_PI 3.1415926535897932384626433832795
00004 #define trigL 0
00005 #define trigR 1
00006 #define btL 2
00007 #define btR 3
00008 #define btTri 4
00009 #define btSph 5
00010 #define btCro 6
00011 #define btSqa 7
00012 #define btSel 0
00013 #define presUp 4
00014 #define presRight 5
00015 #define presDown 6
00016 #define presLeft 7
00017 #define devideNum 32
00018 #define ED 132
00019 #define TD 144//208
00020 #define EL 134
00021 #define TL 200
00022 #define TNR 138//204
00023 #define TNL 150
00024 #define debugFlag 1
00025 #define stopCount 33
00026 #define shotWait 0.5
00027 #define isXBOX true
00028 #define XBOXrate 19200
00029 
00030 Serial pc(SERIAL_TX, SERIAL_RX);
00031 //Serial Dev(D8,D2);//コントローラー
00032 Serial Dev(PC_6,PA_12);//コントローラー
00033 RawSerial Mechanum(D8,D2);//メカナム
00034 I2C i2c(D14,D15);//sda,scl
00035 //const int airAddr=0x20;
00036 const int airAddr=0xd0;
00037 const int airReg=10;
00038 const int valveAddr=0xC0;
00039 BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ
00040 
00041 int Bflag = 0;
00042 DigitalOut myled(LED1);// ,test1(PB_7) ,test2(PA_15) ,test3(PC_13) ,test4(PC_12);
00043 PwmOut Blue(D5),Green(D4),Red(D3);
00044 //PwmOut kRed(A3),kBlue(A1),kGreen(A0);
00045 PwmOut kRed(A3),kBlue(PA_15),kGreen(PB_7);
00046 #define N 7
00047 char DATA[N];
00048 void rotate(double setAngle);
00049 void speed();
00050 int stopCounter=0;
00051 bool pushF=false;
00052 
00053 void num()//割り込まれたら7回受信
00054 {
00055     if(Dev.getc()==114){
00056         //printf("ReceiveCommand\r\n");
00057         for(int i = 0 ;i<N ;i++ )
00058         {
00059             DATA[i]=Dev.getc();
00060         }
00061     }
00062     stopCounter=0;
00063 }
00064 
00065 bool getBt(int num)
00066 {
00067     return (DATA[0]>>num)%2;
00068 }
00069 
00070 bool getPress(int num)
00071 {
00072     return (DATA[6]>>num)%2;
00073 }
00074 
00075 double pointToDeg(double y,double x,int threshold)//座標から角度を求める関数thresholdはしきい値
00076 {
00077     if(abs((int)(y-128))<threshold)
00078         y=128;
00079     if(abs((int)(x-128))<threshold)
00080         x=128;
00081     return atan2(-(y-128),x-128)/M_PI*180;//y軸反転、/PI*180でRadからDeg変換
00082 }
00083 
00084 int devidePoint(double y,double x,int threshold,float devide)
00085 {
00086     double tempX,tempY,dev;//仮のX,Yと分けた角度
00087     tempX=(x-128);tempY=(y-128);//それぞれ-128
00088     if(abs((int)tempY)<threshold)//しきい値以下なら０にする
00089         tempY=0;
00090     if(abs((int)tempX)<threshold)
00091         tempX=0;
00092     if(tempX==0&&tempY==0)
00093         dev=360;
00094     //printf("X:%lfY:%lfDev:%lf\r\n",tempX,tempY,dev); 
00095     else
00096         dev=atan2(tempX,(isXBOX)?(tempY):(-tempY))/M_PI*180;//軸を逆にして代入、Y軸反転
00097     return ((dev<0)?(dev+360):(dev))/devide;//分割
00098 }
00099 
00100 int valueForMechanum(int deg,int devide,bool triL,bool triR,bool btSpd)
00101 {   
00102     bool stpFlag=false,spnFlag=false;//ストップフラグ、スピンフラグ
00103     int val=0,i;//値、For用
00104     for(i=0;i<6;i++){
00105         switch(i){
00106             case 0:
00107                 if(deg!=devide||(triL||triR))//スピンかニュートラル以外なら
00108                     val=1;
00109                 else 
00110                     stpFlag=true;//ストップ
00111                 break;
00112                 
00113             case 1:
00114                 if(stpFlag)
00115                     val++;
00116                 else
00117                     val+=btSpd;//スピード
00118                 break;
00119                 
00120             case 2:
00121                 if(triL||triR){//トリガーのどちらかがONなら
00122                     val++;
00123                     spnFlag=true;//スピンフラグON
00124                 }
00125                 break;
00126                 
00127             case 5:
00128                 val=val<<2;//5bit目から7bit目へ
00129                 if(spnFlag)//トリガーのどちらかの値
00130                     val+=(triL)?1:0;
00131                 else if(!stpFlag)//ジョイスティックの値
00132                     val+=deg;
00133                 break;
00134         }
00135         if(i<5)
00136             val=val<<1;
00137     }
00138     //printf("val:%ddeg:%dtrigL:%dtrigR:%dbtSp:%d\r\n",val,deg,triL,triR,btSpd);
00139     return val;
00140 }
00141 
00142 bool detectPole(int scVector)
00143 {
00144     int vector=TD,laser=0;
00145     bool btFlag=false,canFlag=false,breakFlag=false,stopFlag=false;
00146     Blue=0.0f;Green=0.0f,Red=0.8f;
00147     while(true)
00148     {    
00149         if((getBt(btTri)&&getBt(btSph)&&canFlag)||breakFlag)
00150             return breakFlag;
00151         else if(!(getBt(btTri)&&getBt(btSph)))
00152             canFlag=true;
00153             
00154         //スイッチが押された時
00155         if(((int)DATA[1]&12)==12&&!btFlag){
00156             wait(0.001f);
00157             Mechanum.putc(64);
00158             btFlag=true;
00159             Blue=0;Green=0.8;Red=0;
00160             vector=scVector;
00161         }
00162         
00163         //片方のレーザーが反応したとき
00164         if(btFlag&&!laser){
00165             if(!(((int)DATA[1]&2)==2)){
00166                 laser=1;
00167                 Red=0.8;
00168                 vector=0;
00169             }
00170             else if(!(((int)DATA[1]&1)==1)){
00171                 laser=2;
00172                 Red=0.8;
00173                 vector=0;
00174             }
00175         }
00176         
00177         //もう片方のレーザーが反応した時
00178         else if(!(((int)DATA[1]&laser)==laser)&&laser&&!stopFlag){
00179             Green=0;Red=0.8;Blue=0.8;
00180             vector=64;
00181             stopFlag=true;
00182             wait(0.05);
00183         }
00184         //ストップ後
00185         else if(stopFlag)
00186         {
00187             vector=162;
00188             breakFlag=true;
00189         }
00190         
00191         //pc.printf("%d\r\n",DATA[1]);
00192         //deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
00193         //pc.printf("%d\r\n",scVector);
00194         Mechanum.putc(vector);
00195         wait(0.0126f);
00196     }
00197     btFlag=false;
00198     Blue=0;Green=0;Red=0;   
00199 }
00200 
00201 
00202 bool shotToDenziben(char reg, char data ,int size,int addr)
00203 {
00204     char REG[2] = {reg,size};
00205     char DATA[2]={data,0};
00206     bool A = i2c.write(addr,REG,2);
00207     A|= i2c.write(addr,DATA,size);
00208     return A;
00209 }
00210 
00211 char getshot(char reg)
00212 {
00213     char input=0;
00214     char DATA[2] = {reg,1};
00215     i2c.write(airAddr,DATA,2);
00216     wait(0.005f);
00217     i2c.read(airAddr,&input,1);
00218     return input;
00219 }
00220 
00221 void setFCLED(double RED,double GREEN,double BLUE)
00222 {
00223     Red=RED;
00224     Green=GREEN;
00225     Blue=BLUE;
00226 }
00227 
00228 int main() { 
00229     pc.baud(230400);
00230     //Dev.baud(921600);//PS3
00231     //Dev.baud(57600);//XBOX
00232     Dev.baud((isXBOX)?(XBOXrate):(921600));
00233     Mechanum.baud(230400);
00234     DigitalIn bt(USER_BUTTON);
00235     Dev.attach(num,Serial::RxIrq);//受信割り込み設定
00236     Green=0;Blue=0;Red=0;
00237     double gValue=0,bValue=0,rValue=0;
00238     DigitalOut l(PB_2);
00239     int sqf=0;
00240     double deg=32;
00241     int val;
00242     i2c.frequency(400000); 
00243     setFCLED(0,0.8,0.8);
00244     bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,detFinF=false,touchSF=false;
00245 
00246     while(true)
00247     {           
00248        /* //ポール検出モード
00249         if(getBt(btTri)&&getBt(btSph)){
00250             if(deg>devideNum-1);
00251             else if(deg<17){
00252                 //pc.printf("R\r\n");
00253                 detFinF=detectPole(TNR);
00254             }
00255             else{
00256                 //pc.printf("L\r\n");
00257                 detFinF=detectPole(TNL);
00258             }
00259         }*/
00260         
00261         do{
00262             bool l=getBt(btL),r=getBt(btR),t=getBt(btTri);
00263             if(!l&&!r&&!t)
00264                 pushF=false;
00265             if(pushF||!obonFlag)
00266                 break;
00267                 
00268             //射出
00269             if(l){
00270                 //pc.printf("L\r\n");
00271                 pushF=true;
00272                 kRed=shotToDenziben(airReg,1,1,airAddr);
00273                 wait(shotWait);
00274                 kRed=shotToDenziben(airReg,0,1,airAddr);
00275                 kRed=0;
00276             }
00277             
00278             //射出
00279             else if(r){
00280                 //pc.printf("R\r\n");
00281                 pushF=true;
00282                 kRed=shotToDenziben(airReg,2,1,airAddr);
00283                 wait(shotWait);
00284                 kRed=shotToDenziben(airReg,0,1,airAddr);
00285                 //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
00286                 //i2c.write(airAddr,cmd,2);
00287                 kRed=0;
00288             }
00289             
00290             //射出
00291             else if(t){
00292                 //pc.printf("T\r\n");
00293                 pushF=true;
00294                 kRed=shotToDenziben(airReg,4,1,airAddr);
00295                 wait(shotWait);
00296                 kRed=shotToDenziben(airReg,0,1,airAddr);
00297                 kRed=0;
00298             }
00299                 
00300         }while(false);
00301         
00302         //レーザーポインタ
00303         if(getBt(btSqa)&&sqf){
00304             sqf=0;
00305             l=!l;
00306             kBlue=!kBlue;
00307         }
00308         else if(!getBt(btSqa))
00309             sqf=1;
00310             
00311         //タッチセンサ
00312         if(((int)DATA[1]&12)==12&&!touchSF){
00313             touchSF=true;
00314             gValue=0.5;
00315         }
00316         else if(!(((int)DATA[1]&12)==12)){
00317             touchSF=false;
00318             gValue=0;
00319         }
00320         //お盆回し
00321         if(getPress(presUp)){
00322             //pc.printf("L\r\n");
00323             obonFlag=true;
00324             Mechanum.putc(162);
00325             rValue=0.5;
00326             kRed=0;
00327             wait(0.2);
00328         }
00329         else if(getPress(presDown)){
00330             Mechanum.putc(163);
00331             rValue=0;
00332             obonFlag=false;
00333             kRed=0;
00334             wait(0.2);
00335         }         
00336         
00337         deg=devidePoint((double)DATA[3],(double)DATA[2],30,360/devideNum);
00338         
00339         if(getPress(btSel)||(detFinF&&deg!=devideNum)){
00340             detFinF=false;
00341             Mechanum.putc(163);
00342             wait(0.1);
00343         }
00344         else {
00345             val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
00346             pc.printf("deg:%lf\r\n",deg);
00347         }
00348         
00349         
00350         //一定時間通信してないと停止
00351         if(stopCounter>stopCount)
00352             val=64;
00353         else
00354             stopCounter++;
00355         Mechanum.putc(val);
00356         //bValue=0.8;
00357         setFCLED(rValue,gValue,bValue);
00358         wait(0.03f);
00359         do{
00360             if(!debugFlag)
00361                 break;
00362             //pc.printf("%d\r\n",DATA[6]);
00363             //if(getPress(btSel))
00364             //    pc.printf("up\r\n");
00365             //pc.printf("deg:%d\r\n",deg);
00366             //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
00367             //printf("%d\r\n",(int)DATA[0]);
00368             //pc.printf("DATA[1]%d\r\n",(int)(DATA[1]&0x02));
00369             //printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val);
00370             //pc.printf("%lf\r\n",deg);
00371         }while(false);
00372     }
00373 }