Слушающий эхо
Повесим на Робота Машинку простой эхолокатор, чтобы дать ему возможность ориентироваться в пространстве:
Робот Машинка - эхолокатор from 1i7 on Vimeo.
Простая демонстрационная прошивка работы с эхолокатором (датчиком расстояния, или сонаром): Робот Машинка едет вперёд до тех пор, пока не увидит препятствие в 10ти сантиметрах перед собой. В этом случае он останавливается, отъезжает назад, поворачивает влево или вправо и снова едет вперёд до тех пор, пока не встретит препятствие.
Сонар HC-SR04
Возьмем одну из самых недорогих и популярных модей эхолокаторов для Arduino - ультразвуковой сонар HC-SR04. Он будет рабоать с платами Arduino и ChipKIT и позволяет определять расстояния от 2х до 500т сантиметров.
Он продаётся в любом магазине популярной микроэлектроники или на радиорынке: parkflyer.ru (132 руб), ardunn.ru (190 руб), robocraft.ru (250 руб) и так далее.
Хорошая подробная статья о том, как устроен сонар HC-SR04 и как с ним работать на robocraft.ru: "Ультразвуковой датчик измерения расстояния HC-SR04", рекомендую обратиться к ней.
Приведенные в ней примеры кода будут работать и на наших ChipKIT'ах. Ниже основные моменты и несколько комментариев.
У датчика 4 ножки:
Vcc - питание 5 Вольт
GND - земля
Trig - сигнал для запуска измерения
Echo - сигнал о получении отраженного эхо
Ножки Vcc и GND подключаем к портам питания платы. Для ножки Trig назначаем порт в режиме вывода (OUTPUT), для ножки Echo назначаем порт в режиме ввода (INPUT).
void setup() {
...
// пины сонара
pinMode(SONAR_TRIG, OUTPUT);
pinMode(SONAR_ECHO, INPUT);
...
}
Для того, чтобы измерить расстояние, нам нужно подать прямоугольный импус LOW-HIGH-LOW на ножку Trig шириной 10 микросекунд, подождать 2 микросекунды, пока сенсор подготовит и осуществит отправку исходящего ультразвукового сигнала, и дождаться получения ответного импульса HIGH на ножке Echo. Зная время между отправкой сигнала (фронт HIGH-LOW на ножке Trig + 2 микросекунды) и получением отраженного ответа (HIGH на ножке Echo), мы можем вычислить расстояние до объекта, от которого отразилась ультразвуковая волна.
/**
* Измерить дистанцию, возвращает результат в сантиметрах.
*/
int measureDistanceCm() {
// Отправить на ножку TRIG прямоугольный сигнал
// шириной 10 микросекунд
digitalWrite(SONAR_TRIG, LOW);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, HIGH);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, LOW);
// время на подготовку к отправке импульса
delayMicroseconds(2);
// Получить ответ HIGH на ножке ECHO и замерить время
// в микросекундах
long duration = pulseIn(SONAR_ECHO, HIGH);
// Вычислить расстояние до препятствия в сантиметрах:
// duration - время, мкс
// 29 ~ 30 ~ 1/0.033; 0.033 см/мкс - скорость звука в воздухе
// 2 - делим расстояние туда+обратно пополам
return (duration / 29 / 2);
}
Замечание 1: в статье на robocraft.ru написано, что ширина импульса HIGH для ножки Trig должна быть равна 10 мкс. Но в некоторых других источниках можно найти примеры с немного другой шириной импульса (например 5 мкс для LOW + 5 мкс для HIGH, в сумме всё равно 10 мкс) и в этом случае датчик работает тоже. Очевидно, что отправка сигнала происходит на фронте HIGH-LOW, а ширина импульса может влиять (существенно или не существенно) на некоторые нюансы работы датчика. Насколько принципиально или не принципиально выдерживать ширину импуса HIGH на Trig ровно 10 мкс предлагаю выяснить самостоятельно.
Время до появления сигнала HIGH на ножке Echo замеряется при помощи стандартного вызова Arduino pulseIn: он блокирует выполнение программы до появления нужного значения на указанной ножке, возвращает значение в микросекундах.
Далее, зная время до получения эхо, вычисляем расстояние по формуле:
расстояние до объекта = duration / 29 / 2
здесь:
duration - время от отправки импуса до получения отраженного эхо в микросекундах.
2 - тоже понятно: так как сигнал ультразвука сначала проходит путь от сонара до препятствия, а потом обратно, чтобы получить расстояние до препятсвия, нужно полный путь звуковой волны поделить пополам.
Откуда взялась константа 29 и почему на неё нужно делить? Скорость звука в воздухе = 331 м/с = 0,0331 см/мкс.
Было бы логично использовать формулу:
расстояние до объекта = скорость звука в воздухе * duration / 2 = 0,0331 см/мкс * duration мкс / 2
Однако, как видим, в нашем случае (при размерности см/мкс) скорость звука представлена числом с плавающей точкой, что не очень удобно, особенно на контроллере, где по возможности лучше обходиться целочисленной арифметикой.
Возьмём обратную величину: 1/0,0331 = 30,2 мкс/см ~ 30 мкс/см - получилось хорошее целое число, теперь это значение можно подставить в первую формулу. Можно считать, что 30,2 примерно равно 29, поэтому всё вроде как сходится.
Замечание 2: Однако, мне пока не понятно, почему во всех примерах, которые я видел, используют именно 29, а не 30. Возможно для этого есть какие-то причины, а может быть и нет. Имеет смысл проверить оба варианта, замерив результаты вычислений и сравнив их с реальным расстоянием до объекта.
Вся прошивка
Robot Car/прошивки/sonar_basic/sonar_basic.ino
// Робот Машинка с сонаром: едет вперёд до тех пор, пока не встретит
// препятствие (10 см). При встрече препятствия отъезжает назад,
// поворачивает в сторону и едет вперед до следующего препятствия.
// Ножки для моторов
#define MOTOR_LEFT_1 8
#define MOTOR_LEFT_2 9
#define MOTOR_LEFT_EN 11
#define MOTOR_RIGHT_1 4
#define MOTOR_RIGHT_2 3
#define MOTOR_RIGHT_EN 6
/**
* Сонар.
*/
#define SONAR_TRIG 31
#define SONAR_ECHO 30
void mleft_forward() {
// задать направление
digitalWrite(MOTOR_LEFT_1, HIGH);
digitalWrite(MOTOR_LEFT_2, LOW);
// включить моторы
digitalWrite(MOTOR_LEFT_EN, HIGH);
}
void mleft_backward() {
// задать направление
digitalWrite(MOTOR_LEFT_1, LOW);
digitalWrite(MOTOR_LEFT_2, HIGH);
// включить моторы
digitalWrite(MOTOR_LEFT_EN, HIGH);
}
void mleft_stop() {
// выключить мотор
digitalWrite(MOTOR_LEFT_EN, LOW);
}
void mright_forward() {
// задать направление
digitalWrite(MOTOR_RIGHT_1, HIGH);
digitalWrite(MOTOR_RIGHT_2, LOW);
// включить моторы
digitalWrite(MOTOR_RIGHT_EN, HIGH);
}
void mright_backward() {
// задать направление
digitalWrite(MOTOR_RIGHT_1, LOW);
digitalWrite(MOTOR_RIGHT_2, HIGH);
// включить моторы
digitalWrite(MOTOR_RIGHT_EN, HIGH);
}
void mright_stop() {
// выключить мотор
digitalWrite(MOTOR_RIGHT_EN, LOW);
}
/**
* Измерить дистанцию, возвращает результат в сантиметрах.
*/
int measureDistanceCm() {
// Отправить на ножку TRIG прямоугольный сигнал
// шириной 10 микросекунд
digitalWrite(SONAR_TRIG, LOW);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, HIGH);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, LOW);
// время на подготовку к отправке импульса
delayMicroseconds(2);
// Получить ответ HIGH на ножке ECHO и замерить время
// в микросекундах
long duration = pulseIn(SONAR_ECHO, HIGH);
// Вычислить расстояние до препятствия в сантиметрах:
// duration - время, мкс
// 29 ~ 30 ~ 1/0.033; 0.033 см/мкс - скорость звука в воздухе
// 2 - делим расстояние туда+обратно пополам
return (duration / 29 / 2);
}
void setup() {
Serial.begin(9600);
Serial.println("Start Robot Car - sonar basic");
// if analog input pin 0 is unconnected, random analog
// noise will cause the call to randomSeed() to generate
// different seed numbers each time the sketch runs.
// randomSeed() will then shuffle the random function.
randomSeed(analogRead(0));
pinMode(MOTOR_LEFT_1, OUTPUT);
pinMode(MOTOR_LEFT_2, OUTPUT);
pinMode(MOTOR_LEFT_EN, OUTPUT);
pinMode(MOTOR_RIGHT_1, OUTPUT);
pinMode(MOTOR_RIGHT_2, OUTPUT);
pinMode(MOTOR_RIGHT_EN, OUTPUT);
// пины сонара
pinMode(SONAR_TRIG, OUTPUT);
pinMode(SONAR_ECHO, INPUT);
// остановить моторы при старте
mleft_stop();
mright_stop();
}
void loop() {
if(measureDistanceCm() < 10) {
Serial.print("Met obstruction, distance=");
Serial.print(measureDistanceCm());
Serial.println("cm");
// встретили препятствие, остановить моторы
mleft_stop();
mright_stop();
// подождём долю секунды просто так
delay(500);
// отъезжаем назад 2 секунды
mleft_backward();
mright_backward();
delay(2000);
// полсекунды поворачиваем (~45 градусов)
if(random(0, 2)) {
// поворачиваем влево
mleft_backward();
mright_forward();
} else {
// поворачиваем вправо
mleft_forward();
mright_backward();
}
delay(500);
//delay(random(300, 2000));
// едем вперёд до линии
mleft_forward();
mright_forward();
}
// рекомендуемая задержка между двумя замерами расстояния
delay(50);
}
исходники прошивки, подсветка синтаксиса
Робот Машинка - эхолокатор from 1i7 on Vimeo.
Простая демонстрационная прошивка работы с эхолокатором (датчиком расстояния, или сонаром): Робот Машинка едет вперёд до тех пор, пока не увидит препятствие в 10ти сантиметрах перед собой. В этом случае он останавливается, отъезжает назад, поворачивает влево или вправо и снова едет вперёд до тех пор, пока не встретит препятствие.
Сонар HC-SR04
Возьмем одну из самых недорогих и популярных модей эхолокаторов для Arduino - ультразвуковой сонар HC-SR04. Он будет рабоать с платами Arduino и ChipKIT и позволяет определять расстояния от 2х до 500т сантиметров.
Он продаётся в любом магазине популярной микроэлектроники или на радиорынке: parkflyer.ru (132 руб), ardunn.ru (190 руб), robocraft.ru (250 руб) и так далее.
Хорошая подробная статья о том, как устроен сонар HC-SR04 и как с ним работать на robocraft.ru: "Ультразвуковой датчик измерения расстояния HC-SR04", рекомендую обратиться к ней.
Приведенные в ней примеры кода будут работать и на наших ChipKIT'ах. Ниже основные моменты и несколько комментариев.
У датчика 4 ножки:
Vcc - питание 5 Вольт
GND - земля
Trig - сигнал для запуска измерения
Echo - сигнал о получении отраженного эхо
Ножки Vcc и GND подключаем к портам питания платы. Для ножки Trig назначаем порт в режиме вывода (OUTPUT), для ножки Echo назначаем порт в режиме ввода (INPUT).
void setup() {
...
// пины сонара
pinMode(SONAR_TRIG, OUTPUT);
pinMode(SONAR_ECHO, INPUT);
...
}
Для того, чтобы измерить расстояние, нам нужно подать прямоугольный импус LOW-HIGH-LOW на ножку Trig шириной 10 микросекунд, подождать 2 микросекунды, пока сенсор подготовит и осуществит отправку исходящего ультразвукового сигнала, и дождаться получения ответного импульса HIGH на ножке Echo. Зная время между отправкой сигнала (фронт HIGH-LOW на ножке Trig + 2 микросекунды) и получением отраженного ответа (HIGH на ножке Echo), мы можем вычислить расстояние до объекта, от которого отразилась ультразвуковая волна.
/**
* Измерить дистанцию, возвращает результат в сантиметрах.
*/
int measureDistanceCm() {
// Отправить на ножку TRIG прямоугольный сигнал
// шириной 10 микросекунд
digitalWrite(SONAR_TRIG, LOW);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, HIGH);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, LOW);
// время на подготовку к отправке импульса
delayMicroseconds(2);
// Получить ответ HIGH на ножке ECHO и замерить время
// в микросекундах
long duration = pulseIn(SONAR_ECHO, HIGH);
// Вычислить расстояние до препятствия в сантиметрах:
// duration - время, мкс
// 29 ~ 30 ~ 1/0.033; 0.033 см/мкс - скорость звука в воздухе
// 2 - делим расстояние туда+обратно пополам
return (duration / 29 / 2);
}
Замечание 1: в статье на robocraft.ru написано, что ширина импульса HIGH для ножки Trig должна быть равна 10 мкс. Но в некоторых других источниках можно найти примеры с немного другой шириной импульса (например 5 мкс для LOW + 5 мкс для HIGH, в сумме всё равно 10 мкс) и в этом случае датчик работает тоже. Очевидно, что отправка сигнала происходит на фронте HIGH-LOW, а ширина импульса может влиять (существенно или не существенно) на некоторые нюансы работы датчика. Насколько принципиально или не принципиально выдерживать ширину импуса HIGH на Trig ровно 10 мкс предлагаю выяснить самостоятельно.
Время до появления сигнала HIGH на ножке Echo замеряется при помощи стандартного вызова Arduino pulseIn: он блокирует выполнение программы до появления нужного значения на указанной ножке, возвращает значение в микросекундах.
Далее, зная время до получения эхо, вычисляем расстояние по формуле:
расстояние до объекта = duration / 29 / 2
здесь:
duration - время от отправки импуса до получения отраженного эхо в микросекундах.
2 - тоже понятно: так как сигнал ультразвука сначала проходит путь от сонара до препятствия, а потом обратно, чтобы получить расстояние до препятсвия, нужно полный путь звуковой волны поделить пополам.
Откуда взялась константа 29 и почему на неё нужно делить? Скорость звука в воздухе = 331 м/с = 0,0331 см/мкс.
Было бы логично использовать формулу:
расстояние до объекта = скорость звука в воздухе * duration / 2 = 0,0331 см/мкс * duration мкс / 2
Однако, как видим, в нашем случае (при размерности см/мкс) скорость звука представлена числом с плавающей точкой, что не очень удобно, особенно на контроллере, где по возможности лучше обходиться целочисленной арифметикой.
Возьмём обратную величину: 1/0,0331 = 30,2 мкс/см ~ 30 мкс/см - получилось хорошее целое число, теперь это значение можно подставить в первую формулу. Можно считать, что 30,2 примерно равно 29, поэтому всё вроде как сходится.
Замечание 2: Однако, мне пока не понятно, почему во всех примерах, которые я видел, используют именно 29, а не 30. Возможно для этого есть какие-то причины, а может быть и нет. Имеет смысл проверить оба варианта, замерив результаты вычислений и сравнив их с реальным расстоянием до объекта.
Вся прошивка
Robot Car/прошивки/sonar_basic/sonar_basic.ino
// Робот Машинка с сонаром: едет вперёд до тех пор, пока не встретит
// препятствие (10 см). При встрече препятствия отъезжает назад,
// поворачивает в сторону и едет вперед до следующего препятствия.
// Ножки для моторов
#define MOTOR_LEFT_1 8
#define MOTOR_LEFT_2 9
#define MOTOR_LEFT_EN 11
#define MOTOR_RIGHT_1 4
#define MOTOR_RIGHT_2 3
#define MOTOR_RIGHT_EN 6
/**
* Сонар.
*/
#define SONAR_TRIG 31
#define SONAR_ECHO 30
void mleft_forward() {
// задать направление
digitalWrite(MOTOR_LEFT_1, HIGH);
digitalWrite(MOTOR_LEFT_2, LOW);
// включить моторы
digitalWrite(MOTOR_LEFT_EN, HIGH);
}
void mleft_backward() {
// задать направление
digitalWrite(MOTOR_LEFT_1, LOW);
digitalWrite(MOTOR_LEFT_2, HIGH);
// включить моторы
digitalWrite(MOTOR_LEFT_EN, HIGH);
}
void mleft_stop() {
// выключить мотор
digitalWrite(MOTOR_LEFT_EN, LOW);
}
void mright_forward() {
// задать направление
digitalWrite(MOTOR_RIGHT_1, HIGH);
digitalWrite(MOTOR_RIGHT_2, LOW);
// включить моторы
digitalWrite(MOTOR_RIGHT_EN, HIGH);
}
void mright_backward() {
// задать направление
digitalWrite(MOTOR_RIGHT_1, LOW);
digitalWrite(MOTOR_RIGHT_2, HIGH);
// включить моторы
digitalWrite(MOTOR_RIGHT_EN, HIGH);
}
void mright_stop() {
// выключить мотор
digitalWrite(MOTOR_RIGHT_EN, LOW);
}
/**
* Измерить дистанцию, возвращает результат в сантиметрах.
*/
int measureDistanceCm() {
// Отправить на ножку TRIG прямоугольный сигнал
// шириной 10 микросекунд
digitalWrite(SONAR_TRIG, LOW);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, HIGH);
delayMicroseconds(5);
digitalWrite(SONAR_TRIG, LOW);
// время на подготовку к отправке импульса
delayMicroseconds(2);
// Получить ответ HIGH на ножке ECHO и замерить время
// в микросекундах
long duration = pulseIn(SONAR_ECHO, HIGH);
// Вычислить расстояние до препятствия в сантиметрах:
// duration - время, мкс
// 29 ~ 30 ~ 1/0.033; 0.033 см/мкс - скорость звука в воздухе
// 2 - делим расстояние туда+обратно пополам
return (duration / 29 / 2);
}
void setup() {
Serial.begin(9600);
Serial.println("Start Robot Car - sonar basic");
// if analog input pin 0 is unconnected, random analog
// noise will cause the call to randomSeed() to generate
// different seed numbers each time the sketch runs.
// randomSeed() will then shuffle the random function.
randomSeed(analogRead(0));
pinMode(MOTOR_LEFT_1, OUTPUT);
pinMode(MOTOR_LEFT_2, OUTPUT);
pinMode(MOTOR_LEFT_EN, OUTPUT);
pinMode(MOTOR_RIGHT_1, OUTPUT);
pinMode(MOTOR_RIGHT_2, OUTPUT);
pinMode(MOTOR_RIGHT_EN, OUTPUT);
// пины сонара
pinMode(SONAR_TRIG, OUTPUT);
pinMode(SONAR_ECHO, INPUT);
// остановить моторы при старте
mleft_stop();
mright_stop();
}
void loop() {
if(measureDistanceCm() < 10) {
Serial.print("Met obstruction, distance=");
Serial.print(measureDistanceCm());
Serial.println("cm");
// встретили препятствие, остановить моторы
mleft_stop();
mright_stop();
// подождём долю секунды просто так
delay(500);
// отъезжаем назад 2 секунды
mleft_backward();
mright_backward();
delay(2000);
// полсекунды поворачиваем (~45 градусов)
if(random(0, 2)) {
// поворачиваем влево
mleft_backward();
mright_forward();
} else {
// поворачиваем вправо
mleft_forward();
mright_backward();
}
delay(500);
//delay(random(300, 2000));
// едем вперёд до линии
mleft_forward();
mright_forward();
}
// рекомендуемая задержка между двумя замерами расстояния
delay(50);
}
исходники прошивки, подсветка синтаксиса