Tom Fransen / angledSonar

Sonar

angledSonar.cpp@0:932004bfb013, 2018-06-13 (annotated)

Committer:
twjfransen
Date:
Wed Jun 13 09:22:37 2018 +0000
Revision:
0:932004bfb013
...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
twjfransen 0:932004bfb013 1 #include "angledSonar.h"
twjfransen 0:932004bfb013 2
twjfransen 0:932004bfb013 3 Sonar::Sonar(PinName trig, PinName echo, float angle_z, float x, float y, float z)
twjfransen 0:932004bfb013 4 :_trig(trig)
twjfransen 0:932004bfb013 5 ,_echo(echo)
twjfransen 0:932004bfb013 6 ,_x(x)
twjfransen 0:932004bfb013 7 ,_y(y)
twjfransen 0:932004bfb013 8 ,_z(z)
twjfransen 0:932004bfb013 9 {
twjfransen 0:932004bfb013 10 _timer.start();
twjfransen 0:932004bfb013 11 _echo.rise(callback(this, &Sonar::echoBegin));
twjfransen 0:932004bfb013 12 _echo.fall(callback(this, &Sonar::echoEnd));
twjfransen 0:932004bfb013 13 _pinged = false;
twjfransen 0:932004bfb013 14
twjfransen 0:932004bfb013 15 _ratio_x = -sin(angle_z);
twjfransen 0:932004bfb013 16 _ratio_y = cos(angle_z);
twjfransen 0:932004bfb013 17 }
twjfransen 0:932004bfb013 18
twjfransen 0:932004bfb013 19 void Sonar::echoBegin()
twjfransen 0:932004bfb013 20 {
twjfransen 0:932004bfb013 21 _timer.reset();
twjfransen 0:932004bfb013 22 _begin = _timer.read_us();}
twjfransen 0:932004bfb013 23
twjfransen 0:932004bfb013 24 void Sonar::echoEnd()
twjfransen 0:932004bfb013 25 {
twjfransen 0:932004bfb013 26 _end = _timer.read_us();
twjfransen 0:932004bfb013 27 _distance = (_end - _begin)/58.3;
twjfransen 0:932004bfb013 28 _pinged = true;
twjfransen 0:932004bfb013 29 }
twjfransen 0:932004bfb013 30
twjfransen 0:932004bfb013 31 float Sonar::getDistance()
twjfransen 0:932004bfb013 32 {
twjfransen 0:932004bfb013 33 _trig = 1;
twjfransen 0:932004bfb013 34 wait_us(10);
twjfransen 0:932004bfb013 35 _trig = 0;
twjfransen 0:932004bfb013 36 while(!_pinged){printf("");}
twjfransen 0:932004bfb013 37 _pinged = false;
twjfransen 0:932004bfb013 38 return _distance;
twjfransen 0:932004bfb013 39 }
twjfransen 0:932004bfb013 40
twjfransen 0:932004bfb013 41 void Sonar::pulse()
twjfransen 0:932004bfb013 42 {
twjfransen 0:932004bfb013 43 _trig = 1;
twjfransen 0:932004bfb013 44 wait_us(10);
twjfransen 0:932004bfb013 45 _trig = 0;
twjfransen 0:932004bfb013 46 }
twjfransen 0:932004bfb013 47
twjfransen 0:932004bfb013 48 void Sonar::autoPulse(bool on_off, float frequency_ms /*=1*/)
twjfransen 0:932004bfb013 49 {
twjfransen 0:932004bfb013 50 if (on_off) {
twjfransen 0:932004bfb013 51 _ticker.attach(callback(this, &Sonar::pulse), frequency_ms/1000);
twjfransen 0:932004bfb013 52 }
twjfransen 0:932004bfb013 53 else {
twjfransen 0:932004bfb013 54 _ticker.detach();
twjfransen 0:932004bfb013 55 }
twjfransen 0:932004bfb013 56 }
twjfransen 0:932004bfb013 57
twjfransen 0:932004bfb013 58 Point Sonar::getCoordinate()
twjfransen 0:932004bfb013 59 {
twjfransen 0:932004bfb013 60 float distance = getDistance();
twjfransen 0:932004bfb013 61 Point newPoint;
twjfransen 0:932004bfb013 62 newPoint.x = distance * _ratio_x + _x;
twjfransen 0:932004bfb013 63 newPoint.y = distance * _ratio_y + _y;
twjfransen 0:932004bfb013 64 return newPoint;
twjfransen 0:932004bfb013 65 }