WALTER: фотоискающий насекомый робот на Arduino

Компоненты и материалы

Необходимые инструменты и станки

Приложения и онлайн-сервисы

О проекте

[Прошу прощения за мой английский]

Мне очень нравится популярная настройка 2-сервовых ардуино-насекомых на YouTube. Посмотрев на них, я всегда вспоминал, что делали энтузиасты BEAM-робототехники задолго до того, как эта схема стала популярной. Эти фанаты аналоговой робототехники добивались лучшей походки благодаря лучшему углу между двумя моторами (microcore / bicore walker и т.д.).

Однако, на мой взгляд, ни один из упомянутых выше не выглядит более живым, чем VBug1.5 (также известный как Walkman), созданный основателем BEAM-робототехники Марком Тилденом. Он использует 5 моторов, поэтому обладает большей маневренностью.

Создать простого BEAM-робота не сложно, но построить что-то столь сложное, как VBug1.5, может быть пугающим для новичка в электронике, как я. Поэтому, когда я решил сделать что-то вроде жуков Тилдена, мне пришлось остановиться на платформе Arduino — самом простом выборе для неинженеров (или в моём случае, стыдно признаться, инженера-желающего).

В результате получился Walter — четырёхногой Arduino-робот с 5 сервоприводами. Вы можете спросить: если я хотел сделать похожего на живого жука робота, почему не использовал 8 или 12 сервоприводов? Ну, я думал о чём-то максимально простом, чтобы получить максимальную маневренность. Речь идёт о большом количестве клея вместо изготовления рам.

ПОВЕДЕНИЕ

Как и многие другие Arduino-роботы, Walter может избегать препятствий с помощью ультразвуковых датчиков HC-SR04. Чтобы добавить характер жука, Walter также является фотоискателем (photovore) — он притягивается к свету. Фотодиоды используются для обнаружения света. В скетче Arduino генерируются случайные значения, чтобы Walter решал, когда остановиться для отдыха, и случайно менять скорость походки (3 скорости).

Когда я начинал, я хотел сделать тактильные кнопки под каждой ногой Walter, чтобы у него были датчики поверхности. Но батарея (пауэрбанк для смартфона) заставила сервоприводы нести слишком большой вес. Я знаю, что тактильные кнопки весят почти ничего, но иронично, веса робота недостаточно, чтобы нажать перевёрнутые кнопки.

Планировал сделать версию Walter 2 с более крупными сервоприводами и включить эти кнопки как датчики поверхности.

Больше деталей

Код

WALTER.ino Arduino

/* WALTER - 4-НОГИЙ ФОТОИСКАТЕЛЬ
Этот скетч Arduino — моя попытка построить 5-сервовый квадрупёд (4-ногого) робота по имени "WALTER".
Для использования этого скетча вам, возможно, придётся изменить некоторые значения для удобства или подстройки под ваше оборудование.
Найдите (Ctrl+F) эти метки, чтобы легко найти значения, которые могут потребовать изменения:
- **** : Эти метки означают центральное положение сервоприводов, их нужно откалибровать (положение ног робота в покое).
- *** : Эти метки означают назначение пинов Arduino (подключение датчиков и сервоприводов). Обратитесь к этому при сборке робота.
- ** : Эти значения можно при желании изменить по вкусу (ширина шага ног, насколько поворачивать при обнаружении света/препятствия и т.д.). Оставьте как есть, если не знаете, что делаете.
Вы можете использовать этот скетч на свой страх и риск.. он предоставлен "как есть".. и.. э-э.. что за авторские права люди обычно пишут в опубликованном коде?? Суть в том, что я не хочу нести ответственность, если что-то плохое случится при использовании этого кода. Веселитесь!
Yohanes Martedi - 2015 */

#include <Servo.h>

// **** Калибровка центрального угла сервоприводов (в микросекундах, так как используем "xx.writeMicroseconds();"). Начните с 1500.
const int ANGLE_mid_Shaft = 1520;
const int ANGLE_mid_FLeft = 1550;
const int ANGLE_mid_FRight = 1570;
const int ANGLE_mid_BLeft = 1450;
const int ANGLE_mid_BRight = 1450;

