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