Diff: main.cpp

Revision:

9:c992dfaa77ce

Parent:

8:52f8023cdb8f

Child:

11:735a59ebc63f

--- a/main.cpp	Fri Feb 27 04:43:53 2015 +0000
+++ b/main.cpp	Mon Mar 02 23:53:34 2015 +0000
@@ -20,7 +20,7 @@
 
 #define PING_COUNT 5 //5回に1回にPing処理
 #define IR_LEVEL 400 //irのしきい値
-#define I2C_ADDRESS 0xAA+2 //
+#define I2C_ADDRESS 0xAA //
 
 
 
@@ -34,7 +34,47 @@
 Serial pc(USBTX,USBRX);
  
 
+void IrSort(int input[], uint8_t output[]){
+    /*
+     *1,2位の値とその場所の番号をアウトプットに格納する.
+     *値が0だとボールを検知してないことになる。
+     */
+    int near[] = {1000, 1000};//rank1, rank2
+    uint8_t position[] = {12, 0}, i;
+    
+    for(i=0; i < 6; i++){
+        
+        if(input[i] == 0){
+            continue;
+        }
+        
+        if(near[1] < input[i]){
+            continue;
+        }
+        if(near[0] < input[i]){
+            near[1] = input[i];
+            position[1] = i;
+            continue;
+        }
+        
+        near[1] = near[0];
+        position[1]   = position[0];
+        near[0] = input[i];
+        position[0]   = i;
+    }
 
+    if(position[0] == 12){//not found
+        output[0] = 125;
+        output[1] = 125;
+        output[2] = 125;
+        return;
+    }
+    
+    output[0] = position[0]*8 | position[1];
+    output[1] = near[0]/10;
+    output[2] = near[1]/10;
+    
+}
 
 
 int main() {
@@ -52,7 +92,8 @@
     
     /*Data*/
     uint8_t PingData[2] = {0};
-    unsigned int ir__data[6] = {0};
+    int ir__data[6] = {0};
+    uint8_t ir_data[6] = {0};
     
     /*State(operate_value etc...)*/
     int I2CState;
@@ -91,10 +132,13 @@
             
             ir__data[i] = Ir[i].Read();
             
-            if(ir__data[i] > 1000) ir__data[i] = 1000;
-            ir__data[i] = ir__data[i] / 10;
+            if(ir__data[i] > 1250) ir__data[i] = 1250;
         }
         
+        IrSort(ir__data, ir_data);
+        
+        
+        
         I2CState = Mbed.receive();
         
         switch(I2CState){
@@ -110,14 +154,17 @@
                         break;
                     case 0x00:
                     case 0x02://IR
-                        for(i=0; i < 6; i++) SendData[i] = (char)ir__data[i];
                         
-                        /*for(i=0; i<6; i++){
+                        SendData[0] = (char)ir_data[0];
+                        SendData[1] = (char)ir_data[1];
+                        SendData[2] = (char)ir_data[2];
+                        
+                        /*for(i=0; i<3; i++){
                             pc.printf("%d ", (char)SendData[i]);
                         }
                         pc.printf("\n");*/
                         
-                        Mbed.write(SendData, 6);
+                        Mbed.write(SendData, 3);
                         
                         break;
                     default: