Diff: main.cpp

Revision:

10:2ebbea0bcc6e

Parent:

6:f64989ad4746

--- a/main.cpp	Thu Feb 26 05:32:09 2015 +0000
+++ b/main.cpp	Tue Mar 03 02:37:24 2015 +0000
@@ -26,8 +26,8 @@
 
 IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,dp28};
 
-Ping Ping1(dp1,dp2);
-Ping Ping2(dp4,dp6);
+Ping Ping1(dp2,dp1);
+Ping Ping2(dp6,dp4);
 DigitalOut Led(LED1);
 
 I2CSlave Mbed(dp5,dp27);//sda,scl
@@ -56,48 +56,48 @@
     return  sum[num]/10;   
 }
 
-void IrDataSelect(int input[], uint8_t output[]){
+void IrSort(unsigned int input[], char output[]){
+    
     /*
      *1,2位の値とその場所の番号をアウトプットに格納する.
      *値が255以上だとボールを検知してないことになる。
      */
-    uint8_t nigh1 = 255, nigh2 = 255;
-    uint8_t ni1=12, ni2 = 12, i;
+    unsigned int  near[2] = {1000,1000};
+    uint8_t num[2] = {12,0}, i;
     
-    for(i = 6; i > 0; i-- ){
-        if(nigh2 < input[i]){
+    for(i = 0; i < 6; i++ ){
+        if(!input[i]){
             continue;
         }
-        if(nigh1 < input[i]){
-            nigh2 = input[i];
-            ni2 = i;
+        
+        if(near[1] < input[i]){
             continue;
         }
-        nigh2 = nigh1;
-        ni2   = ni1;
-        nigh1 = input[i];
-        ni1   = i;
+        if(near[0] < input[i]){
+            near[1] = input[i];
+            num[1] = i;
+            continue;
+        }
+        near[1] = near[0];
+        num [1]  = num[0];
+        near[0] = input[i];
+        num [0]   = i;
     }
 
-    if(ni1 == 12){//not found
-        output[0] = 255;
-        output[1] = 255;
-        output[2] = 255;
+    if(num[0] == 12){//not found
+        output[0] = 125;
+        output[1] = 125;
+        output[2] = 125;
         return;
     }
-    
-    output[0] = ni1*6 + ni2;
-    output[1] = nigh1;
-    output[2] = nigh2;
+    if(num[1] == 12){
+        num[1] = 0;
+    }
+    output[0] = num[0]*6 + num[1];
+    output[1] = near[0] /10;
+    output[2] = near[1] /10 ;
     
 }
-/*周回の初期動作*/
-void ResetSet(uint8_t *PingCk, uint8_t *IrCk){
-
-    Led = 1;//周回の初め    
-    //PingCk ++;
-
-}
 
 
 
@@ -113,64 +113,69 @@
     
     /*Count*/
     uint8_t PingCk = 0;
-    uint8_t IrCk = 0;
     
     /*Data*/
     uint8_t PingData[2] = {0};
-    unsigned long      IrTemp[6]   = {0};  //仮置き場
-    uint8_t IrData[3];          // irのデータ
-    unsigned int ir__data[6] = {0};
+    char IrData[3] = {0};
+    unsigned int IrBase[6] = {0};
+    
     /*State(operate_value etc...)*/
     int I2CState;
     
     /* I2Cdata*/
     char SendData[9]={0};
     char AddressData[1] = {0};
-    
-    /*Debug valiable*/
-    bool valid = 0,busy = 0;
+
 
     while(1) {
-        
-        ResetSet(&PingCk,&IrCk);
-
-        /*
-        IrData[0] = (IR_LEVEL - Ir[0].Read())*255/400;
-        IrData[1] = (IR_LEVEL - Ir[1].Read())*255/400;
-        IrData[2] = (IR_LEVEL - Ir[2].Read())*255/400;
-        IrTemp[3] = (IR_LEVEL - Ir[3].Read())*255/400;
-        IrTemp[4] = (IR_LEVEL - Ir[4].Read())*255/400;
-        IrTemp[5] = (IR_LEVEL - Ir[5].Read())*255/400;
-        */
+        Led = 1;//周回の初め    
 
-        /*平均化
-        IrTemp[0] = IrMoving_ave(0,IrTemp[0]);
-        IrTemp[1] = IrMoving_ave(1,IrTemp[1]);
-        IrTemp[2] = IrMoving_ave(2,IrTemp[2]);
-        IrTemp[3] = IrMoving_ave(3,IrTemp[3]);
-        IrTemp[4] = IrMoving_ave(4,IrTemp[4]);
-        IrTemp[5] = IrMoving_ave(5,IrTemp[5]);
-        */
-        
-
-        //IrDataSelect(IrTemp,IrData);
+        //通信していないとき
+        if(!AddressData[0])
+        {
+            PingCk ++;
+            
+            IrBase[0] = Ir[0].Read();
+            IrBase[1] = Ir[1].Read();
+            IrBase[2] = Ir[2].Read();
+            IrBase[3] = Ir[3].Read();
+            IrBase[4] = Ir[4].Read();
+            IrBase[5] = Ir[5].Read();
+            
+            /*平均化
+            
+            IrTemp[0] = IrMoving_ave(0,IrTemp[0]);
+            IrTemp[1] = IrMoving_ave(1,IrTemp[1]);
+            IrTemp[2] = IrMoving_ave(2,IrTemp[2]);
+            IrTemp[3] = IrMoving_ave(3,IrTemp[3]);
+            IrTemp[4] = IrMoving_ave(4,IrTemp[4]);
+            IrTemp[5] = IrMoving_ave(5,IrTemp[5]);
+            
+            */
+            
+            
+            IrSort(IrBase,IrData);
+            /*
+            if(PingCk == 2){
+                
+                Ping1.Send();    
+                wait_ms(30);
+                PingData[0] = Ping1.Read_cm();
+            }
+            
+            
+            if(PingCk <= PING_COUNT){
+                
+                Ping2.Send();    
+                wait_ms(30);
+                PingData[1] = Ping2.Read_cm();
+                PingCk = 0;
+                //pc.printf("%d \t %d \t %d \n",PingData[0],PingData[1],IrBase[0]);
+            
+            }*/
+            
         
-        /*
-        if(PingCk >= PING_COUNT){
-            Ping1.Send();
-            Ping2.Send();    
-            wait_ms(30);
-            PingData[0] = Ping1.Read_cm();
-            PingData[1] = Ping2.Read_cm();
-            PingCk = 0;
         }
-        */
-        ir__data[0] = Ir[0].Read();
-        ir__data[1] = Ir[1].Read();
-        ir__data[2] = Ir[2].Read();
-        ir__data[3] = Ir[3].Read();
-        ir__data[4] = Ir[4].Read();
-        ir__data[5] = Ir[5].Read();
         I2CState = Mbed.receive();
         
         switch(I2CState){    
@@ -178,44 +183,48 @@
             
                  if(AddressData[0]){
                     switch(AddressData[0]){
-                        case 0x01://ping
+                        case 0x01://PING
                                 SendData[0] = (char)PingData[0];
                                 SendData[1] = (char)PingData[1];
                                 Mbed.write(SendData, 2);
                                 break;
-                        case 0x02://IR
+                        case 0x02://BALL
                                 SendData[0] = (char)IrData[0];
                                 SendData[1] = (char)IrData[1];
                                 SendData[2] = (char)IrData[2];
                                 Mbed.write(SendData, 3);
                                 break;                   
                             
-
                         default:
                                 break;
                     }
+                    AddressData[0] = 0;
+                 }else{
+                    SendData[0] = (char)IrData[0];         
+                    SendData[1] = (char)IrData[1];
+                    SendData[2] = (char)IrData[2];
+                    SendData[3] = (char)PingData[0];
+                    SendData[4] = (char)PingData[1];
+                    Mbed.write(SendData, 5);
                  }
                  
-                 AddressData[0] = 0;
                  break;
-            case I2CSlave::WriteGeneral:
+            case I2CSlave::WriteAddressed:
                  Mbed.read(AddressData,1);
                  break;
-            case I2CSlave::WriteAddressed:
+                 
+            case I2CSlave::WriteGeneral:
+                 
                  break;
         
         }
         Led = 0;
-        pc.printf("%d \t %d \t %d ",ir__data[0],ir__data[1],ir__data[2]);
+        /*
+        pc.printf("%d \t %d \t %d ",IrBase[0],IrBase[1],IrBase[2]);
         
-        pc.printf("%d \t %d \t %d \n",ir__data[3],ir__data[4],ir__data[5]);
-        /*
-        IrCk ++;
-        if(IrCk > 5){
-            IrCk = 0;
-            
-        }
-        */    
+        pc.printf("\t %d \t %d \t %d \n",IrBase[3],IrBase[4],IrBase[5]);
+        */
+  
         //wait(0.1);
     }
 }