Sampling an XL-MaxSonar-EZ from Teensy 3.0.

sonar.cpp

#include "WProgram.h"

#define PIN 3
#define SAMPLE_RATE 10 /* Hz */
#define SAMPLE_SIZE 9

/* uS / cm - see XL-MaxSonar-EZ Series data sheet for MB1200 scaling factor */
#define SCALE 58

#define SWAP(a,b) do {                                       \
          *(a) ^= *(b);                                      \
          *(b) ^= *(a);                                      \
          *(a) ^= *(b);                                      \
     } while (0)

#define SORT(a,b) do {                                                  \
          if (*(a) > *(b)) {                                            \
               SWAP((a), (b));                                          \
     }                                                                  \
     } while (0)

/*
 * Use a median filter on the assumption that the sensor/subject distance
 * will change while sampling. A mode filter would likely behave better
 * in static situations.
 *
 * WARNING: Assumes SAMPLE_SIZE == 9, above!
 */
long
median_filter(long* p)
{
     SORT(p + 1, p + 2);
     SORT(p + 4, p + 5);
     SORT(p + 7, p + 8);
     SORT(p + 0, p + 1);
     SORT(p + 3, p + 4);
     SORT(p + 6, p + 7);
     SORT(p + 1, p + 2);
     SORT(p + 4, p + 5);
     SORT(p + 7, p + 8);
     SORT(p + 0, p + 3);
     SORT(p + 5, p + 8);
     SORT(p + 4, p + 7);
     SORT(p + 3, p + 6);
     SORT(p + 1, p + 4);
     SORT(p + 2, p + 5);
     SORT(p + 4, p + 7);
     SORT(p + 4, p + 2);
     SORT(p + 6, p + 4);
     SORT(p + 4, p + 2);
     return p[4];
}

int
main(void)
{
     int i;
     long sample;
     long samples[SAMPLE_SIZE];

     pinMode(PIN, INPUT);

     for (;;) {
          for (i = 0; i < SAMPLE_SIZE; i++) {
               sample = pulseIn(PIN, HIGH);
               samples[i] = sample / SCALE;
               delay(1000 / SAMPLE_RATE);
          }

          Serial.print("median: ");
          Serial.print(median_filter(samples));
          Serial.println("cm");
     }
}