-
Notifications
You must be signed in to change notification settings - Fork 49
/
SharpIR.cpp
204 lines (169 loc) · 6.63 KB
/
SharpIR.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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
/*
SharpIR
Arduino library for retrieving distance (in cm) from the analog GP2Y0A21Y and GP2Y0A02YK
From an original version of Dr. Marcal Casas-Cartagena ([email protected])
Version : 1.0 : Guillaume Rico
+ Remove average and use median
+ Definition of number of sample in .h
+ Define IR pin as input
Version : 1.1 : Thibaut Mauon
+ Add SHARP GP2Y0A710K0F for 100cm to 500cm by Thibaut Mauron
Version : 1.2 : Archery2000
+ Add Median of Medians algorithm to speed up sensor reading computation
https://github.com/guillaume-rico/SharpIR
Original comment from Dr. Marcal Casas-Cartagena :
The Sahrp IR sensors are cheap but somehow unreliable. I've found that when doing continous readings to a
fix object, the distance given oscilates quite a bit from time to time. For example I had an object at
31 cm. The readings from the sensor were mainly steady at the correct distance but eventually the distance
given dropped down to 25 cm or even 16 cm. That's quite a bit and for some applications it is quite
unacceptable. I checked the library http://code.google.com/p/gp2y0a21yk-library/ by Jeroen Doggen
([email protected]) and what the author was doing is to take a bunch of readings and give an average of them
The present library works similary. It reads a bunch of readings (avg), it checks if the current reading
differs a lot from the previous one (tolerance) and if it doesn't differ a lot, it takes it into account
for the mean distance.
The distance is calculated from a formula extracted from the graphs on the sensors datasheets
After some tests, I think that a set of 20 to 25 readings is more than enough to get an accurate distance
Reading 25 times and return a mean distance takes 53 ms. For my application of the sensor is fast enough.
This library has the formulas to work with the GP2Y0A21Y and the GP2Y0A02YK sensors but exanding it for
other sensors is easy enough.
*/
#ifdef Arduino
#include "Arduino.h"
#elif defined(SPARK)
#include "Particle.h"
#include "math.h"
#endif
#include "SharpIR.h"
// Initialisation function
// + irPin : is obviously the pin where the IR sensor is attached
// + sensorModel is a int to differentiate the two sensor models this library currently supports:
// > 1080 is the int for the GP2Y0A21Y and
// > 20150 is the int for GP2Y0A02YK and
// > 100500 is the long for GP2Y0A710K0F
// The numbers reflect the distance range they are designed for (in cm)
SharpIR::SharpIR(int irPin, long sensorModel) {
_irPin=irPin;
_model=sensorModel;
// Define pin as Input
pinMode (_irPin, INPUT);
#ifdef ARDUINO
analogReference(DEFAULT);
#endif
}
// Sort an array
void SharpIR::sort(int a[], int size) {
for(int i=0; i<(size-1); i++) {
bool flag = true;
for(int o=0; o<(size-(i+1)); o++) {
if(a[o] > a[o+1]) {
int t = a[o];
a[o] = a[o+1];
a[o+1] = t;
flag = false;
}
}
if (flag) break;
}
}
// Read distance and compute it
int SharpIR::distance() {
int ir_val[NB_SAMPLE] = {};
int distanceCM;
float current;
int median;
for (int i=0; i<NB_SAMPLE; i++){
// Read analog value
ir_val[i] = analogRead(_irPin);
}
// Get the approx median
#if USE_MEDOFMEDIANS
median = medianOfMedians(ir_val, NB_SAMPLE);
#else
sort(ir_val, NB_SAMPLE);
median = ir_val[NB_SAMPLE/2];
#endif
if (_model==1080) {
// Different expressions required as the Photon has 12 bit ADCs vs 10 bit for Arduinos
#ifdef ARDUINO
distanceCM = 29.988 * pow(map(median, 0, 1023, 0, 5000)/1000.0, -1.173);
#elif defined(SPARK)
distanceCM = 29.988 * pow(map(median, 0, 4095, 0, 5000)/1000.0, -1.173);
#endif
} else if (_model==20150){
// Previous formula used by Dr. Marcal Casas-Cartagena
// puntualDistance=61.573*pow(voltFromRaw/1000, -1.1068);
// Different expressions required as the Photon has 12 bit ADCs vs 10 bit for Arduinos
#ifdef ARDUINO
distanceCM = 60.374 * pow(map(median, 0, 1023, 0, 5000)/1000.0, -1.16);
#elif defined(SPARK)
distanceCM = 60.374 * pow(map(median, 0, 4095, 0, 5000)/1000.0, -1.16);
#endif
} else if (_model==430){
// Different expressions required as the Photon has 12 bit ADCs vs 10 bit for Arduinos
#ifdef ARDUINO
distanceCM = 12.08 * pow(map(median, 0, 1023, 0, 5000)/1000.0, -1.058);
#elif defined(SPARK)
distanceCM = 12.08 * pow(map(median, 0, 4095, 0, 5000)/1000.0, -1.058);
#endif
} else if (_model==215){
// Different expressions required as the Photon has 12 bit ADCs vs 10 bit for Arduinos
// puntualDistance=5.2819*pow(voltFromRaw/1000, -1.161);
#ifdef ARDUINO
distanceCM = 5.2819 * pow(map(median, 0, 1023, 0, 5000)/1000.0, -1.161);
#elif defined(SPARK)
distanceCM = 5.2819 * pow(map(median, 0, 4095, 0, 5000)/1000.0, -1.161);
#endif
} else if (_model==100500){
#ifdef ARDUINO
current = map(median, 0, 1023, 0, 5000);
#elif defined(SPARK)
current = map(median, 0, 4095, 0, 5000);
#endif
// use the inverse number of distance like in the datasheet (1/L)
// y = mx + b = 137500*x + 1125
// x = (y - 1125) / 137500
// Different expressions required as the Photon has 12 bit ADCs vs 10 bit for Arduinos
if (current < 1400 || current > 3300) {
//false data
distanceCM = 0;
} else {
distanceCM = 1.0 / (((current - 1125.0) / 1000.0) / 137.5);
}
}
return distanceCM;
}
int SharpIR::medianOfMedians(int a[], int size){
int ans;
int numMedians = size / 5;
int* medians = new int[numMedians];
for(int i = 0; i < numMedians; i++){
partialSort(a, i * 5, i * 5 + 4);
medians[i] = a[i * 5 + 2];
}
if(numMedians > 25){
ans = medianOfMedians(medians, numMedians);
}else{
sort(medians, numMedians);
ans = medians[numMedians/2];
}
delete [] medians;
medians = nullptr;
return ans;
}
// Sort a partial array
void SharpIR::partialSort(int a[], int min, int max) {
int t;
bool flag;
for(int i=min; i<max; i++) {
flag = true;
for(int o=min; o<(max-i); o++) {
if(a[o] > a[o+1]) {
t = a[o];
a[o] = a[o+1];
a[o+1] = t;
flag = false;
}
}
if (flag) break;
}
}