Revision 0:7d0227dcc289, committed 2013-06-19

Comitter:

akudohune

Date:

Wed Jun 19 08:42:10 2013 +0000

Commit message:

slave

Changed in this revision

diff -r 000000000000 -r 7d0227dcc289 IR.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR.cpp	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,230 @@
+
+#include "mbed.h"
+#include "IR.h"
+
+extern Timer timer_ir;
+
+int direction = 0;
+int Distance  = 0;
+int IR_found = 0; 
+
+void IR_Position(){
+
+    int ir_value[ALL_IR+100] = {0};
+
+    int active_ir = 0;     /* 今回更新する赤外線の番号 */
+    int memory_ir = 0;         /*赤外線時間カウンタ*/
+    int flag_ir = 0;
+    int value = 0;
+    
+    static int direc = 99; 
+    static int past_direc = 99;
+    
+    int liftball;
+
+    for(int i=0; i<ALL_IR; i++) {
+        flag_ir = 1;
+
+        DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */
+
+        timer_ir.start();   /* タイマー起動 */
+
+        if(sensor_ir) {              /* もし立ち上がっていたら */
+            while(sensor_ir) {       /* 立ち下がるまで待つ */
+                if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+                    flag_ir = 0;
+                    break;  /* 立ち上がっている時間が指定時間越えたらブレイク */
+                }
+            }
+        }
+
+        timer_ir.stop();    /* タイマー停止 */
+        timer_ir.reset();   /* タイマーリセット */
+
+        if(flag_ir) {
+            timer_ir.start();   /* タイマー起動 */
+
+            while(!(sensor_ir)) {       /* 立ち上がるまで待つ */
+                if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+                    flag_ir = 0;
+                    break;  /* 立ち上がっている時間が指定時間越えたらブレイク */
+                }
+            }
+        }
+
+        /*ボールが指定時間内に見つかっていたら*/
+        if(flag_ir) {
+            memory_ir = timer_ir.read_us();
+
+            while(1) {
+                if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break;
+
+                if(!(sensor_ir)) {
+                    //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir );
+                    value = (timer_ir.read_us()-memory_ir)/10;
+
+                    break;
+                }
+            }
+        } else {
+            /*ボールが見つかっていない場合*/
+            value = 0;
+        }
+        timer_ir.stop();    /* タイマー停止 */
+        timer_ir.reset();   /* タイマーリセット */
+
+        memory_ir = 0;
+
+        ir_value[active_ir] =  value; //direction array
+
+        if((ir_value[0])&&(active_ir == 0)){
+            ir_value[0] -= 3;     
+        }
+        
+        if((ir_value[8])&&(active_ir == 8)){
+            ir_value[8] -= 2;     
+        }
+
+        active_ir++;
+        
+
+        if( active_ir >= ALL_IR) {
+            active_ir = 0;
+
+            /***********direction***********/
+    
+            int min = 100,youso_min = 100;
+    
+            for(int i = 0; i<DIREC_IR; i++) {
+                if((ir_value[i]<min)&&(ir_value[i])) {
+                    min = ir_value[i];
+                    youso_min = i;
+                }
+            }
+            
+            if(youso_min == 8){
+                direc = 1;        
+            }else if(youso_min == 9){
+                direc = 15;            
+            }else{
+                direc = youso_min * 2;
+            }
+            /*
+            if(ir_value[youso_min] > 50){
+                liftball = 1;
+            }else{
+                liftball = 0;
+            }
+            
+            if((past_direc != 99)&&(liftball == 0)){
+                if(past_direc == 0){
+                    if((direc >= 4)&&(direc <= 12)){
+                        direc = past_direc;
+                    }
+                }else if(past_direc == 1){
+                    if((direc >= 4)&&(direc <= 14)){
+                        direc = past_direc;
+                    }
+                }else if(past_direc == 15){
+                    if((direc >= 2)&&(direc <= 12)){
+                        direc = past_direc;
+                    }
+                }else{
+                    if(abs(past_direc - direc) >= 4){
+                        direc = past_direc;
+                    }
+                }
+            }*/
+            
+            //direc = youso_min * 2;
+    
+            /*******  direction end  *******/
+    
+            /*******     distance    *******/
+    
+            int dista;
+            
+            if((ir_value[youso_min]<=28 + TERM)) {
+                dista = 30;
+            } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) {
+                dista = 90;
+            } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) {
+                dista = 120;
+            } else if( ir_value[youso_min]>40 + TERM) {
+                dista = 180;
+            } else {
+                dista = 0;
+            }
+            
+            int count_ir = 0,total_ir = 0;
+            
+            for(int i=0; i<DIREC_IR; i++){
+                if(ir_value[i]){
+                    total_ir += ir_value[i];
+                    count_ir++;
+                }
+            }
+            
+            double hihhihi = 0;
+            
+            if(!ir_value[10]) ir_value[10] = 1000000;
+            
+            hihhihi = (double)ir_value[youso_min]/(double)ir_value[10];
+            
+            if(ir_value[10] <= 38){ 
+                if((direc == 0)&&(hihhihi  >= 1.0)){
+                    dista = 10;
+                }else if((direc == 1)&&(hihhihi  >= 1.0)){
+                    dista = 10;
+                }else if((direc == 2)&&(hihhihi  >= 0.75)){
+                    dista = 10;
+                }else if((direc == 3)&&(hihhihi  >= 0.65)){
+                    dista = 10;
+                }else if((direc == 4)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 5)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 6)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 7)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 8)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 9)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 10)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 11)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 12)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 13)&&(hihhihi  >= 0.65)){
+                    dista = 10;
+                }else if((direc == 14)&&(hihhihi  >= 0.75)){
+                    dista = 10;
+                }else if((direc == 15)&&(hihhihi  >= 1.0)){
+                    dista = 10;
+                }
+            }  
+            
+            int count = 0;
+            
+            for(int i=0;i<DIREC_IR;i++){
+                if(ir_value[i])count++;
+            } 
+            
+            if(count)   IR_found = 1;
+            else        IR_found = 0;
+            
+
+            /********  distance end  *******/
+            
+            past_direc = direc;
+            
+            direction = direc;
+            Distance  = dista;
+            
+            //printf("derection:%d distance:%d\n",direction,Distance);
+        }
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 7d0227dcc289 IR.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR.h	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,116 @@
+#define IR_TIME_NOTFOUND 833    /* 見つけられなかったと判断するまでの時間(単位：us) */
+#define IR_COUNTMAX 487     /*最大パルス幅 パルスの存在しうる最大時間は487us*/
+#define ALL_IR 11
+#define DIREC_IR 10
+#define DIRECTION 16
+#define TERM 0
+#define DELTA -2
+#define SWAP(type,a,b) { type temp = a; a = b; b = temp; }
+
+/* 赤外線センサに使うpinを配列に格納 */
+PinName ir_num[ALL_IR] = {
+    p26,
+    p30,
+    p11,
+    p12,
+    p20,
+    p19,
+    p18,
+    p17,
+    p29,
+    p15,
+    p16
+    /*
+     p15,
+    p17,
+    p18,
+    p19,
+    p20,
+    p12,
+    p11,
+    p30,
+    p16,
+    p29,
+    p26
+    */
+};
+
+int Convert_Direction[DIRECTION] = {
+    90,
+    67,
+    30,
+    22,
+    0,
+    337,
+    315,
+    292,
+    270,
+    247,
+    225,
+    202,
+    180,
+    157,
+    150,
+    112
+};
+
+//ball direction
+double ball_sankaku[16][2] = {
+    {0     , 1    },
+    {0.390 , 0.920},
+    {0.866 , 0.500},
+    {0.927 , 0.374},
+    {1     , 0    },
+    {0.920 ,-0.390},
+    {0.707 ,-0.707},
+    {0.374 ,-0.927},
+    {0     ,-1    },
+    {-0.390,-0.920},
+    {-0.707,-0.707},
+    {-0.927,-0.374},
+    {-1    , 0    },
+    {-0.920, 0.390},
+    {-0.866, 0.500},
+    {-0.374, 0.927}
+};
+
+double turn_sankaku[16][2] = {
+    { 0    ,0      },
+    { 1    ,0      },//{ 0.920,-0.390 },
+    { 0.707,-0.707 }, //{ 0.500,-0.866 },
+    { 0.374,-0.927 },
+    { 0    ,-0.5   },
+    {-0.390,-0.920 },
+    {-0.707,-0.707 },
+    {-0.927,-0.374 },
+    {-0.927,-0.374 },
+    {0.920 ,-0.390 },
+    {0.707 ,-0.707 },
+    {0.374 ,-0.927 },
+    {0     ,-0.5   },
+    {-0.390,-0.920 },
+    {-0.707,-0.707 }, //{-0.500,-0.866 },
+    {-1    ,0      }//{-0.927,-0.374 }
+};
+
+
+/*
+double turn_sankaku[16][2] = {
+    {0 ,0 },
+    {0 ,0 },
+    {1 ,0 },
+    {0 ,-1},
+    {0 ,-1},
+    {0 ,-1},
+    {-1,0 },
+    {-1,0 },
+    {-1,0 },
+    {1 ,0 },
+    {1 ,0 },
+    {0 ,-1},
+    {0 ,-1},
+    {0 ,-1},
+    {-1,0 },
+    {0 ,0 }
+};
+*/

