Revision 6:97015325107b, committed 2014-03-08

Comitter:

com3

Date:

Sat Mar 08 13:16:25 2014 +0000

Parent:

5:3457e0e66104

Child:

8:f547b66f3c43

Commit message:

sensors

Changed in this revision

--- a/main.cpp	Tue Feb 25 02:29:32 2014 +0000
+++ b/main.cpp	Sat Mar 08 13:16:25 2014 +0000
@@ -1,45 +1,52 @@
 #include "mbed.h"
 #include "HMC6352.h"
 /*PING DEFINE */
-#define ERROR_PING 0xFFF
+#define ERROR_PING 0xFF
 #define ALL_PING 4
 /*IR DEFINE*/
 #define BREAKPT_IR 833
 #define ERROR_IR 0xFFF
-#define ALL_IR 10
+#define ALL_IR 12
+#define NOTBALL 3
+#define NO_BALL 45
+#define ERR 7
 //#define OVERLINE_IR 46
 /*LINE DEFINE*/
-#define JUDGEVOL_LINE 0.9
+#define JUDGEVOL_LINE 0.1
 #define ALL_LINE 4
 //OTHERS
 #define ALL_KIND 3 //kind of sensors
-#define MAX_NUM 10 //sensors most number
+#define MAX_NUM 12 //sensors most number
 extern void micon_tx();
 /*
 Ping , IrSensor , LineSensor ,compass
 */
-//LINE sensor is float value 
+//LINE sensor is float value
+
+BusOut myled(LED1, LED2, LED3, LED4);
+ 
 Serial sensor(p13,p14);
 Serial pc(USBTX,USBRX);
 HMC6352 compass(p9, p10);
-
+ 
 Timer time_ping;
 Timer time_ir; 
-
+ 
 PinName num_ping[ALL_PING] = {p5,p6,p7,p8};
-PinName num_ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30};
+PinName num_ir[ALL_IR] = {p29,p21,p22,p23,p24,p25,p26,p27,p30,p29,p12,p15};
 PinName num_line[ALL_LINE] = {p17,p18,p19,p20};
-
+ 
 enum sensors{Ping,Ir,Line};
 volatile unsigned int value_ping[ALL_PING] = {0};
 volatile unsigned int value_ir[ALL_IR] = {0};
+volatile  int hosei_ir[ALL_IR] = {-1,1,0,0,0,0,0,1,-2,-1,0,0};
 volatile unsigned int value_ir_min = 0;
 volatile unsigned int ir_min_num = 0;
 volatile unsigned int value_line[ALL_LINE] = {0};
 volatile unsigned int value_compass0 = 0;
 volatile unsigned int value_compass[2] = {0};
-
-
+ 
+ 
 /*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){
     static unsigned int sum[ALL_KIND][MAX_NUM] = {0};
     static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0};
@@ -71,11 +78,12 @@
             }else{
                 value_line[num] = 0;//green
             }
+            //pc.printf("%f %f %f %f\n", sum[kind][0]/10000.0, sum[kind][1]/10000.0, sum[kind][2]/10000.0, sum[kind][3]/10000.0);
             break;
         default :
             break;
     }
-
+ 
     //return sum[kind][num];//return no config sum
        
 }
@@ -99,12 +107,13 @@
     }
     time_ping.reset();
     while(sensor_ping){
-        if(time_ping.read_ms() >18.5){
+        if(time_ping.read_ms() >18.5){//18.5
             time_ping.stop();
             return ERROR_PING;
         }
     }
     memory = time_ping.read_us();
+    memory >>= 6;
     time_ping.stop();
     
     return memory;
@@ -134,7 +143,17 @@
             }
         }
         memory = time_ir.read_us();
-        while(1){
+        while(sensor_ir){
+            if((time_ir.read_us() - memory) >= BREAKPT_IR){
+                break;
+            }
+        }
+        temp = (time_ir.read_us() - memory);
+        time_ir.stop();
+        time_ir.reset();
+        //wait(0.01);
+        return temp;
+        /*while(1){
             if(!sensor_ir){
                 temp = (time_ir.read_us() - memory);
                 time_ir.stop();
@@ -142,7 +161,7 @@
                 //wait(0.01);
                 return temp;
             }
-        }
+        }*/
     }else{//not found
     }
     time_ir.stop();