const int ANGLE_sweep = 250; // ** Установите это значение (в микросекундах), чтобы определить, насколько широко будут раскачиваться сервоприводы (ширина шага ног). Больше значение — шире угол.
const int ANGLE_res = 10; // ** Установите разрешение движения сервопривода (в микросекундах) минимум в ширину мёртвой зоны сервопривода по умолчанию (наивысшее разрешение) или больше (ниже разрешение). Пример: у сервопривода SG90 мёртвая зона 10 микросекунд.

int sweepSPEED; // переменная для определения скорости раскачивания сервоприводов.
int sweepSPEED_Rand[3] = {4, 6, 8}; // ** Скорость сервопривода (скорость походки) будет случайно меняться в 3 режимах. Установите скорость (в миллисекундах) для каждого режима. Меньше значение — быстрее.

const int ANGLE_turnMAX = ANGLE_sweep * 1.5; // ** Установите это значение, чтобы определить максимальный поворот к свету. Больше значение — больше поворот.
const int ANGLE_turnNARROW = ANGLE_sweep * 0.25; // ** Установите это значение для определения максимального поворота при избегании объектов по бокам в узком пространстве. Больше значение — больше поворот.

const int SONAR_sum = 3; // Количество сонаров.
const int PHOTO_sum = 4; // Количество фотодиодов.

int PIN_trig[SONAR_sum] = {13, 11, 8}; // *** Установите пины Arduino, подключенные к триггерам ультразвуковых датчиков; {передний, левый, правый}.
int PIN_ec[SONAR_sum] = {12, 10, 7}; // *** Установите пины Arduino, подключенные к эхо-выводам ультразвуковых датчиков; {передний, левый, правый}.
int PIN_PHOTO[PHOTO_sum] = {2, 3, 1, 0}; // *** Установите аналоговые входные пины Arduino, подключенные к фотодиодам; {передний левый, передний правый, задний левый, задний правый}.

const int distRotate = 25; // ** Настройте минимальное расстояние (в см) между роботом и препятствием перед избеганием поворотом.
const int distRetreat = 10; // ** Настройте минимальное расстояние (в см) перед избеганием отступлением.
const int distTurn = 20; // ** Настройте минимальное расстояние (в см) перед избеганием поворотом.
const int counter_gait_max = 8; // ** Настройте, сколько шагов робот сделает для избегания препятствия (при повороте или отступлении).

// ** Настройте время отдыха и работы (в миллисекундах).
const int RUN_time = 25000;
const int REST_time = 3000;

// ID сонаров:
int SONAR_id;
const int FRONT = 0;
const int LEFT = 1;
const int RIGHT = 2;

// ID фотодиодов:
const int FRONT_LEFT = 0;
const int FRONT_RIGHT = 1;
const int BACK_LEFT = 2;
const int BACK_RIGHT = 3;

// Переменные для чтения фотодиодов:
int PHOTO_Front_Left;
int PHOTO_Front_Right;
int PHOTO_Back_Left;
int PHOTO_Back_Right;

const int SONAR_TrigSig = 10; // Длительность (в мкс) триггерного сигнала, необходимого датчикам для генерации ультразвука (уже указано в спецификации, не меняйте).
const unsigned long SONAR_MaxEc = 50000; // Максимальная длительность (в мкс) эхо-сигнала от датчиков (уже указано в спецификации, не меняйте).
const float SOUND_speed = 0.034; // Скорость звука в воздухе в см/мкс (уже определена наукой, аватар Аанг нужен, если хотите изменить это значение).

int distance[SONAR_sum]; // Результаты расчёта расстояния.

// Объявление сервоприводов:
Servo SERVO_shaft;
Servo SERVO_front_left;
Servo SERVO_front_right;
Servo SERVO_back_left;
Servo SERVO_back_right;

// Переменные для углов каждого сервопривода:
int ANGLE_shaft = ANGLE_mid_Shaft;
int ANGLE_front_left = ANGLE_mid_FLeft;
int ANGLE_front_right = ANGLE_mid_FRight;
int ANGLE_back_left = ANGLE_mid_BLeft;
int ANGLE_back_right = ANGLE_mid_BRight;

// Манипуляция углами для среднего сервопривода (shaft).
const int ANGLE_max_Shaft = ANGLE_mid_Shaft + ANGLE_sweep;
const int ANGLE_min_Shaft = ANGLE_mid_Shaft - ANGLE_sweep;
int ANGLE_sweep_val;

