sonar.cpp

00001 #include "sonar.h"
00002 extern Serial pc;
00003 
00004 Sonar::Sonar(PinName input, Timer& t) :
00005     interrupt(input),
00006     time(t),
00007     pulseStartTime(0),
00008     range(0) {
00009     interrupt.rise(callback(this,&Sonar::pulseStart));
00010     interrupt.fall(callback(this,&Sonar::pulseStop));
00011 }
00012 
00013 int Sonar::read() {
00014     return range;
00015 }
00016 
00017 int Sonar::readCm() {
00018     return range  / 58; // 58uS per CM
00019 }
00020 
00021 Sonar::operator int() {
00022     return read();
00023 }
00024 
00025 void Sonar::pulseStart() {
00026     pulseStartTime = time.read_us();
00027 }
00028 
00029 void Sonar::pulseStop() {
00030     int endTime = time.read_us();
00031 
00032     if (endTime < pulseStartTime) return; // Escape if there's been a roll over
00033     range = (endTime - pulseStartTime); //   / 58; // 58uS per CM
00034 }
00035 
00036 
00037 // Functions used in processing readings
00038 
00039 /* isort - Simple sort function for the set of readings
00040  * Sorting function (Author: Bill Gentles, Nov. 12, 2010)
00041  * The sorted array is returned in the original array
00042  * @param a - Array of unsigned 16 bit values to sort
00043  * @param n - Number of values in array
00044  */
00045 void isort(int *a, int n)
00046 {
00047     for (int i = 1; i < n; ++i)  {
00048         int j = a[i];
00049         int k;
00050         for (k = i - 1; (k >= 0) && (j < a[k]); k--) {
00051             a[k + 1] = a[k];
00052         }
00053         a[k + 1] = j;
00054     }
00055 }
00056 
00057 /** mode - Mode function, returning the mode or median.
00058  * @param x - Array of unsigned 16 bit values to sort
00059  * @param n - Number of values in array
00060  * @return mode or median of the passed data.
00061  */
00062 int mode(int *x,int n)
00063 {
00064     int i = 0;
00065     int count = 0;
00066     int maxCount = 0;
00067     int mode = 0;
00068     int bimodal = 0;
00069     int prevCount = 0;
00070     while(i<(n-1)) {
00071         prevCount=count;
00072         count=0;
00073         while( x[i]==x[i+1] ) {
00074             count++;
00075             i++;
00076         }
00077         if( count > prevCount & count > maxCount) {
00078             mode=x[i];
00079             maxCount=count;
00080             bimodal=0;
00081         }
00082         if( count == 0 ) {
00083             i++;
00084         }
00085         if( count == maxCount ) {      //If the dataset has 2 or more modes.
00086             bimodal=1;
00087         }
00088         if( mode==0 || bimodal==1 ) {  // Return the median if there is no mode.
00089             mode=x[(n/2)];
00090         }
00091         return mode;
00092     }
00093     return mode;
00094 }
00095 
00096 // Ultrasonic sensor code
00097 
00098 /** getrange - Get the range in cm. Need to enable sensor first, waiting briefly for it to power up.
00099  * Request temperature sensor to take a reading.
00100  * Disable ultrasonic sensor after use for power saving.
00101  * Returns uncompensated, compensated and temperature values by reference.
00102  * Calling function can still do checks to determine whether to use reading or not.
00103  *
00104  * @param outRawDistance - Raw uncompensated distance
00105  * @param outDistance - Compensated distance if temperature available, otherwise raw distance
00106  * @param outTemperature - Temperature reading
00107  * @return boolean true returned form function for valid reading. false for <23cm or out of range
00108  */
00109 bool Sonar::getRange( int *outRawDistance, int *outDistance, float temperature )
00110 {
00111     int pulse;  // number of pulses from sensor
00112     int8_t i=0;
00113     // These values are for calculating a mathematical median for a number of samples as
00114     // suggested by Maxbotix instead of a mathematical average
00115     int8_t arraysize = 9; // quantity of values to find the median (sample size). Needs to be an odd number
00116     //declare an array to store the samples. not necessary to zero the array values here, it just makes the code clearer
00117     int pulsevalue[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
00118 //    float temperature = 0.0;
00119 
00120     // Start Timer for Maxbotix ultrasonic sensor
00121     time.reset();
00122     time.start();
00123 
00124 //    SENSOR_ON;
00125 //    wait_ms(100);        // Startup delay, datasheet says 175mS, but we have a temperature sensor to read
00126     // Read temperature for compensation calculation
00127 //    ds18b20.startConversion();
00128 //    ds18b20.convertTemperature(true, DS1820::this_device); //Start temperature conversion, wait until ready, 9 bits takes 93.75mS
00129 //    temperature = ds18b20.temperature();
00130 
00131     // Perform a quick dummy read
00132     while( i++ < arraysize ) {
00133         pulse = read();
00134         wait_ms(50);
00135     }
00136 
00137     // Read temrperature
00138 //    temperature = ds18b20.read();
00139 //    *outTemperature = temperature;
00140 
00141     i = 0;
00142 //pc.printf("Reading sensor\n\r");
00143 //    pc.printf("Timer: %d \n\r",maxBotixTimer.read_us());
00144 
00145     while( i < arraysize ) {
00146         pulse = read();
00147         if( pulse == 0 ) {
00148 //            SENSOR_OFF;
00149             time.stop();
00150             return 0;
00151         }
00152         if( pulse >= 1500 && pulse < 36000 )
00153             pulsevalue[i++] = pulse;  // ensure no values out of range
00154         wait_ms(35);                      // wait between samples
00155     }
00156 
00157     // Turn off sensor as no longer needed
00158 //    SENSOR_OFF;
00159     time.stop();
00160 
00161     isort(pulsevalue,arraysize);        // sort samples
00162     int rawtof = mode(pulsevalue,arraysize);  // get median
00163     *outRawDistance = rawtof/58;      // time of flight / 58uS for both there and back
00164     *outDistance = (rawtof * (331.3f + 0.606f * temperature)) / 20000;
00165 
00166     // Add check for validity of reading, between 27 and 600cm
00167     return *outRawDistance > 26 && *outRawDistance < 601;
00168 }