roboforum.ru

// Подключение модуля ультрозвукового сенсора
#include <Ultrasonic.h>


//объявление глобальных переменных
int L_Sensor=0; //Левый сенсор
int R_Sensor=0; //Правый сенсор
float dist_cm=0; // Значение расстояния 
Ultrasonic ultrasonic(4, 2); // Назначение портов УЗ сенсора. Trig, Echo



void setup()// Модуль инициализации
{
pinMode( 15 , INPUT); // Вход правого сенсора
pinMode( 17 , INPUT); // Вход левого сенсора
pinMode(3, OUTPUT); //B-B     Назад  - Левый мотор 
pinMode(5, OUTPUT); //B-1A    Вперед - Левый мотор 
pinMode(6, OUTPUT); //A-B     Назад  - Правый мотор
pinMode(9, OUTPUT); //A-1A    Вперед - Правый мотор
Serial.begin(9600); // Активация передачи данных через терминал
delay(5000); // Задержка старта 5 сек.
}

void go_forward()  //Атакуем движение вперед 
{
    analogWrite(3, 0);    //LEFT MOTOR
    analogWrite(5, 255);  //LEFT MOTOR
    analogWrite(6, 0);    //RIGHT MOTOR
    analogWrite(9, 255);  //RIGHT MOTOR
}

void go_left() // поиск цели или движение налево
{     
    analogWrite(3, 0);   //LEFT MOTOR
    analogWrite(5, 100); //LEFT MOTOR
    analogWrite(6, 100); //RIGHT MOTOR
    analogWrite(9, 0);   //RIGHT MOTOR
}

void go_back () //двидение назад
{ 
    analogWrite(3, 255); //LEFT MOTOR
    analogWrite(5, 0);   //LEFT MOTOR
    analogWrite(6, 255); //RIGHT MOTOR
    analogWrite(9, 0);   //RIGHT MOTOR 
}
void go_right () //поиск цели или движение направо
{
    analogWrite(3, 100);  //LEFT MOTOR
    analogWrite(5, 0);    //LEFT MOTOR
    analogWrite(6, 0);    //RIGHT MOTOR
    analogWrite(9, 100);  //RIGHT MOTOR 
}

void go_stop() //остановка
{
    analogWrite(3, 255); //LEFT MOTOR
    analogWrite(5, 255); //LEFT MOTOR
    analogWrite(6, 255); //RIGHT MOTOR
    analogWrite(9, 255); //RIGHT MOTOR 
}

void check_sensor() // Подпрограмма проверки сенсоров.
{
  R_Sensor=analogRead(15); // считываем показания с правого сенсора
  delay(10); // Ожидание преобразования АЦП
  L_Sensor=analogRead(17);// считываем показания с левого сенсора
  delay(10); // Ожидание преобразования АЦП
  dist_cm = ultrasonic.Ranging(CM);
  delay(10);// Ожидание преобразования 
}

void display(const char* text) {
  Serial.print(text);
  Serial.print("  R_Sensor:"); Serial.print(R_Sensor);
  Serial.print("  L_Sensor:"); Serial.print(L_Sensor);
  Serial.print("  dist_cm:");  Serial.println(dist_cm);
}

void loop()
{

  check_sensor(); // Проверяем сенсоры
  
  if (R_Sensor<100 and L_Sensor<100)
   { 
     display("WALK"); // <----------------------------------------------
     if (dist_cm>40) //Если дистанция до объекта больше 40 см то
         {
           display("SEARCH"); // <----------------------------------------------
           go_left(); // поиск цели
         }
         else
         {
           display("PUSH"); // <----------------------------------------------
           go_forward(); //если поймана цель атакуем
         }
   }
    else
     {
     display("STOP&TURN"); // <----------------------------------------------
     go_stop();      //Стоп!
     delay(100);     //Джем 100 мс
     go_back();      //Движемся назад
     delay(1000);    //1 секунду.
     go_right();     //Поворачиваем направо 
     delay(300);     //300 мс
     go_forward();   //Движемся вперед
     delay(300);     //300 мс
     }
 
}

void go_forward()  //Атакуем движение вперед 
{
    analogWrite(3, 0);    //LEFT MOTOR
    analogWrite(5, 255);  //LEFT MOTOR
    analogWrite(6, 0);    //RIGHT MOTOR
    analogWrite(9, 255);  //RIGHT MOTOR
}

void go_forward()  //Атакуем движение вперед 
{
    analogWrite(3, 255);  //LEFT MOTOR
    analogWrite(5, 0);    //LEFT MOTOR
    analogWrite(6, 255);  //RIGHT MOTOR
    analogWrite(9, 0);    //RIGHT MOTOR
}

if (R_Sensor>100 and L_Sensor>100)