Revision 0:932004bfb013, committed 2018-06-13

Comitter:

twjfransen

Date:

Wed Jun 13 09:22:37 2018 +0000

Commit message:

...

Changed in this revision

diff -r 000000000000 -r 932004bfb013 angledSonar.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angledSonar.cpp	Wed Jun 13 09:22:37 2018 +0000
@@ -0,0 +1,65 @@
+#include "angledSonar.h"
+
+Sonar::Sonar(PinName trig, PinName echo, float angle_z, float x, float y, float z)
+    :_trig(trig)
+    ,_echo(echo)
+    ,_x(x)
+    ,_y(y)
+    ,_z(z)
+{
+    _timer.start();
+    _echo.rise(callback(this, &Sonar::echoBegin));
+    _echo.fall(callback(this, &Sonar::echoEnd));
+    _pinged = false;
+    
+    _ratio_x = -sin(angle_z);
+    _ratio_y = cos(angle_z);
+}
+
+void Sonar::echoBegin()
+{
+    _timer.reset();
+    _begin = _timer.read_us();}
+
+void Sonar::echoEnd()
+{
+    _end = _timer.read_us();
+    _distance = (_end - _begin)/58.3;
+    _pinged = true;
+}
+
+float Sonar::getDistance()
+{
+    _trig = 1;
+    wait_us(10);
+    _trig = 0;
+    while(!_pinged){printf("");}
+    _pinged = false;
+    return _distance;
+}
+
+void Sonar::pulse()
+{
+    _trig = 1;
+    wait_us(10);
+    _trig = 0;
+}
+
+void Sonar::autoPulse(bool on_off, float frequency_ms /*=1*/)
+{
+    if (on_off) {
+        _ticker.attach(callback(this, &Sonar::pulse), frequency_ms/1000);
+    }
+    else {
+        _ticker.detach();
+    }
+}
+
+Point Sonar::getCoordinate()
+{
+    float distance = getDistance();
+    Point newPoint;
+    newPoint.x = distance * _ratio_x + _x;
+    newPoint.y = distance * _ratio_y + _y;
+    return newPoint;
+}
\ No newline at end of file

diff -r 000000000000 -r 932004bfb013 angledSonar.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angledSonar.h	Wed Jun 13 09:22:37 2018 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+
+struct Point{
+    float x;
+    float y;
+};
+
+class Sonar{
+    public:
+    Sonar(PinName trig, PinName echo, float angle_z = 0, float x = 0, float y = 0, float z = 0);
+    float getDistance();
+    float getLastDistance() {return _distance;}
+    Point getCoordinate();
+    void autoPulse(bool on_off, float frequency_ms = 1);
+    void pulse();
+    
+    private:
+    Ticker _ticker;
+    Timer _timer;
+    DigitalOut _trig;
+    InterruptIn _echo;
+    
+    int _begin;
+    int _end;
+    float _distance;
+    bool _pinged;
+    
+    float _ratio_x;
+    float _ratio_y;
+    float _x;
+    float _y;
+    float _z;
+    
+    void echoBegin();
+    void echoEnd();
+};
\ No newline at end of file