// Переменные для записи текущих углов каждого сервопривода:
int ANGLE_shaft_record;
int ANGLE_front_left_record;
int ANGLE_front_right_record;
int ANGLE_back_left_record;
int ANGLE_back_right_record;

// Переменные для коррекции углов сервоприводов согласно обнаружению света:
int LIGHT_left;
int LIGHT_right;

// Переменные для коррекции углов сервоприводов согласно обнаружению сонара:
int SONAR_left;
int SONAR_right;

// Разные флаги, счётчики, записи, которые я всегда не уверен, как объяснить. :(
int ANGLE_prev;
int flag_shaft_reverse;
int flag_transition_rotate;
int flag_transition_start = 1;
int flag_rest = 0;
int flag_RUN_time = 0;
int rotate_random;
int counter_gait;

void setup() {
  // Serial.begin(9600); // Serial.. знаете, проверка и отладка..
  SERVO_shaft.attach(2); // *** Настройте пин сигнала горизонтального (shaft) сервопривода на Arduino.
  SERVO_front_left.attach(4); // *** Настройте пин сигнала переднего левого сервопривода.
  SERVO_front_right.attach(3); // *** Настройте пин сигнала переднего правого сервопривода.
  SERVO_back_left.attach(6); // *** Настройте пин сигнала заднего левого сервопривода.
  SERVO_back_right.attach(5); // *** Настройте пин сигнала заднего правого сервопривода.

  // Подготовка сервоприводов к их центральным углам.
  SERVO_shaft.writeMicroseconds(ANGLE_mid_Shaft);
  SERVO_front_left.writeMicroseconds(ANGLE_mid_FLeft);
  SERVO_front_right.writeMicroseconds(ANGLE_mid_FRight);
  SERVO_back_left.writeMicroseconds(ANGLE_mid_BLeft);
  SERVO_back_right.writeMicroseconds(ANGLE_mid_BRight);

  // Настройка пинов для сонаров, и pinMode, и значение.
  for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) {
    pinMode(PIN_trig[SONAR_id], OUTPUT);
    pinMode(PIN_ec[SONAR_id], INPUT);
    digitalWrite(PIN_trig[SONAR_id], LOW);
  }

  randomSeed(analogRead(5)); // Используем аналоговый пин 5 для генерации случайных значений для последующего использования.
  SONAR_READ_ALL(); // Инициируем первое чтение сонара перед любыми действиями,
  SONAR_READ_ALL(); // и на самом деле я забыл, почему нужно вызывать его дважды в первый раз. Но поверьте, это нужно.
  START(); // Эта функция обеспечивает случайное поднятие ног на первом шаге.
  delay(3000); // ** ту-дум.. даа.. ту-дум..
}

void loop() {
  int state = random(0, 2);
  if(state == 0) {
    REST();
  } else {
    int randomSPEED = random(0, 3);
    sweepSPEED = sweepSPEED_Rand[randomSPEED];
    RUN();
    flag_rest = 0;
  }
}

void REST() {
  if(flag_rest == 0) {
    TRANSITION_GAIT();
    flag_rest = 1;
  }
  delay(REST_time);
}

void RUN() {
  unsigned long TIMER_state = millis();
  while((millis() - TIMER_state) <= RUN_time) {
    if(distance[FRONT] > distRotate) {
      flag_RUN_time = 0;
      while(flag_RUN_time == 0) {
        FORWARD();
      }
    }
    while(distance[FRONT] > distRetreat && distance[FRONT] <= distRotate) {
      while(distance[LEFT] > distance[RIGHT]) {
        ROTATE_LEFT_AVOID();
        break;
      }
      while(distance[LEFT] < distance[RIGHT]) {
        ROTATE_RIGHT_AVOID();
        break;
      }
      while(distance[LEFT] == distance[RIGHT]) {
        ROTATE_RANDOM_AVOID();
        break;
      }
    }
    while(distance[FRONT] <= distRetreat) {
      RETREAT_AVOID();
    }
  }
}

/*________________________________________________________________________________________
########################################## ПОХОДКА ########################################*/

