ultrasonic.cpp

00001 
00002 #include "mbed.h"
00003 #include "ultrasonic.h"
00004 
00005 
00006 extern Timer timer2;
00007 extern Serial pc; // tx, rx 
00008 
00009 uint16_t ultrasonicVal[ALL_ULTRASONIC] = {0};
00010 
00011 
00012 void Ultrasonic()
00013 {
00014     for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
00015         uint8_t flag = 0;
00016     
00017         DigitalOut PingPinOut(ultrasonic_pin[i]);
00018         PingPinOut = 1;
00019         wait_us(10);
00020         PingPinOut = 0;
00021         DigitalIn PingPin(ultrasonic_pin[i]);
00022         timer2.reset();
00023         while(PingPin == 0){
00024             if(timer2.read_us() > 1000){   //1ms以上応答なし
00025                 ultrasonicVal[i] =  PING_ERR;
00026                 flag = 1;
00027                 break;
00028             }
00029         } 
00030         timer2.reset();
00031         while(PingPin == 1){
00032             if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
00033                 ultrasonicVal[i] =  PING_ERR;
00034                 flag = 1;
00035                 break;
00036             }
00037         }
00038         if(flag == 0){
00039             ultrasonicVal[i] = timer2.read_us();
00040         }
00041         //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
00042     }
00043     pc.printf("%d\n",ultrasonicVal[0] + ultrasonicVal[2]);
00044     
00045 }