Resonance Frequency of Ultrasonic Modules

/*
  ATmega328P
  US transmitter: connect to p1 and p2
  from US receiver to A0:
  .        |\ |
  o-----+--| >|------+--o  A0
  .     |  |/ |      |
  .   ____           _
  .    /\           | |
  .   /__\          | | R
  -     |            _
  .     |            |
  o-----+------------+---o  GND
  diodes: Schottky
  resistor R: 100 kOhms
  for safety reasons add two green LEDs
  parallel to R
*/

// #define FAST

const long f0 = 40000; // kHz
const long df1 = 3000;
const long f1 = f0 - df1;
const long f2 = f0 + df1;
const word icr_L = F_CPU / f1;
const word icr_H = F_CPU / f2;
const float q = 0.397E12 / df1;
const float inc = F_CPU / q;
const long df = 1000;// tickmarks

#ifdef FAST
int tm = 5; // time to measure
// Prezision:
const byte adcsra = B11000010;
#else
int tm = 100;
const byte adcsra = B11000111;
#endif

#ifdef __AVR_ATmega328P__
const byte p1 = 9;
const byte p2 = 10;
#else
# error "Unknown MCU type"
#endif

void setup() {
  // init:
  Serial.begin(9600);
  Serial.println(__FILE__);
  pinMode(p1, OUTPUT);
  pinMode(p2, OUTPUT);
  TCCR1A = B10110010;
  TCCR1B = B00011001;
  ADCSRA = adcsra;
  measure();
}

void loop() {}

void measure() {
  // some distance:
  for (int i = 0; i < 30; i++)
    Serial.println("0 0");
  yAxis();
  long fc = f1 + df;
  long max_f = f1;
  long max_v = 0;
  // oversample:
  for (float icr = icr_L; icr > icr_H + inc; icr = icr - inc) {
    ICR1 = icr;
    OCR1A = icr / 2;
    OCR1B = icr / 2;
    long f = F_CPU / icr;
    Serial.print("freq=_");
    Serial.print(f);
    Serial.println("_Hz");
    long adc = 0;
    long t = millis() + tm;
    // take a lot of readings:
    while (millis() < t)
      adc = adc + analogRead(A0);
    adc = adc / 500;
    Serial.print(adc);
    Serial.print("\t");
    // tickmarks:
    if (f < fc) Serial.print(0);
    else {
      if (abs(f - f0) < df / 2)
        Serial.print(1000);
      else
        Serial.print(250);
      fc = fc + df;
    }
    Serial.println();
    if (adc > max_v) {
      max_v = adc;
      max_f = f;
    }
  }
  yAxis();
  Serial.println("0 0");
  Serial.print("Tickmarks_every_kHz__resonance_at=_");
  Serial.print(max_f);
  Serial.print("_Hz");
  if (max_v > 950) {
    Serial.print("__Overdriven._Redo_it_please!");
  }
  Serial.println();
}

void yAxis() {
  Serial.println("0 1000");
}