/*=================================== ДВИЖЕНИЕ SHAFT ===================================*/
void SHAFT() {
  unsigned long TIMER_servo = millis();
  while((millis() - TIMER_servo) <= sweepSPEED) {
    while(ANGLE_shaft == ANGLE_mid_Shaft) {
      counter_gait++;
      SONAR_READ_ALL();
      LIGHT_COMPARE_EXECUTE();
      SIDE_AVOID();
      flag_RUN_time = 1;
      break;
    }
  }
  if(ANGLE_prev < ANGLE_shaft && ANGLE_shaft < ANGLE_max_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft += ANGLE_res;
  } else if(ANGLE_shaft >= ANGLE_max_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft -= ANGLE_res;
  } else if(ANGLE_prev > ANGLE_shaft && ANGLE_shaft > ANGLE_min_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft -= ANGLE_res;
  } else if(ANGLE_shaft <= ANGLE_min_Shaft) {
    ANGLE_prev = ANGLE_shaft;
    ANGLE_shaft += ANGLE_res;
  }
  SERVO_shaft.writeMicroseconds(ANGLE_shaft);
}

void SHAFT_REVERSE() {
  if(ANGLE_prev < ANGLE_shaft) {
    ANGLE_prev = ANGLE_shaft + 1;
  } else if(ANGLE_prev > ANGLE_shaft) {
    ANGLE_prev = ANGLE_shaft - 1;
  }
}
/*================================ КОНЕЦ ДВИЖЕНИЯ SHAFT ================================*/

/*===================================== ПЕРЕХОД =====================================*/
void TRANSITION_GAIT() {
  ANGLE_front_left_record = ANGLE_front_left;
  ANGLE_front_right_record = ANGLE_front_right;
  ANGLE_back_left_record = ANGLE_back_left;
  ANGLE_back_right_record = ANGLE_back_right;
  ANGLE_shaft_record = ANGLE_shaft;
  int flag = HIGH;
  int counter = 0;
  while(flag == HIGH) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    counter++;
    ANGLE_front_left = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_left_record, ANGLE_mid_FLeft);
    ANGLE_front_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_right_record, ANGLE_mid_FRight);
    ANGLE_back_left = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_left_record, ANGLE_mid_BLeft);
    ANGLE_back_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_right_record, ANGLE_mid_BRight);
    SERVO_shaft.writeMicroseconds(ANGLE_shaft);
    SERVO_front_left.writeMicroseconds(ANGLE_front_left);
    SERVO_front_right.writeMicroseconds(ANGLE_front_right);
    SERVO_back_left.writeMicroseconds(ANGLE_back_left);
    SERVO_back_right.writeMicroseconds(ANGLE_back_right);
    if(counter == ((ANGLE_sweep * 2) / ANGLE_res)) {
      flag = LOW;
      START();
      flag_transition_rotate = 0;
    }
  }
}

void TRANSITION_START() {
  if(ANGLE_shaft == ANGLE_mid_Shaft || (ANGLE_shaft > ANGLE_mid_Shaft && ANGLE_shaft > ANGLE_prev) || (ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_shaft < ANGLE_prev)) {
    ANGLE_sweep_val = 0;
  } else {
    flag_transition_start = 0;
  }
}

void START() {
  ANGLE_prev = random((ANGLE_shaft), (ANGLE_shaft + 2));
  if(ANGLE_prev == ANGLE_shaft) {
    ANGLE_prev -= 1;
  }
  flag_transition_start = 1;
}

int ROTATE_RANDOM() {
  return random(0, 2);
}
/*================================== КОНЕЦ ПЕРЕХОДА ==================================*/

/*======================================== ХОДЬБА ========================================*/
void FORWARD() {
  while(flag_transition_rotate == 2) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 1;
  while(flag_shaft_reverse == 0) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 1;
  while(flag_transition_start == 1) {
    SHAFT();
    TRANSITION_START();
    WALK();
  }
  SHAFT();
  ANGLE_sweep_val = ANGLE_sweep;
  WALK();
}

void RETREAT() {
  while(flag_transition_rotate == 2) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 1;
  while(flag_shaft_reverse == 1) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 0;
  while(flag_transition_start == 1) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    TRANSITION_START();
    WALK();
  }
  SHAFT();
  LIGHT_left = 0;
  LIGHT_right = 0;
  ANGLE_sweep_val = (ANGLE_sweep * -1);
  WALK();
}

