-
-
Notifications
You must be signed in to change notification settings - Fork 348
/
Ultrasonic.cpp
134 lines (119 loc) · 3.11 KB
/
Ultrasonic.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
/*
* Sming library: Ultrasonic sonar modules driver
*
* Tested on modules:
* HC-SR04 - ranges: 2-400cm, power: 5v, levels: TTL, for work with 3.3v need voltage divider on ECHO pin
*
* Author: sharky <nik.sharky@gmail.com>
* Thanks to: ITead studio, DIMOSUS, ElecFreaks, Wiki, Parallax for information and ideas
*
* Changed: 2015-04-07
* TODO: Add multisampling to ping and range methods
* TODO: Add setMedium method to set current medium parameters
* TODO: Fix bugs with sqrt in temp measurement
*/
#include "Ultrasonic.h"
Ultrasonic::Ultrasonic()
{
}
/**
* Initialize ultrasonic sensor
*/
void Ultrasonic::begin(uint16_t trigPin, uint8_t echoPin)
{
pinTRIG = trigPin;
pinECHO = echoPin;
pinMode(pinTRIG, OUTPUT);
pinMode(pinECHO, INPUT);
digitalWrite(pinTRIG, LOW);
}
/**
* Convert ping duration to inches
*
* The speed of sound is 1130 feet/s or 73.746 microseconds per inch.
* The ping travels out and back, so to find the distance to the obstacle we take half of the distance traveled.
*/
uint16_t Ultrasonic::us2inch(unsigned long microseconds)
{
return microseconds / usPerInch;
}
/**
* Convert ping duration to centimeters
*
* The speed of sound is 340 m/s or 29 microseconds per centimeter.
* The ping travels out and back, so to find the distance to the obstacle we take half of the distance traveled.
*/
uint16_t Ultrasonic::us2cm(unsigned long microseconds)
{
return microseconds / usPerCM;
}
/**
* Trigger pulse and wait for echo
*/
uint32_t Ultrasonic::ping()
{
digitalWrite(pinTRIG, LOW);
delayMicroseconds(2);
digitalWrite(pinTRIG, HIGH);
delayMicroseconds(trigDuration);
digitalWrite(pinTRIG, LOW);
return pulseIn(pinECHO, HIGH, echoTimeout);
}
/**
* Measure distance in centimeters
*/
uint16_t Ultrasonic::rangeCM()
{
return us2cm(ping()) / 2;
}
/**
* Measure distance in inches
*/
uint16_t Ultrasonic::rangeInch()
{
return us2inch(ping()) / 2;
}
/**
* Calculate temperature in Celsius biasing on known base dist/temp
*
* c = sqrt(X*R*T), where:
* ñ - speed of sound, m/s
* X - adiabatic index (is about 1.4 for air under normal conditions of pressure and temperature)
* R - gas constant (for air approximately 8.3145 J/mol·K, ~286.9 J/kg·K)
* T - the absolute temperature in kelvin
*
* @param baseDist base calibration distance in cm
* @param baseTemp base calibration temp (valid for baseDist)
* @param samples number of samples for approximation result
*
* See http://habrahabr.ru/post/243357/ for details
*/
uint16_t Ultrasonic::temp(uint16_t baseDist, uint16_t baseTemp, uint16_t samples)
{
uint32_t dist = 0;
for (int i = 0; i < samples; i++)
{
dist += rangeCM();
delay(60);
}
// avg dist
dist /= samples;
// c = sqrt(X*R*T)
// TODO !!! Draft for compile, in root need to check types/overflows/signs here
float speedOfSound = baseDist / dist * root(XR * (K + baseTemp));
// T = (c*c)/(X*R) in Kelvin
return (speedOfSound * speedOfSound) / (XR) - K;
}
unsigned int Ultrasonic::root(unsigned int x)
{
unsigned int a, b;
b = x;
a = x = 0x3f;
x = b / x;
a = x = (x + a) >> 1;
x = b / x;
a = x = (x + a) >> 1;
x = b / x;
x = (x + a) >> 1;
return (x);
}