final_8조 / Mbed OS Line-Tracking

ss

Dependencies:   WS2812 PixelArray Adafruit_GFX

IRreflection.cpp@3:700a0cf6beea, 2019-06-15 (annotated)

Committer:
eunsong
Date:
Sat Jun 15 13:48:45 2019 +0000
Revision:
3:700a0cf6beea
Parent:
0:27e31cadeb36 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eunsong 0:27e31cadeb36 1 #include "IRreflection.h"
eunsong 0:27e31cadeb36 2
eunsong 0:27e31cadeb36 3
eunsong 0:27e31cadeb36 4
eunsong 0:27e31cadeb36 5 // Base class data member initialization (called by derived class init())
eunsong 0:27e31cadeb36 6
eunsong 0:27e31cadeb36 7 TRSensors::TRSensors(PinName MOSI,PinName MISO, PinName CLK, PinName CS) : _spi(MOSI,MISO,CLK), _spi_cs(CS)
eunsong 0:27e31cadeb36 8
eunsong 0:27e31cadeb36 9 {
eunsong 0:27e31cadeb36 10
eunsong 0:27e31cadeb36 11 _spi.format(16,0);
eunsong 0:27e31cadeb36 12
eunsong 0:27e31cadeb36 13 _spi.frequency(2000000);
eunsong 0:27e31cadeb36 14
eunsong 0:27e31cadeb36 15 _spi_cs = 1;
eunsong 0:27e31cadeb36 16
eunsong 0:27e31cadeb36 17 _numSensors = NUMSENSORS;
eunsong 0:27e31cadeb36 18
eunsong 0:27e31cadeb36 19 }
eunsong 0:27e31cadeb36 20
eunsong 0:27e31cadeb36 21
eunsong 0:27e31cadeb36 22
eunsong 0:27e31cadeb36 23
eunsong 0:27e31cadeb36 24
eunsong 0:27e31cadeb36 25
eunsong 0:27e31cadeb36 26
eunsong 0:27e31cadeb36 27 void TRSensors::AnalogRead(int *sensor_values)
eunsong 0:27e31cadeb36 28
eunsong 0:27e31cadeb36 29 {
eunsong 0:27e31cadeb36 30
eunsong 0:27e31cadeb36 31 int i,j,ch;
eunsong 0:27e31cadeb36 32
eunsong 0:27e31cadeb36 33 int values[] = {0,0,0,0,0,0};
eunsong 0:27e31cadeb36 34
eunsong 0:27e31cadeb36 35
eunsong 0:27e31cadeb36 36
eunsong 0:27e31cadeb36 37 for(j = 0;j < _numSensors + 1;j++)
eunsong 0:27e31cadeb36 38
eunsong 0:27e31cadeb36 39 {
eunsong 0:27e31cadeb36 40
eunsong 0:27e31cadeb36 41 ch =j;
eunsong 0:27e31cadeb36 42
eunsong 0:27e31cadeb36 43 _spi_cs = 0;
eunsong 0:27e31cadeb36 44
eunsong 0:27e31cadeb36 45 wait_us(2);
eunsong 0:27e31cadeb36 46
eunsong 0:27e31cadeb36 47 values[j] = _spi.write(ch<<12);
eunsong 0:27e31cadeb36 48
eunsong 0:27e31cadeb36 49 _spi_cs = 1;
eunsong 0:27e31cadeb36 50
eunsong 0:27e31cadeb36 51 wait_us(21);
eunsong 0:27e31cadeb36 52
eunsong 0:27e31cadeb36 53 values[j] = (values[j]>>6);
eunsong 0:27e31cadeb36 54
eunsong 0:27e31cadeb36 55 }
eunsong 0:27e31cadeb36 56
eunsong 0:27e31cadeb36 57
eunsong 0:27e31cadeb36 58
eunsong 0:27e31cadeb36 59 for(i = 0;i < _numSensors;i++)
eunsong 0:27e31cadeb36 60
eunsong 0:27e31cadeb36 61 {
eunsong 0:27e31cadeb36 62
eunsong 0:27e31cadeb36 63 sensor_values[i] = values[i+1];
eunsong 0:27e31cadeb36 64
eunsong 0:27e31cadeb36 65 }
eunsong 0:27e31cadeb36 66
eunsong 0:27e31cadeb36 67
eunsong 0:27e31cadeb36 68
eunsong 0:27e31cadeb36 69 }
eunsong 0:27e31cadeb36 70
eunsong 0:27e31cadeb36 71
eunsong 0:27e31cadeb36 72
eunsong 0:27e31cadeb36 73 // Reads the sensors 10 times and uses the results for
eunsong 0:27e31cadeb36 74
eunsong 0:27e31cadeb36 75 // calibration. The sensor values are not returned; instead, the
eunsong 0:27e31cadeb36 76
eunsong 0:27e31cadeb36 77 // maximum and minimum values found over time are stored internally
eunsong 0:27e31cadeb36 78
eunsong 0:27e31cadeb36 79 // and used for the readCalibrated() method.
eunsong 0:27e31cadeb36 80
eunsong 0:27e31cadeb36 81 void TRSensors::calibrate_init(int *calibratedMin, int *calibratedMax)
eunsong 0:27e31cadeb36 82
eunsong 0:27e31cadeb36 83 {
eunsong 0:27e31cadeb36 84
eunsong 0:27e31cadeb36 85 for(int i=0;i<_numSensors;i++)
eunsong 0:27e31cadeb36 86
eunsong 0:27e31cadeb36 87 {
eunsong 0:27e31cadeb36 88
eunsong 0:27e31cadeb36 89 calibratedMin[i] = 1023;
eunsong 0:27e31cadeb36 90
eunsong 0:27e31cadeb36 91 calibratedMax[i] = 0;
eunsong 0:27e31cadeb36 92
eunsong 0:27e31cadeb36 93 }
eunsong 0:27e31cadeb36 94
eunsong 0:27e31cadeb36 95 }
eunsong 0:27e31cadeb36 96
eunsong 0:27e31cadeb36 97 void TRSensors::calibrate(int *sensor_values, int *calibratedMin, int *calibratedMax)
eunsong 0:27e31cadeb36 98
eunsong 0:27e31cadeb36 99 {
eunsong 0:27e31cadeb36 100
eunsong 0:27e31cadeb36 101 int i=0;
eunsong 0:27e31cadeb36 102
eunsong 0:27e31cadeb36 103 int j=0;
eunsong 0:27e31cadeb36 104
eunsong 0:27e31cadeb36 105
eunsong 0:27e31cadeb36 106
eunsong 0:27e31cadeb36 107 for(j=0;j<10;j++)
eunsong 0:27e31cadeb36 108
eunsong 0:27e31cadeb36 109 {
eunsong 0:27e31cadeb36 110
eunsong 0:27e31cadeb36 111 AnalogRead(sensor_values);
eunsong 0:27e31cadeb36 112
eunsong 0:27e31cadeb36 113 for(i=0;i<_numSensors;i++)
eunsong 0:27e31cadeb36 114
eunsong 0:27e31cadeb36 115 {
eunsong 0:27e31cadeb36 116
eunsong 0:27e31cadeb36 117 // set the max we found THIS time
eunsong 0:27e31cadeb36 118
eunsong 0:27e31cadeb36 119 if(j == 0 || max_sensor_values[i] < sensor_values[i])
eunsong 0:27e31cadeb36 120
eunsong 0:27e31cadeb36 121 max_sensor_values[i] = sensor_values[i];
eunsong 0:27e31cadeb36 122
eunsong 0:27e31cadeb36 123
eunsong 0:27e31cadeb36 124
eunsong 0:27e31cadeb36 125 // set the min we found THIS time
eunsong 0:27e31cadeb36 126
eunsong 0:27e31cadeb36 127 if(j == 0 || min_sensor_values[i] > sensor_values[i])
eunsong 0:27e31cadeb36 128
eunsong 0:27e31cadeb36 129 min_sensor_values[i] = sensor_values[i];
eunsong 0:27e31cadeb36 130
eunsong 0:27e31cadeb36 131 }
eunsong 0:27e31cadeb36 132
eunsong 0:27e31cadeb36 133 }
eunsong 0:27e31cadeb36 134
eunsong 0:27e31cadeb36 135
eunsong 0:27e31cadeb36 136
eunsong 0:27e31cadeb36 137 // record the min and max calibration values
eunsong 0:27e31cadeb36 138
eunsong 0:27e31cadeb36 139 for(i=0;i<_numSensors;i++)
eunsong 0:27e31cadeb36 140
eunsong 0:27e31cadeb36 141 {
eunsong 0:27e31cadeb36 142
eunsong 0:27e31cadeb36 143 if(min_sensor_values[i] > calibratedMax[i])
eunsong 0:27e31cadeb36 144
eunsong 0:27e31cadeb36 145 calibratedMax[i] = min_sensor_values[i];
eunsong 0:27e31cadeb36 146
eunsong 0:27e31cadeb36 147 if(max_sensor_values[i] < calibratedMin[i])
eunsong 0:27e31cadeb36 148
eunsong 0:27e31cadeb36 149 calibratedMin[i] = max_sensor_values[i];
eunsong 0:27e31cadeb36 150
eunsong 0:27e31cadeb36 151 }
eunsong 0:27e31cadeb36 152
eunsong 0:27e31cadeb36 153 }
eunsong 0:27e31cadeb36 154
eunsong 0:27e31cadeb36 155
eunsong 0:27e31cadeb36 156
eunsong 0:27e31cadeb36 157
eunsong 0:27e31cadeb36 158
eunsong 0:27e31cadeb36 159 // Returns values calibrated to a value between 0 and 1000, where
eunsong 0:27e31cadeb36 160
eunsong 0:27e31cadeb36 161 // 0 corresponds to the minimum value read by calibrate() and 1000
eunsong 0:27e31cadeb36 162
eunsong 0:27e31cadeb36 163 // corresponds to the maximum value. Calibration values are
eunsong 0:27e31cadeb36 164
eunsong 0:27e31cadeb36 165 // stored separately for each sensor, so that differences in the
eunsong 0:27e31cadeb36 166
eunsong 0:27e31cadeb36 167 // sensors are accounted for automatically.
eunsong 0:27e31cadeb36 168
eunsong 0:27e31cadeb36 169 void TRSensors::readCalibrated(int *sensor_values, int *calibratedMin, int *calibratedMax)
eunsong 0:27e31cadeb36 170
eunsong 0:27e31cadeb36 171 {
eunsong 0:27e31cadeb36 172
eunsong 0:27e31cadeb36 173 int i;
eunsong 0:27e31cadeb36 174
eunsong 0:27e31cadeb36 175
eunsong 0:27e31cadeb36 176
eunsong 0:27e31cadeb36 177 // read the needed values
eunsong 0:27e31cadeb36 178
eunsong 0:27e31cadeb36 179 AnalogRead(sensor_values);
eunsong 0:27e31cadeb36 180
eunsong 0:27e31cadeb36 181
eunsong 0:27e31cadeb36 182
eunsong 0:27e31cadeb36 183 for(i=0;i<_numSensors;i++)
eunsong 0:27e31cadeb36 184
eunsong 0:27e31cadeb36 185 {
eunsong 0:27e31cadeb36 186
eunsong 0:27e31cadeb36 187 int denominator;
eunsong 0:27e31cadeb36 188
eunsong 0:27e31cadeb36 189
eunsong 0:27e31cadeb36 190
eunsong 0:27e31cadeb36 191 denominator = calibratedMax[i] - calibratedMin[i];
eunsong 0:27e31cadeb36 192
eunsong 0:27e31cadeb36 193
eunsong 0:27e31cadeb36 194
eunsong 0:27e31cadeb36 195 int x = 0;
eunsong 0:27e31cadeb36 196
eunsong 0:27e31cadeb36 197
eunsong 0:27e31cadeb36 198
eunsong 0:27e31cadeb36 199
eunsong 0:27e31cadeb36 200
eunsong 0:27e31cadeb36 201 if(denominator != 0){
eunsong 0:27e31cadeb36 202
eunsong 0:27e31cadeb36 203 if(((signed int)sensor_values[i] - (signed int)calibratedMin[i])<0){
eunsong 0:27e31cadeb36 204
eunsong 0:27e31cadeb36 205 x = 0;}
eunsong 0:27e31cadeb36 206
eunsong 0:27e31cadeb36 207 else {
eunsong 0:27e31cadeb36 208
eunsong 0:27e31cadeb36 209 x = ((sensor_values[i] - calibratedMin[i])*1000/denominator);
eunsong 0:27e31cadeb36 210
eunsong 0:27e31cadeb36 211 }
eunsong 0:27e31cadeb36 212
eunsong 0:27e31cadeb36 213 }
eunsong 0:27e31cadeb36 214
eunsong 0:27e31cadeb36 215
eunsong 0:27e31cadeb36 216
eunsong 0:27e31cadeb36 217 if( x > 1000) x = 1000;
eunsong 0:27e31cadeb36 218
eunsong 0:27e31cadeb36 219
eunsong 0:27e31cadeb36 220
eunsong 0:27e31cadeb36 221 sensor_values[i] = x;
eunsong 0:27e31cadeb36 222
eunsong 0:27e31cadeb36 223
eunsong 0:27e31cadeb36 224
eunsong 0:27e31cadeb36 225
eunsong 0:27e31cadeb36 226
eunsong 0:27e31cadeb36 227 }
eunsong 0:27e31cadeb36 228
eunsong 0:27e31cadeb36 229
eunsong 0:27e31cadeb36 230
eunsong 0:27e31cadeb36 231 }
eunsong 0:27e31cadeb36 232
eunsong 0:27e31cadeb36 233
eunsong 0:27e31cadeb36 234
eunsong 0:27e31cadeb36 235
eunsong 0:27e31cadeb36 236
eunsong 0:27e31cadeb36 237
eunsong 0:27e31cadeb36 238
eunsong 0:27e31cadeb36 239 // Operates the same as read calibrated, but also returns an
eunsong 0:27e31cadeb36 240
eunsong 0:27e31cadeb36 241 // estimated position of the robot with respect to a line. The
eunsong 0:27e31cadeb36 242
eunsong 0:27e31cadeb36 243 // estimate is made using a weighted average of the sensor indices
eunsong 0:27e31cadeb36 244
eunsong 0:27e31cadeb36 245 // multiplied by 1000, so that a return value of 0 indicates that
eunsong 0:27e31cadeb36 246
eunsong 0:27e31cadeb36 247 // the line is directly below sensor 0, a return value of 1000
eunsong 0:27e31cadeb36 248
eunsong 0:27e31cadeb36 249 // indicates that the line is directly below sensor 1, 2000
eunsong 0:27e31cadeb36 250
eunsong 0:27e31cadeb36 251 // indicates that it's below sensor 2000, etc. Intermediate
eunsong 0:27e31cadeb36 252
eunsong 0:27e31cadeb36 253 // values indicate that the line is between two sensors. The
eunsong 0:27e31cadeb36 254
eunsong 0:27e31cadeb36 255 // formula is:
eunsong 0:27e31cadeb36 256
eunsong 0:27e31cadeb36 257 //
eunsong 0:27e31cadeb36 258
eunsong 0:27e31cadeb36 259 // 0*value0 + 1000*value1 + 2000*value2 + ...
eunsong 0:27e31cadeb36 260
eunsong 0:27e31cadeb36 261 // --------------------------------------------
eunsong 0:27e31cadeb36 262
eunsong 0:27e31cadeb36 263 // value0 + value1 + value2 + ...
eunsong 0:27e31cadeb36 264
eunsong 0:27e31cadeb36 265 //
eunsong 0:27e31cadeb36 266
eunsong 0:27e31cadeb36 267 // By default, this function assumes a dark line (high values)
eunsong 0:27e31cadeb36 268
eunsong 0:27e31cadeb36 269 // surrounded by white (low values). If your line is light on
eunsong 0:27e31cadeb36 270
eunsong 0:27e31cadeb36 271 // black, set the optional second argument white_line to true. In
eunsong 0:27e31cadeb36 272
eunsong 0:27e31cadeb36 273 // this case, each sensor value will be replaced by (1000-value)
eunsong 0:27e31cadeb36 274
eunsong 0:27e31cadeb36 275 // before the averaging.
eunsong 0:27e31cadeb36 276
eunsong 0:27e31cadeb36 277
eunsong 0:27e31cadeb36 278
eunsong 0:27e31cadeb36 279 int TRSensors::readLine(int *sensor_values , int *calibratedMin, int *calibratedMax, int *online, char white_line)
eunsong 0:27e31cadeb36 280
eunsong 0:27e31cadeb36 281 {
eunsong 0:27e31cadeb36 282
eunsong 0:27e31cadeb36 283 char i, on_line = 0;
eunsong 0:27e31cadeb36 284
eunsong 0:27e31cadeb36 285 long int avg; // this is for the weighted total, which is long
eunsong 0:27e31cadeb36 286
eunsong 0:27e31cadeb36 287 // before division
eunsong 0:27e31cadeb36 288
eunsong 0:27e31cadeb36 289 int sum; // this is for the denominator which is <= 64000
eunsong 0:27e31cadeb36 290
eunsong 0:27e31cadeb36 291 static int last_value=0; // assume initially that the line is left.
eunsong 0:27e31cadeb36 292
eunsong 0:27e31cadeb36 293
eunsong 0:27e31cadeb36 294
eunsong 0:27e31cadeb36 295 readCalibrated(sensor_values, calibratedMin, calibratedMax);
eunsong 0:27e31cadeb36 296
eunsong 0:27e31cadeb36 297
eunsong 0:27e31cadeb36 298
eunsong 0:27e31cadeb36 299 online[0] = 0;
eunsong 0:27e31cadeb36 300
eunsong 0:27e31cadeb36 301 avg = 0;
eunsong 0:27e31cadeb36 302
eunsong 0:27e31cadeb36 303 sum = 0;
eunsong 0:27e31cadeb36 304
eunsong 0:27e31cadeb36 305
eunsong 0:27e31cadeb36 306
eunsong 0:27e31cadeb36 307 for(i=0;i<_numSensors;i++) {
eunsong 0:27e31cadeb36 308
eunsong 0:27e31cadeb36 309
eunsong 0:27e31cadeb36 310
eunsong 0:27e31cadeb36 311 long int value = sensor_values[i];
eunsong 0:27e31cadeb36 312
eunsong 0:27e31cadeb36 313
eunsong 0:27e31cadeb36 314
eunsong 0:27e31cadeb36 315 //if(!white_line) value = 1000-value;
eunsong 0:27e31cadeb36 316
eunsong 0:27e31cadeb36 317 if(white_line){
eunsong 0:27e31cadeb36 318
eunsong 0:27e31cadeb36 319 value = 1000-value;
eunsong 0:27e31cadeb36 320
eunsong 0:27e31cadeb36 321 //sensor_values[i] = (1000 - sensor_values[i])/100;
eunsong 0:27e31cadeb36 322
eunsong 0:27e31cadeb36 323 }
eunsong 0:27e31cadeb36 324
eunsong 0:27e31cadeb36 325
eunsong 0:27e31cadeb36 326
eunsong 0:27e31cadeb36 327 //sensor_values[i] = value;
eunsong 0:27e31cadeb36 328
eunsong 0:27e31cadeb36 329 // keep track of whether we see the line at all
eunsong 0:27e31cadeb36 330
eunsong 0:27e31cadeb36 331 if(value > 300) {
eunsong 0:27e31cadeb36 332
eunsong 0:27e31cadeb36 333 online[0] = 1;
eunsong 0:27e31cadeb36 334
eunsong 0:27e31cadeb36 335 }
eunsong 0:27e31cadeb36 336
eunsong 0:27e31cadeb36 337
eunsong 0:27e31cadeb36 338
eunsong 0:27e31cadeb36 339 // only average in values that are above a noise threshold
eunsong 0:27e31cadeb36 340
eunsong 0:27e31cadeb36 341
eunsong 0:27e31cadeb36 342 if(value > 30) {
eunsong 0:27e31cadeb36 343
eunsong 0:27e31cadeb36 344
eunsong 0:27e31cadeb36 345
eunsong 0:27e31cadeb36 346 avg += value*(1000*i);//(value/100)*(1000*i) // * sqrt(100,i);
eunsong 0:27e31cadeb36 347
eunsong 0:27e31cadeb36 348 sum += value; //(value/100);
eunsong 0:27e31cadeb36 349
eunsong 0:27e31cadeb36 350 }
eunsong 0:27e31cadeb36 351
eunsong 0:27e31cadeb36 352 }
eunsong 0:27e31cadeb36 353
eunsong 0:27e31cadeb36 354 /*
eunsong 0:27e31cadeb36 355
eunsong 0:27e31cadeb36 356 if(!on_line)
eunsong 0:27e31cadeb36 357
eunsong 0:27e31cadeb36 358 {
eunsong 0:27e31cadeb36 359
eunsong 0:27e31cadeb36 360 // If it last read to the left of center, return 0.
eunsong 0:27e31cadeb36 361
eunsong 0:27e31cadeb36 362 if(last_value < (_numSensors-1)*1000/2)
eunsong 0:27e31cadeb36 363
eunsong 0:27e31cadeb36 364 return 0;
eunsong 0:27e31cadeb36 365
eunsong 0:27e31cadeb36 366
eunsong 0:27e31cadeb36 367
eunsong 0:27e31cadeb36 368 // If it last read to the right of center, return the max.
eunsong 0:27e31cadeb36 369
eunsong 0:27e31cadeb36 370 else
eunsong 0:27e31cadeb36 371
eunsong 0:27e31cadeb36 372 return (_numSensors-1)*1000;
eunsong 0:27e31cadeb36 373
eunsong 0:27e31cadeb36 374 }
eunsong 0:27e31cadeb36 375
eunsong 0:27e31cadeb36 376 */
eunsong 0:27e31cadeb36 377
eunsong 0:27e31cadeb36 378
eunsong 0:27e31cadeb36 379
eunsong 0:27e31cadeb36 380 last_value = avg/sum;
eunsong 0:27e31cadeb36 381
eunsong 0:27e31cadeb36 382
eunsong 0:27e31cadeb36 383
eunsong 0:27e31cadeb36 384 // if(sum == 0) last_value = 0;
eunsong 0:27e31cadeb36 385
eunsong 0:27e31cadeb36 386
eunsong 0:27e31cadeb36 387
eunsong 0:27e31cadeb36 388 return last_value;
eunsong 0:27e31cadeb36 389
eunsong 0:27e31cadeb36 390 }
eunsong 0:27e31cadeb36 391
eunsong 0:27e31cadeb36 392
eunsong 0:27e31cadeb36 393
eunsong 0:27e31cadeb36 394 int TRSensors::sqrt(int m, int k){
eunsong 0:27e31cadeb36 395
eunsong 0:27e31cadeb36 396 int temp = m;
eunsong 0:27e31cadeb36 397
eunsong 0:27e31cadeb36 398 if(k == 0) m = 1;
eunsong 0:27e31cadeb36 399
eunsong 0:27e31cadeb36 400 else{
eunsong 0:27e31cadeb36 401
eunsong 0:27e31cadeb36 402 for(int i=0; i<(k-1); i++){
eunsong 0:27e31cadeb36 403
eunsong 0:27e31cadeb36 404 m = temp *m;
eunsong 0:27e31cadeb36 405
eunsong 0:27e31cadeb36 406 }
eunsong 0:27e31cadeb36 407
eunsong 0:27e31cadeb36 408 }
eunsong 0:27e31cadeb36 409
eunsong 0:27e31cadeb36 410 return m;
eunsong 0:27e31cadeb36 411
eunsong 0:27e31cadeb36 412 }