Подвижный дальномер на сервомоторе часто используется в разных ардуино проектах. У меня возникла идея заставить ардуино следить за объектом двух плоскостях. Если объект будет смещаться - задача не терять его из поля зрения. Проект выполнен на ультразвуковом датчике HC-SR04 и паре сервомашинок.
Для изготовления потребуется:
- Любая ардуино плата
- Ультразвуковой датчик расстояния HC-SR04
- Одна-две сервомашинки SG-90
- Провода и руки

Схема подключения ультразвукового датчика слежения за объектом:

В первом скетче выполняется вращение от 30 до 150 градусов. Угол обзора можно задать для SG90 от 0 до 180 градусов. Это три положения: 30, 90 и 150 градусов. В коде программы задаем значение переменной distance которая определяет на каком расстоянии производить захват объекта в фокус. Как только будет зафиксировано приближение объекта ближе чем distance - сервомотор остановится и загорится светодиод.
Библиотека для ультразвукового датчика HC-RS04 Ultrasonic.h, для сервомотора - включена в состав программы Arduino IDE.
#include //подключаем библиотеку для сервомотора #include "Ultrasonic.h" //подключаем библиотеку для ультразвукового датчика Servo servoneck; // создаем объект серво-шея Ultrasonic ultrasonic(12,11); //trigPin - 12; echoPin - 11; const int ledPin = 13; // пин 13 – светодиод const int distance = 30; // минимальное расстояние до попадения в фокус void setup() { Serial.begin(9600); pinMode(ledPin, OUTPUT); // задаем светодиод - выходной пин servoneck.attach(9); // attaches the servo on pin 9 to the servo object servoneck.write(150); digitalWrite(ledPin, 0); // выключаем светодиод } void loop() { if (ultrasonic.Ranging(CM)>distance){ digitalWrite(ledPin, 0); // выключаем светодиод servoneck.write(90); delay(500); if (ultrasonic.Ranging(CM)>distance){ digitalWrite(ledPin, 0); // выключаем светодиод servoneck.write(150); delay(500); } if (ultrasonic.Ranging(CM)>distance){ digitalWrite(ledPin, 0); // выключаем светодиод servoneck.write(90); delay(500); } if (ultrasonic.Ranging(CM)>distance){ digitalWrite(ledPin, 0); // выключаем светодиод servoneck.write(30); delay(500); } } else digitalWrite(ledPin, 1); // включаем светодиод delay(500); }
Для эксперимента программа была модернизирована для добавления второго сервомотора. Он будет следить за объектом по вертикали. Что бы сервомашинки не молотили в режиме поиска постоянно можно было бы еще подключить датчик движения для активации поиска, а в остальное время переходить в режим ожидания.
Вот скетч, где добавлен второй сервомотор для поиска границ объекта по вертикали:
#include //подключаем библиотеку для сервомотора #define trigPin 12 // подключаем Trig к пин 12. отправка сигнала #define echoPin 11 // подключаем Echo к пин 11. прием ответа Servo servoneck; // создаем объект серво-шея Servo servohead; // создаем объект серво-голова int s1, s2, s3; //создаем переменные для записи расстояний по трем направлениям //----------------------------------------------------------------------------------- //будем замер делать дважды и высчитывать среднюю арифметическую. для исключения ошибок. //результат измерений будет записываться последовательно в переменные s_1 и s_2 int get_distance(){ long duration; int s_1, s_2, average = -1; // запись в s_1 digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); s_1 = (int) duration / 58.2; // запись в s_2 digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); s_2 = (int) duration / 58.2; // подсчитываем среднее арифметическое s_1 и s_2 average = (int) (s_1 + s_2) / 2; //Serial.print("distance: "); //Serial.println(average); return average; } //--------------------------------------------------------------- void setup() { Serial.begin(9600); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); servoneck.attach(9); // подключаем servoneck к pin 9 servohead.attach(10); // подключаем servohead к pin 10 servoneck.write(30); // устанавливаем серво-шея в первоначальное положение servohead.write(30); // устанавливаем серво-голова в первоначальное положение } void loop() { s1 = get_distance(); //делаем замер s1 в направлении 30 град. if (s1 < 30) // если меньше заданного в см., то качаем головой { servohead.write(20); delay(100); servohead.write(30); delay(100); } else // иначе, поворачиваем шею в положение 90 градусов(вперед) { servoneck.write(90); delay(500); } s2 = get_distance(); //делаем замер s2 в направлении 90 град. if (s2 <30) // если меньше заданного в см., то качаем головой { servohead.write(20); delay(100); servohead.write(30); delay(100); } else // иначе, поворачиваем шею в положение 150 градусов(налево) { servoneck.write(150); delay(500); } s3 = get_distance(); //делаем замер s3 в направлении 150 град. if (s3 <30) // если меньше заданного, то качаем головой { servohead.write(20); delay(100); servohead.write(30); delay(100); } else // иначе, поворачиваем шею в положение 90 градусов(прямо) { servoneck.write(90); delay(500); } s2 = get_distance(); //делаем замер s2 в направлении 90 град. if (s2 <30) // если меньше заданного, то качаем головой { servohead.write(20); delay(100); servohead.write(30); delay(100); } else // иначе, поворачиваем шею в положение 30 градусов(налево) { servoneck.write(30); delay(500); } }