diff -r 000000000000 -r 7d0227dcc289 TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/TextLCD/#e4cb7ddee0d3

diff -r 000000000000 -r 7d0227dcc289 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,110 @@
+#include "mbed.h"
+#include "TextLCD.h"
+#include "main.h"
+
+int ave5point(int source)
+{
+    static int tmp[5] = {0};
+    static int sum = 0;
+    
+    sum -= tmp[4];
+    sum += source;
+    tmp[4] = tmp[3];
+    tmp[3] = tmp[2];
+    tmp[2] = tmp[1];
+    tmp[1] = tmp[0];
+    tmp[0] = source;
+    
+    for(uint8_t i = 0; i < 5; i++){
+        if((tmp[i] > 180) || (tmp[i] < 10)){
+            return 0xFF;
+        }
+    }
+    
+    return (int)(sum / 5.0);
+
+}
+
+void init()
+{
+    device.baud(BAUD_RATE);
+    device.printf("START");
+    device2.printf("START"); 
+    device.attach(&dev_tx,Serial::TxIrq);
+    device.attach(&dev_rx,Serial::RxIrq);
+    device2.attach(&dev2_tx,Serial::TxIrq);
+    device2.attach(&dev2_rx,Serial::RxIrq);
+    Button.mode(PullUp);
+    
+    timer2.start();
+    
+    lcd.cls();
+}
+
+int main() 
+{
+    uint8_t state = 0;
+
+    init();
+    
+    for(;;){
+        if((!Button) && (state == 0)){
+            wait(0.3);
+            state = 1;
+        }else if((!Button) && (state == 1)){
+            wait(0.3);
+            state = 2;
+        }else if((!Button) &&(state == 2)){
+            wait(0.3);
+            state = 0;
+        }
+        
+        mbedleds = 0;
+        
+        Ultrasonic();
+        IR_Position();
+        AveDistance = ave5point(Distance);
+        
+        lcd.cls();
+        
+        if(state == 0){
+            lcd.locate(0,0);
+            lcd.printf("R:%d,F:%d",(int)ultrasonicValue[1],(int)ultrasonicValue[0]);
+            lcd.locate(0,1);
+            lcd.printf("L:%d,B:%d",(int)ultrasonicValue[3],(int)ultrasonicValue[2]);
+        }else if(state == 1){
+            lcd.locate(0,0);
+            lcd.printf("%d,%d,%d",direction,data[2],(data[0] << 8) + data[1]);//data[2]
+            lcd.locate(0,1);
+            lcd.printf("%d,%d",AveDistance,data2[0]);
+        }else if(state == 2){
+            lcd.locate(0,0);
+            lcd.printf("%d,%d,%d",irDistance[0],irDistance[1],irDistance[2]);//data[2]
+            lcd.locate(0,1);
+            lcd.printf("%d,%d,%d",irDistance[3],irDistance[4],irDistance[5]);
+        }
+        
+        //pc.printf("%d\n",direction);
+        /*
+        pc.printf("R:%d,\tF:%d\t",(int)ultrasonicValue[1],(int)ultrasonicValue[0]);
+        pc.printf("L:%d,\tB:%d\n",(int)ultrasonicValue[3],(int)ultrasonicValue[2]);
+        */
+        /*
+        lcd.locate(0,0);
+        lcd.printf("%d",(data[0] << 8) + data[1]);
+        lcd.locate(0,1);
+        lcd.printf("%d",data[2]);
+        */
+        /*
+        lcd.locate(0,0);
+        lcd.printf("%d,%d",direction,data[3]);//data[2]
+        lcd.locate(0,1);
+        lcd.printf("%d",AveDistance);
+        */
+        /*
+        lcd.locate(0,0);
+        lcd.printf("%d",data[0]);
+        lcd.locate(0,1);
+        lcd.printf("%d",data[1]);*/
+    }
+}

