Maxbotix ultrasonic distance sensor library
Fork of MaxbotixDriver by
sonar.cpp@4:95f696f59d94, 2020-01-30 (annotated)
- Committer:
- SomeRandomBloke
- Date:
- Thu Jan 30 11:52:10 2020 +0000
- Revision:
- 4:95f696f59d94
- Parent:
- 3:c231deea4d36
Added range averaging
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DanielC | 0:7e65f5077f5a | 1 | #include "sonar.h" |
SomeRandomBloke | 4:95f696f59d94 | 2 | extern Serial pc; |
DanielC | 0:7e65f5077f5a | 3 | |
DanielC | 0:7e65f5077f5a | 4 | Sonar::Sonar(PinName input, Timer& t) : |
DanielC | 0:7e65f5077f5a | 5 | interrupt(input), |
DanielC | 0:7e65f5077f5a | 6 | time(t), |
DanielC | 0:7e65f5077f5a | 7 | pulseStartTime(0), |
DanielC | 0:7e65f5077f5a | 8 | range(0) { |
SomeRandomBloke | 3:c231deea4d36 | 9 | interrupt.rise(callback(this,&Sonar::pulseStart)); |
SomeRandomBloke | 3:c231deea4d36 | 10 | interrupt.fall(callback(this,&Sonar::pulseStop)); |
DanielC | 0:7e65f5077f5a | 11 | } |
DanielC | 0:7e65f5077f5a | 12 | |
DanielC | 0:7e65f5077f5a | 13 | int Sonar::read() { |
DanielC | 0:7e65f5077f5a | 14 | return range; |
DanielC | 0:7e65f5077f5a | 15 | } |
DanielC | 0:7e65f5077f5a | 16 | |
SomeRandomBloke | 1:330989f98a6e | 17 | int Sonar::readCm() { |
SomeRandomBloke | 1:330989f98a6e | 18 | return range / 58; // 58uS per CM |
SomeRandomBloke | 1:330989f98a6e | 19 | } |
SomeRandomBloke | 1:330989f98a6e | 20 | |
DanielC | 0:7e65f5077f5a | 21 | Sonar::operator int() { |
DanielC | 0:7e65f5077f5a | 22 | return read(); |
DanielC | 0:7e65f5077f5a | 23 | } |
DanielC | 0:7e65f5077f5a | 24 | |
DanielC | 0:7e65f5077f5a | 25 | void Sonar::pulseStart() { |
DanielC | 0:7e65f5077f5a | 26 | pulseStartTime = time.read_us(); |
DanielC | 0:7e65f5077f5a | 27 | } |
DanielC | 0:7e65f5077f5a | 28 | |
DanielC | 0:7e65f5077f5a | 29 | void Sonar::pulseStop() { |
DanielC | 0:7e65f5077f5a | 30 | int endTime = time.read_us(); |
SomeRandomBloke | 4:95f696f59d94 | 31 | |
DanielC | 0:7e65f5077f5a | 32 | if (endTime < pulseStartTime) return; // Escape if there's been a roll over |
SomeRandomBloke | 1:330989f98a6e | 33 | range = (endTime - pulseStartTime); // / 58; // 58uS per CM |
SomeRandomBloke | 4:95f696f59d94 | 34 | } |
SomeRandomBloke | 4:95f696f59d94 | 35 | |
SomeRandomBloke | 4:95f696f59d94 | 36 | |
SomeRandomBloke | 4:95f696f59d94 | 37 | // Functions used in processing readings |
SomeRandomBloke | 4:95f696f59d94 | 38 | |
SomeRandomBloke | 4:95f696f59d94 | 39 | /* isort - Simple sort function for the set of readings |
SomeRandomBloke | 4:95f696f59d94 | 40 | * Sorting function (Author: Bill Gentles, Nov. 12, 2010) |
SomeRandomBloke | 4:95f696f59d94 | 41 | * The sorted array is returned in the original array |
SomeRandomBloke | 4:95f696f59d94 | 42 | * @param a - Array of unsigned 16 bit values to sort |
SomeRandomBloke | 4:95f696f59d94 | 43 | * @param n - Number of values in array |
SomeRandomBloke | 4:95f696f59d94 | 44 | */ |
SomeRandomBloke | 4:95f696f59d94 | 45 | void isort(int *a, int n) |
SomeRandomBloke | 4:95f696f59d94 | 46 | { |
SomeRandomBloke | 4:95f696f59d94 | 47 | for (int i = 1; i < n; ++i) { |
SomeRandomBloke | 4:95f696f59d94 | 48 | int j = a[i]; |
SomeRandomBloke | 4:95f696f59d94 | 49 | int k; |
SomeRandomBloke | 4:95f696f59d94 | 50 | for (k = i - 1; (k >= 0) && (j < a[k]); k--) { |
SomeRandomBloke | 4:95f696f59d94 | 51 | a[k + 1] = a[k]; |
SomeRandomBloke | 4:95f696f59d94 | 52 | } |
SomeRandomBloke | 4:95f696f59d94 | 53 | a[k + 1] = j; |
SomeRandomBloke | 4:95f696f59d94 | 54 | } |
SomeRandomBloke | 4:95f696f59d94 | 55 | } |
SomeRandomBloke | 4:95f696f59d94 | 56 | |
SomeRandomBloke | 4:95f696f59d94 | 57 | /** mode - Mode function, returning the mode or median. |
SomeRandomBloke | 4:95f696f59d94 | 58 | * @param x - Array of unsigned 16 bit values to sort |
SomeRandomBloke | 4:95f696f59d94 | 59 | * @param n - Number of values in array |
SomeRandomBloke | 4:95f696f59d94 | 60 | * @return mode or median of the passed data. |
SomeRandomBloke | 4:95f696f59d94 | 61 | */ |
SomeRandomBloke | 4:95f696f59d94 | 62 | int mode(int *x,int n) |
SomeRandomBloke | 4:95f696f59d94 | 63 | { |
SomeRandomBloke | 4:95f696f59d94 | 64 | int i = 0; |
SomeRandomBloke | 4:95f696f59d94 | 65 | int count = 0; |
SomeRandomBloke | 4:95f696f59d94 | 66 | int maxCount = 0; |
SomeRandomBloke | 4:95f696f59d94 | 67 | int mode = 0; |
SomeRandomBloke | 4:95f696f59d94 | 68 | int bimodal = 0; |
SomeRandomBloke | 4:95f696f59d94 | 69 | int prevCount = 0; |
SomeRandomBloke | 4:95f696f59d94 | 70 | while(i<(n-1)) { |
SomeRandomBloke | 4:95f696f59d94 | 71 | prevCount=count; |
SomeRandomBloke | 4:95f696f59d94 | 72 | count=0; |
SomeRandomBloke | 4:95f696f59d94 | 73 | while( x[i]==x[i+1] ) { |
SomeRandomBloke | 4:95f696f59d94 | 74 | count++; |
SomeRandomBloke | 4:95f696f59d94 | 75 | i++; |
SomeRandomBloke | 4:95f696f59d94 | 76 | } |
SomeRandomBloke | 4:95f696f59d94 | 77 | if( count > prevCount & count > maxCount) { |
SomeRandomBloke | 4:95f696f59d94 | 78 | mode=x[i]; |
SomeRandomBloke | 4:95f696f59d94 | 79 | maxCount=count; |
SomeRandomBloke | 4:95f696f59d94 | 80 | bimodal=0; |
SomeRandomBloke | 4:95f696f59d94 | 81 | } |
SomeRandomBloke | 4:95f696f59d94 | 82 | if( count == 0 ) { |
SomeRandomBloke | 4:95f696f59d94 | 83 | i++; |
SomeRandomBloke | 4:95f696f59d94 | 84 | } |
SomeRandomBloke | 4:95f696f59d94 | 85 | if( count == maxCount ) { //If the dataset has 2 or more modes. |
SomeRandomBloke | 4:95f696f59d94 | 86 | bimodal=1; |
SomeRandomBloke | 4:95f696f59d94 | 87 | } |
SomeRandomBloke | 4:95f696f59d94 | 88 | if( mode==0 || bimodal==1 ) { // Return the median if there is no mode. |
SomeRandomBloke | 4:95f696f59d94 | 89 | mode=x[(n/2)]; |
SomeRandomBloke | 4:95f696f59d94 | 90 | } |
SomeRandomBloke | 4:95f696f59d94 | 91 | return mode; |
SomeRandomBloke | 4:95f696f59d94 | 92 | } |
SomeRandomBloke | 4:95f696f59d94 | 93 | return mode; |
SomeRandomBloke | 4:95f696f59d94 | 94 | } |
SomeRandomBloke | 4:95f696f59d94 | 95 | |
SomeRandomBloke | 4:95f696f59d94 | 96 | // Ultrasonic sensor code |
SomeRandomBloke | 4:95f696f59d94 | 97 | |
SomeRandomBloke | 4:95f696f59d94 | 98 | /** getrange - Get the range in cm. Need to enable sensor first, waiting briefly for it to power up. |
SomeRandomBloke | 4:95f696f59d94 | 99 | * Request temperature sensor to take a reading. |
SomeRandomBloke | 4:95f696f59d94 | 100 | * Disable ultrasonic sensor after use for power saving. |
SomeRandomBloke | 4:95f696f59d94 | 101 | * Returns uncompensated, compensated and temperature values by reference. |
SomeRandomBloke | 4:95f696f59d94 | 102 | * Calling function can still do checks to determine whether to use reading or not. |
SomeRandomBloke | 4:95f696f59d94 | 103 | * |
SomeRandomBloke | 4:95f696f59d94 | 104 | * @param outRawDistance - Raw uncompensated distance |
SomeRandomBloke | 4:95f696f59d94 | 105 | * @param outDistance - Compensated distance if temperature available, otherwise raw distance |
SomeRandomBloke | 4:95f696f59d94 | 106 | * @param outTemperature - Temperature reading |
SomeRandomBloke | 4:95f696f59d94 | 107 | * @return boolean true returned form function for valid reading. false for <23cm or out of range |
SomeRandomBloke | 4:95f696f59d94 | 108 | */ |
SomeRandomBloke | 4:95f696f59d94 | 109 | bool Sonar::getRange( int *outRawDistance, int *outDistance, float temperature ) |
SomeRandomBloke | 4:95f696f59d94 | 110 | { |
SomeRandomBloke | 4:95f696f59d94 | 111 | int pulse; // number of pulses from sensor |
SomeRandomBloke | 4:95f696f59d94 | 112 | int8_t i=0; |
SomeRandomBloke | 4:95f696f59d94 | 113 | // These values are for calculating a mathematical median for a number of samples as |
SomeRandomBloke | 4:95f696f59d94 | 114 | // suggested by Maxbotix instead of a mathematical average |
SomeRandomBloke | 4:95f696f59d94 | 115 | int8_t arraysize = 9; // quantity of values to find the median (sample size). Needs to be an odd number |
SomeRandomBloke | 4:95f696f59d94 | 116 | //declare an array to store the samples. not necessary to zero the array values here, it just makes the code clearer |
SomeRandomBloke | 4:95f696f59d94 | 117 | int pulsevalue[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
SomeRandomBloke | 4:95f696f59d94 | 118 | // float temperature = 0.0; |
SomeRandomBloke | 4:95f696f59d94 | 119 | |
SomeRandomBloke | 4:95f696f59d94 | 120 | // Start Timer for Maxbotix ultrasonic sensor |
SomeRandomBloke | 4:95f696f59d94 | 121 | time.reset(); |
SomeRandomBloke | 4:95f696f59d94 | 122 | time.start(); |
SomeRandomBloke | 4:95f696f59d94 | 123 | |
SomeRandomBloke | 4:95f696f59d94 | 124 | // SENSOR_ON; |
SomeRandomBloke | 4:95f696f59d94 | 125 | // wait_ms(100); // Startup delay, datasheet says 175mS, but we have a temperature sensor to read |
SomeRandomBloke | 4:95f696f59d94 | 126 | // Read temperature for compensation calculation |
SomeRandomBloke | 4:95f696f59d94 | 127 | // ds18b20.startConversion(); |
SomeRandomBloke | 4:95f696f59d94 | 128 | // ds18b20.convertTemperature(true, DS1820::this_device); //Start temperature conversion, wait until ready, 9 bits takes 93.75mS |
SomeRandomBloke | 4:95f696f59d94 | 129 | // temperature = ds18b20.temperature(); |
SomeRandomBloke | 4:95f696f59d94 | 130 | |
SomeRandomBloke | 4:95f696f59d94 | 131 | // Perform a quick dummy read |
SomeRandomBloke | 4:95f696f59d94 | 132 | while( i++ < arraysize ) { |
SomeRandomBloke | 4:95f696f59d94 | 133 | pulse = read(); |
SomeRandomBloke | 4:95f696f59d94 | 134 | wait_ms(50); |
SomeRandomBloke | 4:95f696f59d94 | 135 | } |
SomeRandomBloke | 4:95f696f59d94 | 136 | |
SomeRandomBloke | 4:95f696f59d94 | 137 | // Read temrperature |
SomeRandomBloke | 4:95f696f59d94 | 138 | // temperature = ds18b20.read(); |
SomeRandomBloke | 4:95f696f59d94 | 139 | // *outTemperature = temperature; |
SomeRandomBloke | 4:95f696f59d94 | 140 | |
SomeRandomBloke | 4:95f696f59d94 | 141 | i = 0; |
SomeRandomBloke | 4:95f696f59d94 | 142 | //pc.printf("Reading sensor\n\r"); |
SomeRandomBloke | 4:95f696f59d94 | 143 | // pc.printf("Timer: %d \n\r",maxBotixTimer.read_us()); |
SomeRandomBloke | 4:95f696f59d94 | 144 | |
SomeRandomBloke | 4:95f696f59d94 | 145 | while( i < arraysize ) { |
SomeRandomBloke | 4:95f696f59d94 | 146 | pulse = read(); |
SomeRandomBloke | 4:95f696f59d94 | 147 | if( pulse == 0 ) { |
SomeRandomBloke | 4:95f696f59d94 | 148 | // SENSOR_OFF; |
SomeRandomBloke | 4:95f696f59d94 | 149 | time.stop(); |
SomeRandomBloke | 4:95f696f59d94 | 150 | return 0; |
SomeRandomBloke | 4:95f696f59d94 | 151 | } |
SomeRandomBloke | 4:95f696f59d94 | 152 | if( pulse >= 1500 && pulse < 36000 ) |
SomeRandomBloke | 4:95f696f59d94 | 153 | pulsevalue[i++] = pulse; // ensure no values out of range |
SomeRandomBloke | 4:95f696f59d94 | 154 | wait_ms(35); // wait between samples |
SomeRandomBloke | 4:95f696f59d94 | 155 | } |
SomeRandomBloke | 4:95f696f59d94 | 156 | |
SomeRandomBloke | 4:95f696f59d94 | 157 | // Turn off sensor as no longer needed |
SomeRandomBloke | 4:95f696f59d94 | 158 | // SENSOR_OFF; |
SomeRandomBloke | 4:95f696f59d94 | 159 | time.stop(); |
SomeRandomBloke | 4:95f696f59d94 | 160 | |
SomeRandomBloke | 4:95f696f59d94 | 161 | isort(pulsevalue,arraysize); // sort samples |
SomeRandomBloke | 4:95f696f59d94 | 162 | int rawtof = mode(pulsevalue,arraysize); // get median |
SomeRandomBloke | 4:95f696f59d94 | 163 | *outRawDistance = rawtof/58; // time of flight / 58uS for both there and back |
SomeRandomBloke | 4:95f696f59d94 | 164 | *outDistance = (rawtof * (331.3f + 0.606f * temperature)) / 20000; |
SomeRandomBloke | 4:95f696f59d94 | 165 | |
SomeRandomBloke | 4:95f696f59d94 | 166 | // Add check for validity of reading, between 27 and 600cm |
SomeRandomBloke | 4:95f696f59d94 | 167 | return *outRawDistance > 26 && *outRawDistance < 601; |
SomeRandomBloke | 4:95f696f59d94 | 168 | } |