В данной статье будет рассмотрен один из способов взаимодействия сервопривода и внешних сенсоров на примере трёх микрофонов.

Пример заключается в том, чтобы определять положение точечного источника звука по разнице времени прихода звука на микрофоны.

Умение может быть очень полезным для создания робота, которому надо ориентироваться в пространстве: он сможет слышать, что происходит вокруг.

Концептуальная схема примера следующая:

С помощью трёх микрофонов установка должна определять направление, с которого пришёл звук, а затем выставлять стрелку таким образом, чтобы она указывала в этом направлении.

Сервопривод обладает углом поворота 180°, этим вызван такой вид конструкции.

Итак, приступим.

Для нашего примера нам могут понадобиться следующие делали:

• Arduino Uno
• Troyka Shield
• Сервопривод FS5106B
• Три микрофона
• Пластина для прототипирования

Для начала попробуем считать звук с микрофона. Соберём следующую схему:

Микрофоны подключим к аналоговым входам A3, A4 и A5.

Напишем простой код, который будет «слушать» три микрофона и, если на микрофоны придёт громкий звук, даст нам знать об этом. Он будет выглядить примерно так:

microphone_read.ino

#define MIC_PIN_0 A3
#define MIC_PIN_1 A4
#define MIC_PIN_2 A5
 
//устанавливаем нижний порог громкости для звукового сигнала
#define VOL_THRESHOLD 70 
 
void setup()
{
  pinMode(MIC_PIN_0, INPUT);
  pinMode(MIC_PIN_1, INPUT);
  pinMode(MIC_PIN_2, INPUT);
  Serial.begin(9600);
}
 
void loop() 
{
  int vol[] = {analogRead(MIC_PIN_0), analogRead(MIC_PIN_1), analogRead(MIC_PIN_2)};
  for (size_t i = 0; i < 3; ++i) {
    //Если на микрофон пришёл сигнал сильнее порогового, просто выведем об этом информацию
    if (vol[i] > VOL_THRESHOLD) {
      Serial.print(i);
      Serial.print(": ");
      Serial.println(vol[i]);
    } 
  }
}

Ограничение на громкость необходимо, для того чтобы отсечь ненужные внешние шумы. Напомним, что сигнал с аналогового входа занимает 10 бит, то есть принимает значения от 0 до 1023.

Очень важным вопросом является то, насколько быстро работает функция analogRead. Она может работать достаточно медленно, и звук, пока идёт считывание с одного микрофона, пройдёт мимо установки. Это неприемлемо для нас, так как, для того чтобы можно было определять положение источника звука по разнице во времени прихода звука, необходимо, чтобы за время одной итерации звук проходил расстояние, значительно меньшее размера конструкции. Для эксперимента напишем следующий код:

analog_read_experiment.ino

#define MIC_PIN A3
 
#define VOL_THRESHOLD 70
#define ITERATIONS 10000
 
void setup()
{
  pinMode(MIC_PIN_0, INPUT);
  pinMode(MIC_PIN_1, INPUT);
  pinMode(MIC_PIN_2, INPUT);
  Serial.begin(9600);
}
 
void loop() 
{
  int startTime = micros();
 
  //Мы не можем просто замерить время работы analogRead. В общем случае это случайная величина, 
  //но среднее по выборке стремится к её мат.ожиданию с ростом числа итераций
  for (int i = 0; i < ITERATIONS; ++i) {
    int vol = analogRead(MIC_PIN);
  }
  int finishTime = micros();
  Serial.println(finishTime - startTime);
}

Ага, если поделить общее время работы на количество вызовов функции, то получим среднее время её работы: 112 мкс. За это время звук проходит <latex>$ 340\cdot 112\cdot 10^{-6}\approx4 $</latex> см. Не очень здорово. Пока считаем значение на одном микрофоне, звук придёт на второй, конструкция становится зависимой от того, в каком порядке мы считываем значения с микрофонов. Да и погрешность порядка размера конструкции нас совсем не устраивает, пусть даже мы знаем, на какой микрофон звук пришёл первым, для определения направления у нас совершенно нет достоверных данных.

Нас спасает то, что существует зависимость качества преобразования аналогового сигнала в цифровой от частоты АЦП, которая и определяет скорость работы analogRead. Оптимальное качество достигается между 50 кГц и 200 кГц. Однако можно повысить скорость её работы за счёт снижения качества преобразования сигнала.

