Diff: main.cpp

Revision:

4:bae4e2d103d4

Parent:

3:c290afbacd1c

Child:

5:428b7ac86810

--- a/main.cpp	Tue Aug 18 00:20:15 2015 +0000
+++ b/main.cpp	Thu Aug 27 05:03:13 2015 +0000
@@ -9,6 +9,11 @@
 #define btSph 5
 #define btCro 6
 #define btSqa 7
+#define devideNum 32
+#define ED 132
+#define TD 144
+#define ER 130
+#define TR 136
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 //Serial Dev(D8,D2);//コントローラー
@@ -20,7 +25,9 @@
 
 int Bflag = 0;
 DigitalOut myled(LED1);// ,test1(PB_7) ,test2(PA_15) ,test3(PC_13) ,test4(PC_12);
-PwmOut Green(D5),Blue(D4),Red(D3),kRed(A3),kBlue(A1),kGreen(A0);
+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 6
 char DATA[N];
 void rotate(double setAngle);
@@ -69,14 +76,14 @@
     return ((dev<0)?(dev+360):(dev))/devide;//分割
 }
 
-int valueForMechanum(int deg,bool triL,bool triR,bool btSpd)
+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!=8||(triL||triR))//スピンかニュートラル以外なら
+                if(deg!=devide||(triL||triR))//スピンかニュートラル以外なら
                     val=1;
                 else 
                     stpFlag=true;//ストップ
@@ -84,7 +91,7 @@
                 
             case 1:
                 if(stpFlag)
-                    val+=1;
+                    val++;
                 else
                     val+=btSpd;//スピード
                 break;
@@ -97,10 +104,10 @@
                 break;
                 
             case 5:
-                val=val<<2;
+                val=val<<2;//5bit目から7bit目へ
                 if(spnFlag)//トリガーのどちらかの値
                     val+=(triL)?1:0;
-                else//ジョイスティックの値
+                else if(!stpFlag)//ジョイスティックの値
                     val+=deg;
                 break;
         }
@@ -113,27 +120,33 @@
 
 void detectPole()
 {
-    bool btFlag=true,lFlag=false;
-    int vector=130;
-    Blue=0.8f;Green=0;
+    bool btFlag=false;
+    int vector=0;
+    Blue=0.0f;Green=0.0f,Red=0.8f;
     while(true)
     {
-        if((((int)DATA[1]&12)==12)&&btFlag){
-            vector=128;
-            lFlag=true;
-            btFlag=false;
+        //スイッチが押された時
+        if(((int)DATA[1]&12)==12){
+            vector=TR;
+            btFlag=true;
+            Blue=0.0f;Green=0.8f;Red=0;
+        }
+        //スイッチが押されていない時
+        else if(((int)DATA[1]&12)!=12){
+            vector=TD;
         }
         
-        else if(((!(int)DATA[1]&1==1)&&(!(int)DATA[1]&2==2))&&lFlag){
-            vector=72;
+        //レーザーが反応した時
+        if(((!(int)DATA[1]&1==1)&&(!(int)DATA[1]&2==2))&&btFlag){
+            vector=64;
             break;
         }
-        //printf("Vector:%dDATA[1]%d\r\n",vector,(int)DATA[1]);
+        
         Mechanum.putc(vector);
         wait(0.001f);
     }
-    btFlag=true;lFlag=false;
-    Blue=0;Green=0;
+    btFlag=false;
+    Blue=0;Green=0;Red=0;
 }
 
 
@@ -145,13 +158,13 @@
     return A;
 }
 
-int main() {
+int main() { 
     pc.baud(230400);
     Dev.baud(921600);
     Mechanum.baud(230400);
-    char shotL[]={0x80,0};
-    char shotR[]={0x40,0};
-    char shotT[]={0x20,0};
+    char shotL[]={0x01,0};
+    char shotR[]={0x02,0};
+    char shotT[]={0x04,0};
     Dev.attach(num,Serial::RxIrq);//受信割り込み設定
     Green=0;Blue=0;Red=0;
     DigitalOut l(PB_2);
@@ -160,53 +173,64 @@
     double deg;
     int val;
     i2c.frequency(400000); 
-    
+    Blue=0.8f;Green=0;
+    kGreen=0.8f;
 
     while(true)
     {
+        Blue=0.8f;Green=0;Red=0;
         if(getBt(btTri)&&getBt(btSph)){
             detectPole();
             //printf("DetectMode\r\n");
         }
         
         if(getBt(btL)){
-            dI2c=!(shot(0x02,shotL,1));
+            kRed=!(shot(0x02,shotL,1));
             wait(0.3f);
             //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
             //i2c.write(airAddr,cmd,2);
-            dI2c=0;
+            kRed=0;
         }
         
         if(getBt(btR)){
-            dI2c=!(shot(0x02,shotR,1));
+            kRed=!(shot(0x02,shotR,1));
             wait(0.3f);
             //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
             //i2c.write(airAddr,cmd,2);
-            dI2c=0;
+            kRed=0;
         }
         
         if(getBt(btTri)){
-            dI2c=!(shot(0x02,shotT,1));
+            kRed=!(shot(0x02,shotT,1));
             wait(0.3f);
-            dI2c=0;
+            kRed=0;
         }
         
         if(getBt(btSqa)&&sqf){
             sqf=0;
             l=!l;
-            kGreen=!kGreen;
+            kBlue=!kBlue;
         }
         else if(!getBt(btSqa))
             sqf=1;
             
+        if(getBt(btCro)){
+            kRed=!(shot(0x02,0,1));
+            wait(0.3f);
+            kRed=0;
+        }
+        if(((int)DATA[1]&12)==12)
+            Blue=0.0f;Green=0.8f;Red=0;
+        
         //deg=pointToDeg((double)DATA[3],(double)DATA[2],30);
-        deg=devidePoint((double)DATA[3],(double)DATA[2],15,45);
+        deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
         //printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
-        printf("%d\r\n",(int)DATA[0]);
-        val=valueForMechanum(deg,getBt(trigL),getBt(trigR),getBt(btSph));
+        //printf("%d\r\n",(int)DATA[0]);
+        val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
         //printf("val:%d\r\n",val);
         Mechanum.putc(val);
         wait(0.03f);
-        //printf("deg:%lfX1:%d:bt%dval:%di2cS:%d\r\n",deg,(int)DATA[1],getBt(btSqa),val,i2cS);
+        //pc.printf("DATA[1]%d\r\n",(int)DATA[1]);
+        //printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val);
     }
 }
\ No newline at end of file