void WALK() {
  if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_BRight);
  } else if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right));
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right), ANGLE_mid_BRight);
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight + ANGLE_sweep_val) + LIGHT_right + SONAR_right));
  }
  SERVO_front_left.writeMicroseconds(ANGLE_front_left);
  SERVO_front_right.writeMicroseconds(ANGLE_front_right);
  SERVO_back_left.writeMicroseconds(ANGLE_back_left);
  SERVO_back_right.writeMicroseconds(ANGLE_back_right);
}
/*===================================== КОНЕЦ ХОДЬБЫ =====================================*/

/*======================================= ПОВОРОТ =======================================*/
void ROTATE_LEFT() {
  while(flag_transition_rotate == 1) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 2;
  while(flag_shaft_reverse == 2) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 3;
  while(flag_transition_start == 1) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    TRANSITION_START();
    ROTATE();
  }
  SHAFT();
  LIGHT_left = 0;
  LIGHT_right = 0;
  ANGLE_sweep_val = ANGLE_sweep;
  ROTATE();
}

void ROTATE_RIGHT() {
  while(flag_transition_rotate == 1) {
    TRANSITION_GAIT();
  }
  flag_transition_rotate = 2;
  while(flag_shaft_reverse == 3) {
    SHAFT_REVERSE();
    break;
  }
  flag_shaft_reverse = 2;
  while(flag_transition_start == 1) {
    SHAFT();
    LIGHT_left = 0;
    LIGHT_right = 0;
    TRANSITION_START();
    ROTATE();
  }
  SHAFT();
  LIGHT_left = 0;
  LIGHT_right = 0;
  ANGLE_sweep_val = (ANGLE_sweep * -1);
  ROTATE();
}

void ROTATE() {
  if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FLeft + ANGLE_sweep_val), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FRight - ANGLE_sweep_val), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BLeft - ANGLE_sweep_val), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BRight + ANGLE_sweep_val), ANGLE_mid_BRight);
  } else if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft - ANGLE_sweep_val));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight + ANGLE_sweep_val));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft + ANGLE_sweep_val));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight - ANGLE_sweep_val));
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FLeft - ANGLE_sweep_val), ANGLE_mid_FLeft);
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FRight + ANGLE_sweep_val), ANGLE_mid_FRight);
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BLeft + ANGLE_sweep_val), ANGLE_mid_BLeft);
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BRight - ANGLE_sweep_val), ANGLE_mid_BRight);
  } else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
    ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft + ANGLE_sweep_val));
    ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight - ANGLE_sweep_val));
    ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft - ANGLE_sweep_val));
    ANGLE_back_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight + ANGLE_sweep_val));
  }
  SERVO_front_left.writeMicroseconds(ANGLE_front_left);
  SERVO_front_right.writeMicroseconds(ANGLE_front_right);
  SERVO_back_left.writeMicroseconds(ANGLE_back_left);
  SERVO_back_right.writeMicroseconds(ANGLE_back_right);
}
/*==================================== КОНЕЦ ПОВОРОТА ====================================*/

/*_________________________________________________________________________________________
/*##################################### КОНЕЦ ПОХОДКИ #####################################*/

/*================================= ЧТЕНИЕ УЛЬТРАЗВУКА =================================*/
void SONAR_READ_ALL() {
  for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) {
    distance[SONAR_id] = int (SONAR_READ(SONAR_id));
  }
}

float SONAR_READ(int index) {
  float SONAR_distance;
  digitalWrite(PIN_trig[index], HIGH);
  delayMicroseconds(SONAR_TrigSig);
  digitalWrite(PIN_trig[index], LOW);
  float SONAR_EcInterval = float (pulseIn(PIN_ec[index], HIGH, SONAR_MaxEc));
  while(SONAR_EcInterval > 0.0) {
    SONAR_distance = SONAR_EcInterval * (SOUND_speed / 2.0);
    break;
  }
  while(SONAR_EcInterval == 0.0) {
    SONAR_distance = 501.0;
    break;
  }
  return SONAR_distance;
}
/*============================= КОНЕЦ ЧТЕНИЯ УЛЬТРАЗВУКА =============================*/