diff -r 000000000000 -r 7d0227dcc289 main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,30 @@
+
+
+Timer timer2;
+Timer timer_ir;
+DigitalIn Button(p25);
+BusOut mbedleds(LED4,LED3,LED2,LED1); 
+Serial device2(p28, p27);  // tx, rx
+Serial device(p13, p14);  // tx, rx
+Serial pc(USBTX, USBRX); // tx, rx 
+TextLCD lcd(p5, p6, p10, p7, p9, p8);
+
+int AveDistance;
+
+extern uint16_t ultrasonicVal[4];
+extern double ultrasonicValue[4];
+extern int direction;
+extern int Distance;
+extern int IR_found;
+extern void IR_Position(void);
+extern void Ultrasonic(void);
+extern void dev_rx(void);
+extern void dev_tx(void);
+extern void dev2_rx(void);
+extern void dev2_tx(void);
+
+extern uint8_t data[4];
+extern uint8_t data2[5];
+extern int irDistance[6];
+
+#define BAUD_RATE   19200
\ No newline at end of file

diff -r 000000000000 -r 7d0227dcc289 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17
\ No newline at end of file

diff -r 000000000000 -r 7d0227dcc289 uart1.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.cpp	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,80 @@
+
+#include "mbed.h"
+#include "uart1.h"
+
+extern Serial device;
+extern BusOut mbedleds;
+//extern Serial pc;
+extern int direction;
+extern int Distance;
+extern int IR_found;
+extern int AveDistance;
+extern uint16_t ultrasonicVal[4];
+
+uint8_t xbee;
+uint8_t data[4];
+int irDistance[6];
+
+extern uint8_t data2[5];
+
+void dev_rx()
+{   
+    static uint8_t count2 = 0;
+    static uint8_t RecData[RECEIVE_DATA_NUM];
+    
+    RecData[count2] = device.getc();
+    
+    if(RecData[KEY2] == KEYCODE2){
+        count2++;
+    }else{
+        count2 = 0;
+    }
+    if(count2 >= RECEIVE_DATA_NUM){
+        if(RecData[CHECK2] == CHECKCODE2){
+            //mbedleds = 15;
+            data[0] = RecData[DATA1];
+            data[1] = RecData[DATA2];
+            data[2] = RecData[DATA3];
+            data[3] = RecData[DATA4];
+            irDistance[0] = (RecData[DISTANCE1] << 8) + RecData[DISTANCE1_2];
+            irDistance[1] = (RecData[DISTANCE2] << 8) + RecData[DISTANCE2_2];
+            irDistance[2] = (RecData[DISTANCE3] << 8) + RecData[DISTANCE3_2];
+            irDistance[3] = (RecData[DISTANCE4] << 8) + RecData[DISTANCE4_2];
+            irDistance[4] = (RecData[DISTANCE5] << 8) + RecData[DISTANCE5_2];
+            irDistance[5] = (RecData[DISTANCE6] << 8) + RecData[DISTANCE6_2];
+            //pc.printf("%d\n",data[2]);
+        }
+        count2 = 0;
+    }  
+}
+
+void dev_tx()
+{
+    static uint8_t count = 0;
+    static uint8_t SendData0[SEND_DATA_NUM];
+    
+    if(count >= SEND_DATA_NUM){
+        xbee = data2[0];
+        SendData0[KEY]      = KEYCODE;
+        SendData0[DIRECTIONN] = direction;
+        SendData0[DISTANCE] = AveDistance;
+        SendData0[SONIC1_1] = ultrasonicVal[0];
+        SendData0[SONIC1_2] = (ultrasonicVal[0] >> 8);
+        SendData0[SONIC2_1] = ultrasonicVal[1];
+        SendData0[SONIC2_2] = (ultrasonicVal[1] >> 8);
+        SendData0[SONIC3_1] = ultrasonicVal[2];
+        SendData0[SONIC3_2] = (ultrasonicVal[2] >> 8);
+        SendData0[SONIC4_1] = ultrasonicVal[3];
+        SendData0[SONIC4_2] = (ultrasonicVal[3] >> 8);
+        SendData0[IR_FOUND] = IR_found;
+        SendData0[XBEE]     = xbee;
+        SendData0[CHECK]    = CHECKCODE;
+        
+        //pc.printf("%d\t%d\t%d\t%d\n",SendData0[DIRECTIONN],SendData0[DISTANCE],SendData0[IR_FOUND],SendData0[CHECK]);
+        
+        count = 0;
+    }
+    device.putc(SendData0[count]);
+    
+    count++;
+}
\ No newline at end of file

