USN Robotics
Ultrasonic sensor for Robot
ultrasonic.cpp@0:e2f25a48d4d9, 2016-09-01 (annotated)
- Committer:
- azadmehr
- Date:
- Thu Sep 01 08:54:38 2016 +0000
- Revision:
- 0:e2f25a48d4d9
La til ultralyd sensor
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| azadmehr | 0:e2f25a48d4d9 | 1 | #include "ultrasonic.h" |
| azadmehr | 0:e2f25a48d4d9 | 2 | |
| azadmehr | 0:e2f25a48d4d9 | 3 | ultrasonic::ultrasonic(PinName trigPin, PinName echoPin, float updateSpeed, float timeout):_trig(trigPin), _echo(echoPin) |
| azadmehr | 0:e2f25a48d4d9 | 4 | { |
| azadmehr | 0:e2f25a48d4d9 | 5 | _updateSpeed = updateSpeed; |
| azadmehr | 0:e2f25a48d4d9 | 6 | _timeout = timeout; |
| azadmehr | 0:e2f25a48d4d9 | 7 | } |
| azadmehr | 0:e2f25a48d4d9 | 8 | |
| azadmehr | 0:e2f25a48d4d9 | 9 | ultrasonic::ultrasonic(PinName trigPin, PinName echoPin, float updateSpeed, float timeout, void onUpdate(int)) |
| azadmehr | 0:e2f25a48d4d9 | 10 | :_trig(trigPin), _echo(echoPin) |
| azadmehr | 0:e2f25a48d4d9 | 11 | { |
| azadmehr | 0:e2f25a48d4d9 | 12 | _onUpdateMethod=onUpdate; |
| azadmehr | 0:e2f25a48d4d9 | 13 | _updateSpeed = updateSpeed; |
| azadmehr | 0:e2f25a48d4d9 | 14 | _timeout = timeout; |
| azadmehr | 0:e2f25a48d4d9 | 15 | _t.start (); |
| azadmehr | 0:e2f25a48d4d9 | 16 | } |
| azadmehr | 0:e2f25a48d4d9 | 17 | void ultrasonic::_startT() |
| azadmehr | 0:e2f25a48d4d9 | 18 | { |
| azadmehr | 0:e2f25a48d4d9 | 19 | if(_t.read()>600) |
| azadmehr | 0:e2f25a48d4d9 | 20 | { |
| azadmehr | 0:e2f25a48d4d9 | 21 | _t.reset (); |
| azadmehr | 0:e2f25a48d4d9 | 22 | } |
| azadmehr | 0:e2f25a48d4d9 | 23 | start = _t.read_us (); |
| azadmehr | 0:e2f25a48d4d9 | 24 | } |
| azadmehr | 0:e2f25a48d4d9 | 25 | |
| azadmehr | 0:e2f25a48d4d9 | 26 | void ultrasonic::_updateDist() |
| azadmehr | 0:e2f25a48d4d9 | 27 | { |
| azadmehr | 0:e2f25a48d4d9 | 28 | end = _t.read_us (); |
| azadmehr | 0:e2f25a48d4d9 | 29 | done = 1; |
| azadmehr | 0:e2f25a48d4d9 | 30 | _distance = (end - start)/6; |
| azadmehr | 0:e2f25a48d4d9 | 31 | _tout.detach(); |
| azadmehr | 0:e2f25a48d4d9 | 32 | _tout.attach(this,&ultrasonic::_startTrig, _updateSpeed); |
| azadmehr | 0:e2f25a48d4d9 | 33 | } |
| azadmehr | 0:e2f25a48d4d9 | 34 | void ultrasonic::_startTrig(void) |
| azadmehr | 0:e2f25a48d4d9 | 35 | { |
| azadmehr | 0:e2f25a48d4d9 | 36 | _tout.detach(); |
| azadmehr | 0:e2f25a48d4d9 | 37 | _trig=1; |
| azadmehr | 0:e2f25a48d4d9 | 38 | wait_us(10); |
| azadmehr | 0:e2f25a48d4d9 | 39 | done = 0; |
| azadmehr | 0:e2f25a48d4d9 | 40 | _echo.rise(this,&ultrasonic::_startT); |
| azadmehr | 0:e2f25a48d4d9 | 41 | _echo.fall(this,&ultrasonic::_updateDist); |
| azadmehr | 0:e2f25a48d4d9 | 42 | _echo.enable_irq (); |
| azadmehr | 0:e2f25a48d4d9 | 43 | _tout.attach(this,&ultrasonic::_startTrig,_timeout); |
| azadmehr | 0:e2f25a48d4d9 | 44 | _trig=0; |
| azadmehr | 0:e2f25a48d4d9 | 45 | } |
| azadmehr | 0:e2f25a48d4d9 | 46 | |
| azadmehr | 0:e2f25a48d4d9 | 47 | int ultrasonic::getCurrentDistance(void) |
| azadmehr | 0:e2f25a48d4d9 | 48 | { |
| azadmehr | 0:e2f25a48d4d9 | 49 | return _distance; |
| azadmehr | 0:e2f25a48d4d9 | 50 | } |
| azadmehr | 0:e2f25a48d4d9 | 51 | void ultrasonic::pauseUpdates(void) |
| azadmehr | 0:e2f25a48d4d9 | 52 | { |
| azadmehr | 0:e2f25a48d4d9 | 53 | _tout.detach(); |
| azadmehr | 0:e2f25a48d4d9 | 54 | _echo.rise(NULL); |
| azadmehr | 0:e2f25a48d4d9 | 55 | _echo.fall(NULL); |
| azadmehr | 0:e2f25a48d4d9 | 56 | } |
| azadmehr | 0:e2f25a48d4d9 | 57 | void ultrasonic::startUpdates(void) |
| azadmehr | 0:e2f25a48d4d9 | 58 | { |
| azadmehr | 0:e2f25a48d4d9 | 59 | _startTrig(); |
| azadmehr | 0:e2f25a48d4d9 | 60 | } |
| azadmehr | 0:e2f25a48d4d9 | 61 | void ultrasonic::attachOnUpdate(void method(int)) |
| azadmehr | 0:e2f25a48d4d9 | 62 | { |
| azadmehr | 0:e2f25a48d4d9 | 63 | _onUpdateMethod = method; |
| azadmehr | 0:e2f25a48d4d9 | 64 | } |
| azadmehr | 0:e2f25a48d4d9 | 65 | void ultrasonic::changeUpdateSpeed(float updateSpeed) |
| azadmehr | 0:e2f25a48d4d9 | 66 | { |
| azadmehr | 0:e2f25a48d4d9 | 67 | _updateSpeed = updateSpeed; |
| azadmehr | 0:e2f25a48d4d9 | 68 | } |
| azadmehr | 0:e2f25a48d4d9 | 69 | float ultrasonic::getUpdateSpeed() |
| azadmehr | 0:e2f25a48d4d9 | 70 | { |
| azadmehr | 0:e2f25a48d4d9 | 71 | return _updateSpeed; |
| azadmehr | 0:e2f25a48d4d9 | 72 | } |
| azadmehr | 0:e2f25a48d4d9 | 73 | int ultrasonic::isUpdated(void) |
| azadmehr | 0:e2f25a48d4d9 | 74 | { |
| azadmehr | 0:e2f25a48d4d9 | 75 | //printf("%d", done); |
| azadmehr | 0:e2f25a48d4d9 | 76 | d=done; |
| azadmehr | 0:e2f25a48d4d9 | 77 | done = 0; |
| azadmehr | 0:e2f25a48d4d9 | 78 | return d; |
| azadmehr | 0:e2f25a48d4d9 | 79 | } |
| azadmehr | 0:e2f25a48d4d9 | 80 | void ultrasonic::checkDistance(void) |
| azadmehr | 0:e2f25a48d4d9 | 81 | { |
| azadmehr | 0:e2f25a48d4d9 | 82 | if(isUpdated()) |
| azadmehr | 0:e2f25a48d4d9 | 83 | { |
| azadmehr | 0:e2f25a48d4d9 | 84 | (*_onUpdateMethod)(_distance); |
| azadmehr | 0:e2f25a48d4d9 | 85 | } |
| azadmehr | 0:e2f25a48d4d9 | 86 | } |