[ Поставить картинку нормального качества]

Если покопаться в документации микроконтроллера Arduino (стр.266, в англоязычной литературе АЦП называется ADC — Analog-Digital Converter), то можно найти, как это сделать: для этого надо установить биты ADPS2:ADPS0 регистра контроля и состояния АЦП ADCSRA. Эти биты отвечают именно за тактовую частоту АЦП.

В следующей таблице приведены значения этих бит и соответствующие им значения делителя частоты.

Теперь можно поэкспериментировать с различными значениями делителя частоты.

division_factor.ino

//Подключим микрофон к аналоговому входу A5
#define MIC_PIN A5
 
#define ITERATIONS 10000
 
//clearBit делает бит bit переменной sfr нулём
#ifndef clearBit
#define clearBit(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
 
//setBit делает бит bit переменной sfr единицей
#ifndef setBit
#define setBit(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
 
void setup()
{
  pinMode(MIC_PIN, INPUT);
  setBit(ADCSRA,ADPS2) ;
  clearBit(ADCSRA,ADPS1) ;
  clearBit(ADCSRA,ADPS0) ;
  Serial.begin(9600);
}
 
void loop() 
{
  int startTime = micros();
 
  for (int i = 0; i < ITERATIONS; ++i) {
    int vol = analogRead(MIC_PIN);
  }
  int finishTime = micros();
  Serial.println(finishTime - startTime);
}

Будем использовать делитель частоты, равный 16. Тогда за время считывания значения с трёх микрофонов звук пройдёт <latex>$ 340\cdot 3\cdot 16.1\cdot 10^{-6}\approx1.6 $</latex> см.

С установкой разобрались, теперь надо разобраться с логикой примера. Мы не ждём высокой точности от конструкции, поэтому не будем пытаться по трём точкам точно установить источник звука. Мы будем определять сектор, а положение внутри сектора будем выставлять… впрочим, давайте разбираться, как мы будем выставлять это положение.

Пусть <latex>$ t_0 $</latex>, <latex>$ t_1 $</latex> и <latex>$ t_2 $</latex> — моменты времени, когда звук пришёл на нулевой, первый и второй микрофон соответственно. Возможно 6 вариантов взаимного положения этих моментов:

• <latex>$t_0 < t_2 < t_1 $</latex> — сектор 0
• <latex>$t_0 < t_1 < t_2 $</latex> — сектор 1
• <latex>$t_1 < t_0 < t_2 $</latex> — сектор 2
• <latex>$t_1 < t_2 < t_0 $</latex> — сектор 3
• <latex>$t_2 < t_0 < t_1 $</latex> — сектор 4
• <latex>$t_2 < t_1 < t_0 $</latex> — сектор 5

Внутри сектора будем линейно аппроксимировать угол:

• сектор 1: <latex>$\mathtt{Angle}=45^o - \frac{t_1 - t_0}{t_2 - t_0}\cdot 45^o $</latex>
• сектор 2: <latex>$\mathtt{Angle}=45^o + \frac{t_0 - t_1}{t_2 - t_1}\cdot 45^o $</latex>
• сектор 3: <latex>$\mathtt{Angle}=135^o - \frac{t_2 - t_1}{t_0 - t_1}\cdot 45^o $</latex>
• сектор 4: <latex>$\mathtt{Angle}=135^o + \frac{t_1 - t_2}{t_0 - t_2}\cdot 45^o $</latex>

Почему это приблизительно верно, догадаться нетрудно. Предлагаю поразмышлять об этом читателю самостоятельно. Опять-таки, читатель может придумать более удачную аппроксимацию.

Осталось установить сервопривод. Получим в итоге вот такую установку:

Обратите внимание, что сервоприводы могут быть настроены по умолчанию по-разному. Например, может так получиться, что при исполнении команды servo.write(0) привод упрётся в правый край, однако будет считать, что нуля градусов ещё не достиг. И поэтому продолжит упираться, испуская при этом электромагнитный шум, достаточный, чтобы микрофоны «сошли с ума» и начали показывать совершенно произвольные значения. Поэтому рекомендуется вместо простой функции servo.attach(pin) использовать функцию, устанавливающую ширину пульса, соответствующую нулю градусов, и ширину, соответствующую 180 градусов, servo.attach(pin, min, max). По умолчанию min = 544, а max = 2400.

servo_plus_microphones.ino