diff -r 000000000000 -r 7d0227dcc289 uart1.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.h	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,47 @@
+
+#define SEND_DATA_NUM       14
+#define RECEIVE_DATA_NUM    18
+
+#define KEYCODE             120
+#define CHECKCODE           (SendData0[DIRECTIONN] ^ SendData0[DISTANCE] ^ SendData0[SONIC1_1] ^ SendData0[SONIC1_2] ^ SendData0[SONIC2_1] ^ SendData0[SONIC2_2] ^ SendData0[SONIC3_1] ^ SendData0[SONIC3_2] ^ SendData0[SONIC4_1] ^ SendData0[SONIC4_2] ^ SendData0[IR_FOUND] ^ SendData0[XBEE])
+#define KEYCODE2            35
+#define CHECKCODE2          (RecData[DATA1] ^ RecData[DATA2] ^ RecData[DATA3] ^ RecData[DATA4] ^ RecData[DISTANCE1] ^ RecData[DISTANCE1_2] ^ RecData[DISTANCE2] ^ RecData[DISTANCE2_2] ^ RecData[DISTANCE3] ^ RecData[DISTANCE3_2] ^ RecData[DISTANCE4] ^ RecData[DISTANCE4_2] ^ RecData[DISTANCE5] ^ RecData[DISTANCE5_2] ^ RecData[DISTANCE6] ^ RecData[DISTANCE6_2])
+
+
+enum{
+    KEY = 0,
+    DIRECTIONN,
+    DISTANCE,
+    SONIC1_1,
+    SONIC1_2,
+    SONIC2_1,
+    SONIC2_2,
+    SONIC3_1,
+    SONIC3_2,
+    SONIC4_1,
+    SONIC4_2,
+    IR_FOUND,
+    XBEE,
+    CHECK,
+};
+
+enum{
+    KEY2 = 0,
+    DATA1,
+    DATA2,
+    DATA3,
+    DATA4,
+    DISTANCE1,
+    DISTANCE1_2,
+    DISTANCE2,
+    DISTANCE2_2,
+    DISTANCE3,
+    DISTANCE3_2,
+    DISTANCE4,
+    DISTANCE4_2,
+    DISTANCE5,
+    DISTANCE5_2,
+    DISTANCE6,
+    DISTANCE6_2,
+    CHECK2,
+};

