Ультразвуковой контроль за движущимся объектом

Подвижный дальномер на сервомоторе часто используется в разных ардуино проектах. У меня возникла идея заставить ардуино следить за объектом двух плоскостях. Если объект будет смещаться - задача не терять его из поля зрения. Проект выполнен на ультразвуковом датчике 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);
    }
  }