/*==================================== ОБНАРУЖЕНИЕ СВЕТА ====================================*/
void LIGHT_COMPARE_EXECUTE() {
  PHOTO_Front_Left = analogRead(PIN_PHOTO[FRONT_LEFT]);
  PHOTO_Front_Right = analogRead(PIN_PHOTO[FRONT_RIGHT]);
  PHOTO_Back_Left = analogRead(PIN_PHOTO[BACK_LEFT]);
  PHOTO_Back_Right = analogRead(PIN_PHOTO[BACK_RIGHT]);

  if((PHOTO_Front_Left + PHOTO_Front_Right) >= (PHOTO_Back_Left + PHOTO_Back_Right)) {
    int LIGHT_Sensitivity = 50;
    if(LIGHT_COMPARE() > LIGHT_Sensitivity) {
      LIGHT_left = LIGHT_COMPARE();
      LIGHT_right = 0;
    } else if(LIGHT_COMPARE() < -LIGHT_Sensitivity) {
      LIGHT_left = 0;
      LIGHT_right = LIGHT_COMPARE();
    } else {
      LIGHT_left = 0;
      LIGHT_right = 0;
    }
  } else {
    if(PHOTO_Back_Left > PHOTO_Back_Right) {
      LIGHT_right = 0;
      LIGHT_left = ANGLE_turnMAX;
    } else if(PHOTO_Back_Left < PHOTO_Back_Right) {
      LIGHT_right = -ANGLE_turnMAX;
      LIGHT_left = 0;
    } else {
      int RANDOM_back_light = random(0, 2);
      if(RANDOM_back_light == 0) {
        LIGHT_right = 0;
        LIGHT_left = ANGLE_turnMAX;
      } else if(RANDOM_back_light == 1) {
        LIGHT_right = -ANGLE_turnMAX;
        LIGHT_left = 0;
      }
    }
  }
}

int LIGHT_COMPARE() {
  int LIGHT_rate;
  if(PHOTO_Front_Left > PHOTO_Front_Right) {
    LIGHT_rate = PHOTO_Front_Left;
  } else if(PHOTO_Front_Right > PHOTO_Front_Left) {
    LIGHT_rate = PHOTO_Front_Right;
  } else {
    // выберите один и закомментируйте другой из этих переменных ниже
    // LIGHT_rate = PHOTO_Front_Left;
    LIGHT_rate = PHOTO_Front_Right;
  }
  int LIGHT_compareRAW = PHOTO_Front_Left - PHOTO_Front_Right;
  LIGHT_compareRAW = map(LIGHT_compareRAW, -LIGHT_rate, LIGHT_rate, -ANGLE_turnMAX, ANGLE_turnMAX);
  return LIGHT_compareRAW;
}
/*================================= КОНЕЦ ОБНАРУЖЕНИЯ СВЕТА =================================*/

/*====================================== ПОВЕДЕНИЕ ======================================*/
void RETREAT_AVOID() {
  counter_gait = 0;
  while(counter_gait <= counter_gait_max) {
    RETREAT();
  }
}

void ROTATE_LEFT_AVOID() {
  counter_gait = 0;
  rotate_random = 2;
  while(counter_gait <= counter_gait_max) {
    ROTATE_LEFT();
  }
}

void ROTATE_RIGHT_AVOID() {
  counter_gait = 0;
  rotate_random = 2;
  while(counter_gait <= counter_gait_max) {
    ROTATE_RIGHT();
  }
}

void ROTATE_RANDOM_AVOID() {
  rotate_random = ROTATE_RANDOM();
  while(rotate_random == 0) {
    ROTATE_LEFT_AVOID();
  }
  while(rotate_random == 1) {
    ROTATE_RIGHT_AVOID();
  }
}

void SIDE_AVOID() {
  if(distance[LEFT] <= distTurn && distance[RIGHT] > distTurn) {
    LIGHT_left = 0;
    LIGHT_right = 0;
    SONAR_left = 0;
    SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnMAX, 0));
  } else if(distance[RIGHT] <= distTurn && distance[LEFT] > distTurn) {
    LIGHT_left = 0;
    LIGHT_right = 0;
    SONAR_right = 0;
    SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnMAX, 0);
  } else if(distance[LEFT] <= distTurn && distance[RIGHT] <= distTurn) {
    LIGHT_left = 0;
    LIGHT_right = 0;
    if(distance[LEFT] < distance[RIGHT]) {
      SONAR_left = 0;
      SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnNARROW, 0));
    } else if(distance[RIGHT] < distance[LEFT]) {
      SONAR_right = 0;
      SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnNARROW, 0);
    }
  } else {
    SONAR_right = 0;
    SONAR_left = 0;
  }
}
/*================================== КОНЕЦ ПОВЕДЕНИЯ ==================================*/

Схемы