diff -r 000000000000 -r 7d0227dcc289 uart2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart2.cpp	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,57 @@
+
+#include "mbed.h"
+#include "uart2.h"
+
+extern Serial device2;
+extern BusOut mbedleds;
+
+extern uint8_t data[4];
+
+uint8_t data2[5];
+
+
+void dev2_rx()
+{   
+    static uint8_t count3 = 0;
+    static uint8_t RecData2[RECEIVE2_DATA_NUM];
+    
+    RecData2[count3] = device2.getc();
+    
+    if(RecData2[KEY3] == KEYCODE4){
+        count3++;
+    }else{
+        count3 = 0;
+    }
+    if(count3 >= RECEIVE2_DATA_NUM){
+        if(RecData2[CHECK3] == CHECKCODE4){
+            mbedleds = 15;
+            data2[0] = RecData2[DATA31];
+            data2[1] = RecData2[DATA32];
+            data2[2] = RecData2[DATA33];
+            data2[3] = RecData2[DATA34];
+            data2[4] = RecData2[DATA35];
+        }
+        count3 = 0;
+    }  
+}
+
+void dev2_tx()
+{
+    static uint8_t count4 = 0;
+    static uint8_t SendData2[SEND2_DATA_NUM];
+    
+    if(count4 >= SEND2_DATA_NUM){
+        SendData2[KEY4]   = KEYCODE3;
+        SendData2[DATA41] = 33;
+        SendData2[DATA42] = data[2];
+        SendData2[DATA43] = 52;
+        SendData2[DATA44] = 33;
+        SendData2[DATA45] = 38;
+        SendData2[CHECK4] = CHECKCODE3;
+        
+        count4 = 0;
+    }
+    device2.putc(SendData2[count4]);
+    
+    count4++;
+}