#define MIC_PIN_0 A3
#define MIC_PIN_1 A4
#define MIC_PIN_2 A5
#define servoPin 9
#ifndef clearBit
#define clearBit(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef setBit
#define setBit(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
#define T 1000000
#define WAIT_LIMIT 200
#define VOL_THRESHOLD 80
#include <Servo.h>
 
//Заводим переменную для сервопривода
Servo myservo;
 
void setup()
{
  //Присоединяет переменную сервопривода к девятому пину, а также калибрует ширину пульса
  myservo.attach(servoPin, 600, 2400);
  pinMode(MIC_PIN_0, INPUT);
  pinMode(MIC_PIN_1, INPUT);
  pinMode(MIC_PIN_2, INPUT);
  setBit(ADCSRA,ADPS2) ;
  clearBit(ADCSRA,ADPS1) ;
  clearBit(ADCSRA,ADPS0) ;
  Serial.begin(9600);
}
 
int vol[] = {0,0,0};
//Сюда будем писать на каком шаге звук пришёл на каждый из микрофонов
int tick[] = {0,0,0};
//Микрофоны должны ловить один и тот же звук, а значит, разница в приходе звука на микрофоны 
//не должна быть очень большой
int wait = 0;
void loop() 
{
  for (unsigned long i = 0; i < T; ++i) {
    vol = {analogRead(MIC_PIN_0), analogRead(MIC_PIN_1), analogRead(MIC_PIN_2)}; 
    for (int j = 0; j < 3; ++j) {
      //Если хоть на один микрофон уже пришёл звук, мы должны следить, чтобы он пришёл 
      //и на остальные не позднее определённого WAIT_LIMIT момента
      if (tick[0] || tick[1] || tick[2]) {
        ++wait;
        if (wait > WAIT_LIMIT) {
          tick[0] = tick[1] = tick[2] = 0;
          wait = 0;
        }
      }
      if (vol[j] < VOL_THRESHOLD) {
        continue;
      }
      Serial.print(j);
      Serial.print("_");
      Serial.println(vol[j]);
      if (!tick[j]) tick[j] = i;
    }
 
    //Если сигнал пришёл на все три микрофона, определим положение сервопривода
    if (tick[0] && tick[1] && tick[2]) {
      //Весь этот отладочный вывод необязателен, но очень полезен
      Serial.print(0);
      Serial.print(": ");
      Serial.println(tick[0]);
      Serial.print(1);
      Serial.print(": ");
      Serial.println(tick[1]);
      Serial.print(2);
      Serial.print(": ");
      Serial.println(tick[2]);
      Serial.println("######################");
 
      //Реализуем алгоритм определения положения источника звука, как было описано выше
      int delta01 = tick[0]-tick[1];
      int delta21 = tick[2]-tick[1];
      tick[0] = tick[1] = tick[2] = 0;
      if (delta01 <= delta21 && delta21 <= 0) {
        myservo.write(0);
        Serial.println("0");
        Serial.println("++++++++++++++++++++++");
        delay(2000);
        continue;
      }
      if (delta21 <= delta01 && delta01 <= 0) {
        myservo.write(180);
        Serial.println(180);
        Serial.println("++++++++++++++++++++++");
        delay(2000);
        continue;
      }
      if (delta01 >= delta21 && delta21 >= 0) {
        float q = (float)(delta21) / delta01;
        myservo.write((int)(135 - 45 * q));
        Serial.println(135 - 45 * q);
        Serial.println("++++++++++++++++++++++");
        delay(2000);
        continue;
      }
      if (delta21 >= delta01 && delta01 >= 0) {
        float q = (float) (delta01) / delta21;
        myservo.write((int)(45 + 45 * q));
        Serial.println(45 + 45 * q);
        Serial.println("++++++++++++++++++++++");
        delay(2000);
        continue;
      }
      if (delta01 >= 0 && delta21 <= 0) {
        float q = (float) (-delta21) / (delta01 - delta21);
        myservo.write((int)(135 + 45 * q));
        Serial.println(135 + 45 * q);
        Serial.println("++++++++++++++++++++++");
        delay(2000);
        continue;
      }
      if (delta21 >= 0 && delta01 <= 0) {
        float q = (float) (-delta01) / (delta21 - delta01);
        myservo.write((int)(45 * (1 - q)));
        Serial.println(45 * (1 - q));
        Serial.println("++++++++++++++++++++++");
        delay(2000);
        continue;
      }
    }
  }
  delay(1000);
}