Diff: angledSonar.cpp

Revision:

0:932004bfb013

--- /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