@@ -154,30 +173,77 @@
     AnalogIn sensor_line(num_line[num]);
     memory = sensor_line;
     return (int)(memory*1000);
-
+ 
 }
 void ir_min_fun(){
     static unsigned int min;
-    static int i, num = 0;
+    static int i, num = 0, sum = 0;
+    sum = 0;
     min = value_ir[0];
-        for (i = 0;i <=ALL_IR -1;i++){
-            if(min >= value_ir[i]){
+    for (i = 0;i <= ALL_IR - NOTBALL;i++){
+            value_ir[i] += hosei_ir[i];
+            sum += value_ir[i];
+            if(min > value_ir[i]){
                 min = value_ir[i];
                 num = i;
-        }
+            }
     }
     value_ir_min = min;
-    ir_min_num = num;
+    if(/*(value_ir_min < NO_BALL) || */(value_ir[11] > 45)){
+        if(sum < 4000){
+            //myled = 1;
+            if((num == 0) || (num == 8) || (num == 9)){
+                //myled = 4;
+                if(value_ir_min <= 25){
+                    num = 0;
+                } else if((value_ir[1] >= value_ir[7] - ERR) && (value_ir[1] <= value_ir[7] + ERR)){
+                    num = 0;
+                    myled = 0;
+                } else if(value_ir[1] > value_ir[7]){
+                    num = 9;
+                    myled = 3;
+                } else if(value_ir[1] < value_ir[7]){
+                    num = 8;
+                    myled = 12;
+                }
+                if((value_ir[0] + value_ir[8] + value_ir[9]) < 72){
+                    myled = 15;
+                }
+                /*
+                if(value_ir[0] == value_ir[8]){
+                    num = 10;
+                    myled = 6;
+                } else if(value_ir[0] == value_ir[9]){
+                    num = 11;
+                    myled = 12;
+                }
+                */
+            } else if(num == 8){
+                //myled = 2;
+            } else if(num == 9){
+                //myled = 8;
+            }
+            
+            ir_min_num = num;
+        }else{
+            ir_min_num = NO_BALL;
+            myled = 0;
+        }
+    } else {
+        ir_min_num = NO_BALL;
+        myled = 0;
+    }
 }
 int main() {
     //enum sensors kinds;
     int compassdef = 0;
     int num[ALL_KIND] = {0};
+    int ping_flag = 0;
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     //送信開始
     sensor.putc(1);
     //送信空き割り込み設定
     sensor.attach(&micon_tx,Serial::TxIrq);
-    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     compassdef = (compass.sample() / 10);
     
     while(1) {
@@ -189,25 +255,61 @@
             value_compass[0] = value_compass0;
             value_compass[1] = 0;
         }
+        //myled = 1;
+        //pc.printf("%d\t%d\n", value_compass0, compassdef);
+        
+        for(num[Ir] = 0; num[Ir] < ALL_IR; num[Ir]++){
+            moving_ave(Ir,num[Ir],ir_function(num[Ir]));
+        }
+        ir_min_fun();
+        //pc.printf("%d %d\n", value_ir[9], value_compass0);
+        //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t   %d   %d\n", value_ir[0], value_ir[1], value_ir[2], value_ir[3], value_ir[4], value_ir[5], value_ir[6], value_ir[7], value_ir[8], value_ir[9], value_ir[10], value_ir[11], value_ir_min, ir_min_num);
+        //pc.printf("%d\t%d\n",value_ir_min,ir_min_num);
+        //pc.printf("%d\n", value_ir[11]);
+        //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\n", value_ir[1], value_ir[8], value_ir[0], value_ir[9], value_ir[7], ir_min_num);       //前5つ
+        //pc.printf("%d\t%d\t%d\t%d\t%d\n", value_ir[3], value_ir[2], value_ir[1], value_ir[8], ir_min_num);                        //左4つ
+        //myled = 2;
+        
+        if(ping_flag == 2){
+            ping_flag = 0;
+            for(num[Ping] = 0; num[Ping] < ALL_PING; num[Ping]++){
+                 moving_ave(Ping,num[Ping],ping_function(num[Ping]));
+             }
+            //pc.printf("%d %d %d %d\n", value_ping[0], value_ping[1], value_ping[2], value_ping[3]);
+        }
+        ping_flag++;
+        //myled = 4;
+        /*
         moving_ave(Ping,num[Ping],ping_function(num[Ping]));
-        pc.printf("compass0=%d compass[0]= %d  compass[1]= %d\n",value_compass0,value_compass[0],value_compass[1]);
-        //ir_min_fun();
-        //num[Ping]++;
-        //num[Ir]++;
-        //num[Line]++;        
+        //moving_ave(Line,num[Line],line_function(num[Line]));
+        //pc.printf("%d\n", line_function(2));
+        if(line_function(num[Line]) > 500){
+            value_line[num[Line]] = 1;
+        } else {
+            value_line[num[Line]] = 0;
+        }
+        moving_ave(Ir,num[Ir],ir_function(num[Ir]));
+        
+        num[Ping]++;
+        num[Ir]++;
+        num[Line]++;        
         if(num[Ping]>= ALL_PING){
             num[Ping] = 0; 
-            putchar('\n');
+            
+            //putchar('\n');
         }
         if(num[Ir]>= ALL_IR){
             num[Ir] = 0;
-            putchar('\n');
-            pc.printf("min=%d ,num = %d\n",value_ir_min,ir_min_num);
+            ir_min_fun();
+            //pc.printf("%d %d %d %d %d %d %d %d %d %d\n", value_ir[0], value_ir[1], value_ir[2], value_ir[3], value_ir[4], value_ir[5], value_ir[6], value_ir[7], value_ir[8], value_ir[9]);
+            pc.printf("%d %d\n",value_ir_min,ir_min_num);
+            //putchar('\n');
         }
         if(num[Line]>= ALL_LINE){
             num[Line] = 0;
-            putchar('\n');
+            //putchar('\n');
         }
+        */
         
     }
-}
+}
\ No newline at end of file

