hcsr04.cpp

00001 #include "hcsr04.h"
00002 
00003 #define v_o 343
00004 
00005 HCSR04::HCSR04(PinName TrigPin,PinName EchoPin):
00006     trigger(TrigPin), echo(EchoPin)
00007 {
00008     pulsetime.stop();
00009     pulsetime.reset();
00010     echo.rise(callback(this,&HCSR04::isr_rise));
00011     echo.fall(callback(this,&HCSR04::isr_fall));
00012     trigger=0;
00013 }
00014 
00015 HCSR04::~HCSR04()
00016 {
00017 }
00018 
00019 void HCSR04::isr_rise(void)
00020 {
00021     pulsetime.start();
00022 }
00023 void HCSR04::start(void)
00024 {
00025     trigger=1;
00026     wait_us(10);
00027     trigger=0;
00028 }
00029 
00030 void HCSR04::isr_fall(void)
00031 {
00032     pulsetime.stop();
00033     pulsedur = pulsetime.read_us();
00034     distance= (pulsedur*v_o)/20000;
00035     pulsetime.reset();
00036 }
00037 
00038 void HCSR04::rise (void (*fptr)(void))
00039 {
00040     echo.rise(fptr);
00041 }
00042 void HCSR04::fall (void (*fptr)(void))
00043 {
00044     echo.fall(fptr);
00045 }
00046 
00047 float HCSR04::get_dist_cm()
00048 {
00049     return distance;
00050 }
00051 float HCSR04::get_pulse_us()
00052 {
00053     return pulsedur;
00054 }
00055 
00056 LowPassFilter::LowPassFilter(float old_weight)
00057     :data_(0),
00058      old_weight_(old_weight)
00059 {}
00060 
00061 void LowPassFilter::SetData(float new_data)
00062 {
00063     data_ = data_ * old_weight_ + (1- old_weight_) * new_data;
00064 }
00065 
00066 float LowPassFilter::GetData()
00067 {
00068     return data_;
00069 }
00070 void LowPassFilter::SetOldWeight(float old_weight)
00071 {
00072 old_weight_ = old_weight;
00073 }