HCSR04.cpp

00001 #include "HCSR04.h"
00002 #include "mbed.h"
00003 
00004 
00005 HCSR04::HCSR04(PinName t, PinName e) : trig(t), echo(e) {}
00006 
00007 //      Trigger          Echo
00008 //      _______           _____________,,,,,,,,,
00009 // ____|  10us |_________| 150us-25ms, or 38ms if no obstacle
00010 // 
00011 
00012 //return echo duration in us (refer to digram above)
00013 float HCSR04::echo_duration() {
00014     timer.reset();
00015     trig = 0;
00016     wait_us(2);
00017     trig = 1;
00018     wait_us(10);
00019     trig = 0;
00020     while(echo == 0);
00021     timer.start();
00022     while(echo == 1);
00023     timer.stop();
00024     return timer.read_us();
00025 }
00026 
00027 //return distance to nearest obstacle or returns -1 
00028 //if no obstacle within range
00029 //set sys to cm or inch accordingly
00030 float HCSR04::distance(int sys){
00031     duration = echo_duration();
00032     if(duration > 30000)
00033         return -1;
00034     distacne_cm = duration /29 / 2 ;
00035     distance_inc = duration / 74 / 2;
00036     if (sys)
00037         return distacne_cm;
00038     else
00039         return distance_inc;
00040 }
00041