--- a/usart.cpp	Tue Feb 25 02:29:32 2014 +0000
+++ b/usart.cpp	Sat Mar 08 13:16:25 2014 +0000
@@ -1,11 +1,11 @@
 #include "mbed.h"
 
 #define KEYCODE 0xAA
-#define TX_CHECKCODE (tx_data[1]^tx_data[2]^tx_data[3]^tx_data[4]^tx_data[5]^tx_data[6]^tx_data[7]^tx_data[8]^tx_data[9]^tx_data[10])
-#define RX_CHECKCODE (rx_data[1]^rx_data[2]^rx_data[3]^rx_data[4]^rx_data[5]^rx_data[6]^rx_data[7]^rx_data[8]^rx_data[9]^rx_data[10])
-#define DATA_NUM 14
+#define TX_CHECKCODE (tx_data[1]^tx_data[2]^tx_data[3]^tx_data[4]^tx_data[5]^tx_data[6]^tx_data[7]^tx_data[8])
+#define RX_CHECKCODE (rx_data[1]^rx_data[2]^rx_data[3]^rx_data[4]^rx_data[5]^rx_data[6]^rx_data[7]^rx_data[8])
+#define DATA_NUM 12
 #define CHECK (DATA_NUM - 1)
-#define ALL_IR 10
+#define ALL_IR 12
 #define ALL_PING 4
 #define ALL_LINE 4
 
@@ -19,6 +19,7 @@
 extern unsigned int ir_min_num;
 extern unsigned int value_line[ALL_LINE];
 extern unsigned int value_compass[2];
+/*
 void micon_rx(){
     
     static uint8_t rx;
@@ -38,6 +39,7 @@
     }
 
 }
+*/
 
 void micon_tx(){
     
@@ -52,13 +54,16 @@
         tx_data[4] = value_ping[1];
         tx_data[5] = value_ping[2];
         tx_data[6] = value_ping[3];
-        tx_data[7] = value_line[0];
+/*        tx_data[7] = value_line[0];
         tx_data[8] = value_line[1];
         tx_data[9] = value_line[2];
         tx_data[10] = value_line[3];
-        tx_data[11] = value_compass[0];
-        tx_data[12] = value_compass[1];
-        tx_data[13] = TX_CHECKCODE;
+*/
+        tx_data[7] = value_compass[0];
+        tx_data[8] = value_compass[1];
+        tx_data[9] = value_ir[10];
+        tx_data[10] = value_ir[11];
+        tx_data[11] = TX_CHECKCODE;
         
         tx = 0;
     }