Diff: main.cpp

Revision:

3:c290afbacd1c

Parent:

2:c5f038d7be1f

Child:

4:bae4e2d103d4

--- a/main.cpp	Sun Aug 09 06:19:05 2015 +0000
+++ b/main.cpp	Tue Aug 18 00:20:15 2015 +0000
@@ -12,7 +12,7 @@
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 //Serial Dev(D8,D2);//コントローラー
-Serial Dev(PC_6,PA_12);//コンßトローラー
+Serial Dev(PC_6,PA_12);//コントローラー
 RawSerial Mechanum(D8,D2);//メカナム
 I2C i2c(D14,D15);//sda,scl
 const int airAddr=0xA0;
@@ -20,7 +20,7 @@
 
 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);
+PwmOut Green(D5),Blue(D4),Red(D3),kRed(A3),kBlue(A1),kGreen(A0);
 #define N 6
 char DATA[N];
 void rotate(double setAngle);
@@ -141,7 +141,7 @@
 {
     char DATA[2] = {reg,size};
     bool A = i2c.write(airAddr,DATA,2);
-    A|= i2c.write(airAddr,data,size);
+    A&= i2c.write(airAddr,data,size);
     return A;
 }
 
@@ -149,16 +149,18 @@
     pc.baud(230400);
     Dev.baud(921600);
     Mechanum.baud(230400);
-    char shotL[]={0xC0,0};
-    char shotR[]={0xE0,0};
+    char shotL[]={0x80,0};
+    char shotR[]={0x40,0};
+    char shotT[]={0x20,0};
     Dev.attach(num,Serial::RxIrq);//受信割り込み設定
     Green=0;Blue=0;Red=0;
+    DigitalOut l(PB_2);
+    int sqf=0;
     //pc.printf("Hello World !\n");
     double deg;
     int val;
-    bool shotLf=true,shotRf=true;
-    bool i2cS=true;
     i2c.frequency(400000); 
+    
 
     while(true)
     {
@@ -166,34 +168,45 @@
             detectPole();
             //printf("DetectMode\r\n");
         }
-        if(getBt(btL)&shotLf){
-            dI2c=shot(0x02,shotL,1);
+        
+        if(getBt(btL)){
+            dI2c=!(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;
+        }
+        
+        if(getBt(btR)){
+            dI2c=!(shot(0x02,shotR,1));
             wait(0.3f);
             //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
             //i2c.write(airAddr,cmd,2);
-            shotLf=false;
+            dI2c=0;
+        }
+        
+        if(getBt(btTri)){
+            dI2c=!(shot(0x02,shotT,1));
+            wait(0.3f);
             dI2c=0;
         }
-        else if(!getBt(btL))
-            shotLf=true;
         
-        if(getBt(btR)&shotRf){
-            dI2c=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;
-            dI2c=0;
+        if(getBt(btSqa)&&sqf){
+            sqf=0;
+            l=!l;
+            kGreen=!kGreen;
         }
-        else if(!getBt(btR))
-            shotRf=true;
+        else if(!getBt(btSqa))
+            sqf=1;
+            
         //deg=pointToDeg((double)DATA[3],(double)DATA[2],30);
         deg=devidePoint((double)DATA[3],(double)DATA[2],15,45);
         //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("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);
+        //printf("deg:%lfX1:%d:bt%dval:%di2cS:%d\r\n",deg,(int)DATA[1],getBt(btSqa),val,i2cS);
     }
 }
\ No newline at end of file