diff -r 000000000000 -r 7d0227dcc289 uart2.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart2.h	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,28 @@
+
+#define SEND2_DATA_NUM      7
+#define RECEIVE2_DATA_NUM   7
+
+#define KEYCODE3            54
+#define CHECKCODE3          (SendData2[DATA31] ^ SendData2[DATA32] ^ SendData2[DATA33] ^ SendData2[DATA34] ^ SendData2[DATA35])
+#define KEYCODE4            22
+#define CHECKCODE4          (RecData2[DATA41] ^ RecData2[DATA42] ^ RecData2[DATA43] ^ RecData2[DATA44] ^ RecData2[DATA45])
+
+
+enum{
+    KEY3 = 0,
+    DATA31,
+    DATA32,
+    DATA33,
+    DATA34,
+    DATA35,
+    CHECK3,
+};
+enum{
+    KEY4 = 0,
+    DATA41,
+    DATA42,
+    DATA43,
+    DATA44,
+    DATA45,
+    CHECK4,
+};

diff -r 000000000000 -r 7d0227dcc289 ultrasonic.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.cpp	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,44 @@
+
+#include "mbed.h"
+#include "ultrasonic.h"
+ 
+extern Timer timer2;
+
+uint16_t ultrasonicVal[4];
+double ultrasonicValue[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+    for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
+        uint8_t flag = 0;
+    
+        DigitalOut PingPinOut(ultrasonic_pin[i]);
+        PingPinOut = 1;
+        wait_us(10);
+        PingPinOut = 0;
+        DigitalIn PingPin(ultrasonic_pin[i]);
+        timer2.reset();
+        while(PingPin == 0){
+            if(timer2.read_us() > 1500){   //1.5ms以上応答なし
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        } 
+        timer2.reset();
+        while(PingPin == 1){
+            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        }
+        if(flag == 0){
+            ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm  MAX:3145
+            ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
+        }else{
+            ultrasonicVal[i] = PING_ERR;
+        }
+    }
+} 
\ No newline at end of file

diff -r 000000000000 -r 7d0227dcc289 ultrasonic.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.h	Wed Jun 19 08:42:10 2013 +0000
@@ -0,0 +1,12 @@
+
+#define ALL_ULTRASONIC  4
+#define PING_ERR        0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+    p21,
+    p22,
+    p23,
+    p24,
+};
+