Diff: Distance_HC_SR04.cpp

Branch:

CLASS_IMPLEMENTATION

Revision:

5:1a69f40177b0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Distance_HC_SR04.cpp	Mon Dec 21 20:54:09 2015 +0000
@@ -0,0 +1,181 @@
+#include "Distance_HC_SR04.h"
+
+
+    /** Create Distance_HC_SR04 instance
+     */
+    Distance_HC_SR04::Distance_HC_SR04(PinName trig, PinName echo, uint32_t tout_us, float coeff,
+            uint32_t tmin_us, uint32_t tmax_us) :
+            _trig(trig), _echo(echo), _tout_us(tout_us), _coeff(coeff), _tmin_us(tmin_us), _tmax_us(tmax_us) {
+        _trig = 0;
+        _state = IDLE;
+    }
+
+    /** Start the measurement.
+     *
+     */
+    void Distance_HC_SR04::trigger(void) {
+        if (_state == IDLE && _echo == 0) {
+            _timeout.detach();
+            _echo.rise(NULL);
+            _echo.fall(NULL);
+            _timer.stop();
+            _timer.reset();
+
+            _trig = 1;
+            wait_us(TRIG_PULSE_US);
+            _trig = 0;
+
+            if (_echo == 0) {
+                _state = STARTED;
+                _timeout.attach_us(this, &Distance_HC_SR04::_tout, TIMEOUT_DELAY_US);
+
+                _echo.rise(this, &Distance_HC_SR04::_rising);
+                _echo.fall(this, &Distance_HC_SR04::_falling);
+
+                return;
+            }
+        }
+
+        if (_state == IDLE) {
+            _state = ERROR_SIG;
+            _ticks_us = 0;
+        }
+
+        return;
+    }
+
+    /** Returns a state measurement FSM is currently in.
+     *
+     */
+    Distance_HC_SR04_state Distance_HC_SR04::getState(void) {
+        return _state;
+    }
+
+    /** Resets whole device and prepares for triggering next measurement. FSM set to IDLE state.
+     *
+     */
+    void Distance_HC_SR04::reset(void) {
+        _state = IDLE;
+        _echo.rise(NULL);
+        _echo.fall(NULL);
+        _timeout.detach();
+        _timer.stop();
+        _timer.reset();
+    }
+
+    /** Returnes duration of "echo" pulse (microseconds) in case thate state is "COMPLETED".
+     *
+     */
+    uint32_t Distance_HC_SR04::getTicks(void) {
+        return _ticks_us;
+    }
+
+    /** Returns a distance of the obstacle in milimeters calculated from duration of "echo" pulse.
+     *
+     */
+    float Distance_HC_SR04::getDistance(void) {
+        return _ticks_us*_coeff;
+    }
+
+    /** Return actual value of coefficient used to calculate distance from "echo" pulse duration.
+     *
+     */
+    float Distance_HC_SR04::getCoeff(void) {
+        return _coeff;
+    }
+
+    /** Set the actual value of coefficient used to calculate distance from "echo" pulse duration.
+      *
+      * @param coeff Coeficient for multiplication with pulse duration in microseconds
+      * @returns
+      *   void
+      */    
+    void Distance_HC_SR04::setCoeff(float coeff) {
+        _coeff = coeff;
+    }
+
+    /** Measure and return the distance.
+      *
+      * @param void -
+      * @returns
+      *   float value of distance > 0.0f in case of a success,
+      *   0.0f in case of an error
+      */    
+    float Distance_HC_SR04::measureDistance(void) {
+        return measureTicks()*_coeff;
+    }
+
+    /** Measure and return "echo" pulse duration.
+      *
+      * @param void -
+      * @returns
+      *   uint32_t value of distance > 0 in case of a success,
+      *   0 in case of an error
+      */    
+    uint32_t Distance_HC_SR04::measureTicks(void) {
+        reset();
+        trigger();
+
+        while (_state == STARTED)
+            ;
+
+        _echo.rise(NULL);
+        _echo.fall(NULL);
+        _timeout.detach();
+
+        switch (_state) {
+            case COMPLETED:
+                break;
+            default:
+                _ticks_us = 0;
+                break;
+        }
+
+        return _ticks_us;
+    }
+
+    /** Timeout callback function.
+      *
+      * @param void -
+      * @returns
+      *   void -
+      */    
+    void Distance_HC_SR04::_tout(void) {
+        if (_state == STARTED)
+            _state = TIMEOUT;
+    }
+
+    /** Rising edge callback function.
+      *
+      * @param void -
+      * @returns
+      *   void -
+      */    
+    void Distance_HC_SR04::_rising(void) {
+        if (_state == STARTED) {
+            _timer.start();
+        }
+    }
+
+    /** Falling edge callback function.
+      *
+      * @param void -
+      * @returns
+      *   void -
+      */    
+    void Distance_HC_SR04::_falling(void) {
+        if (_state == STARTED) {
+            _timer.stop();
+            _ticks_us = _timer.read_us();
+
+            if (_ticks_us < _tmin_us) {
+                _ticks_us = 0;
+                _state = OUT_OF_RANGE_MIN;
+            } else if (_ticks_us > _tmax_us) {
+                _ticks_us = 0;
+                _state = OUT_OF_RANGE_MAX;
+            } else {
+                _state = COMPLETED;
+            }
+        }
+    }