// Библиотека для работы с сервоприводами #include // Даём понятное имя пину направления вращения левого мотора #define PIN_ML_DIR 7 // Даём понятное имя пину управления скоростью вращения левого мотора #define PIN_ML_SPEED 6 // Даём понятное имя пину направления вращения правого мотора #define PIN_MR_DIR 4 // Даём понятное имя пину управления скоростью вращения правого мотора #define PIN_MR_SPEED 5 // Даём понятное имя пину левого датчика линии #define PIN_LINE_L A0 // Даём понятное имя пину правого датчика линии #define PIN_LINE_R A1 // Даём понятные имена пинам левого энкодера A и B #define PIN_ENC_LA 2 #define PIN_ENC_LB 12 // Даём понятные имена пинам правого энкодера A и B #define PIN_ENC_RA 3 #define PIN_ENC_RB 13 // Даём понятное имя пину ИК-дальномера #define PIN_IRR A2 // Даём понятные имена пинам УЗ-дальномера TRIG и ECHO #define PIN_USR_T 11 #define PIN_USR_E 10 // Даём понятное имя пину сервопривода головы #define PIN_SRV_HEAD 8 // Даём понятное имя пину сервомотора захвата #define PIN_SRV_GRIP 9 // Даём понятное имя пину пищалки #define PIN_BUZZER A3 // Создаём объект для работы сервоприводом головы Servo head; // Создаём объект для работы сервоприводом захвата Servo grip; // Задаём углы для верхнего и нижнего положения захвата // Значения углов подбираются экспериментально в диапазоне от 0 до 180 constexpr int GRIP_UP = 175; constexpr int GRIP_DOWN = 75; // Настраиваем направление вращения моторов // Если мотор вращается в обратную сторону, меняем false на true constexpr bool ML_INVERT = false; constexpr bool MR_INVERT = false; // Задаем порог, ниже которого считаем, что датчик видит чёрную линию constexpr int LINE_BLACK = 20; // Задаём скорост вращения мотора при повороте constexpr int TURN_SPEED = 128; // Задаём время для сьезда с линии при повороте в миллисекундах constexpr int TURN_WAIT = 150; // Задаем время поворота робота назад для выравнивания по линии constexpr int TURN_BACK = 200; // Задаём скорост моторов для движения по линии constexpr int DRIVE_SPEED = 150; void setup() { // Настраиваем пины управления моторами в режим выхода pinMode(PIN_ML_DIR, OUTPUT); pinMode(PIN_ML_SPEED, OUTPUT); pinMode(PIN_MR_DIR, OUTPUT); pinMode(PIN_ML_SPEED, OUTPUT); // Настраиваем пины управления сервоприводами в режим выхода pinMode(PIN_SRV_HEAD, OUTPUT); pinMode(PIN_SRV_GRIP, OUTPUT); // Настраиваем пины УЗ-дальномера // пин TRIG в режим выхода, пин ECHO в режим входа pinMode(PIN_USR_T, OUTPUT); pinMode(PIN_USR_E, INPUT); // Настраиваем пины датчиков линии в режим входа pinMode(PIN_LINE_L, INPUT); pinMode(PIN_LINE_R, INPUT); // Настраиваем пин ИК-дальномера в режим входа pinMode(PIN_IRR, INPUT); // Настраиваем пины энкодеров в режим входа pinMode(PIN_ENC_LA, INPUT); pinMode(PIN_ENC_LB, INPUT); pinMode(PIN_ENC_RA, INPUT); pinMode(PIN_ENC_RB, INPUT); // Инициализируем сервопривод головы head.attach(PIN_SRV_HEAD); // Инициализируем сервопривода захвата head.attach(PIN_SRV_GRIP); // Запускаем выполнение алгоритма runRobot(); } void loop() { // Функция loop() не используется } // Получение показаний с ИК-дальномера int readDistIRR() { // Считываем показания с ИК-дальномера в отчётах АЦП int value = analogRead(PIN_IRR); // Конвертируем показания в напряжение float voltage = value * (5.0 / 1024.0); // Конвертируем напряжение в расстояние в см float distance = 29.988 * pow(voltage, -1.173); // Возвращаем полученное значение return int(distance); } // Считывание левого датчика линии int readLineL() { return map(analogRead(PIN_LINE_L), 0, 1024, 100, 0); } // Считывание правого датчика линии int readLineR() { return map(analogRead(PIN_LINE_R), 0, 1024, 100, 0); } // Функция поднятия захвата void gripUp() { // Плавно поднимаем захват из нижнего положения в верхнее for (int i = GRIP_DOWN; i < GRIP_UP; i += 5) { grip.write(i); delay(50); } } // Функция опускания захвата void gripDown() { // Плавно опускаем захват из верхнего положения в нижнее for (int i = GRIP_UP; i > GRIP_DOWN; i -= 5) { grip.write(i); delay(50); } } // Управление движением робота // Указываем скорость левого и правого мотора от 0 до 255, // где 0 — остановка, а 255 — максимальная скорость // Знак задаёт направление вращения: плюс — вперёд, минус — назад void motorsDrive(int ml, int mr) { // Определяем направление вращения левого мотора bool mlDir = ml < 0; // Определяем направление вращения правого мотора bool mrDir = mr < 0; // При необходимости инвертируем направление моторов if (ML_INVERT) mlDir = !mlDir; if (MR_INVERT) mrDir = !mrDir; // Задаём направление вращения моторов digitalWrite(PIN_ML_DIR, mlDir); digitalWrite(PIN_MR_DIR, mrDir); // Задаём скорость вращения моторов analogWrite(PIN_ML_SPEED, abs(ml)); analogWrite(PIN_MR_SPEED, abs(mr)); } // Функция поворота направо void turnR(int speed, int waitTime) { // Включаем левый мотор motorsDrive(speed, 0); // Даем роботу время съехать с линии delay(waitTime); // Ждём, пока левый датчик снова увидит линию while (readLineL() > LINE_BLACK) { delay(10); } // Останавливаемся после обнаружения линии motorsDrive(0, 0); delay(500); // Немного доворачиваем назад, чтобы выровнять робота motorsDrive(0, speed); delay(TURN_BACK); // Останавливаем моторы motorsDrive(0, 0); delay(250); } // Функция поворота налево void turnL(int speed, int waitTime) { // Включаем правый мотор motorsDrive(0, speed); // Даем роботу время съехать с линии delay(waitTime); // Ждём, пока правый датчик снова увидит линию while (readLineR() > LINE_BLACK) { delay(10); } // Останавливаемся после обнаружения линии motorsDrive(0, 0); delay(500); // Немного доворачиваем назад, чтобы выровнять робота motorsDrive(speed, 0); delay(TURN_BACK); // Останавливаем моторы motorsDrive(0, 0); delay(250); } // Функция возврата после поворота направо void turnBackR(int speed, int waitTime) { // Включаем левый мотор в обратную сторону motorsDrive(-speed, 0); // Ждём, пока левый датчик снова увидит линию while (readLineL() > LINE_BLACK) { delay(10); } // Даем время, чтобы выровнять робота delay(waitTime); // Останавливаем моторы motorsDrive(0, 0); delay(250); } // Функция возврата после поворота налево void turnBackL(int speed, int waitTime) { // Включаем левый мотор в обратную сторону motorsDrive(0, -speed); // Ждём, пока левый датчик снова увидит линию while (readLineR() > LINE_BLACK) { delay(10); } // Даем время, чтобы выровнять робота delay(waitTime); // Останавливаем моторы motorsDrive(0, 0); delay(250); } // Функция разворота на 180° по часовой стрелке (направо) void turn180R(int speed, int waitTime) { // Включаем моторы в разные стороны motorsDrive(speed, -speed); // Даем роботу время съехать с линии delay(waitTime); // Ждём, пока датчик снова увидит линию while (readLineL() > LINE_BLACK) { delay(10); } // Останавливаемся после обнаружения линии motorsDrive(0, 0); delay(200); // Немного доворачиваем назад, чтобы выровнять робота motorsDrive(-speed, speed); delay(TURN_BACK); // Останавливаем моторы motorsDrive(0, 0);; delay(250); } // Функция разворота на 180° против часовой стрелке (налево) void turn180L(int speed, int waitTime) { // Включаем моторы в разные стороны motorsDrive(-speed, speed); // Даем роботу время съехать с линии delay(waitTime); // Ждём, пока датчик снова увидит линию while (readLineR() > LINE_BLACK) { delay(10); } // Останавливаемся после обнаружения линии motorsDrive(0, 0); delay(200); // Немного доворачиваем назад, чтобы выровнять робота motorsDrive(speed, -speed); delay(TURN_BACK); // Останавливаем моторы motorsDrive(0, 0);; delay(250); } // Функция езды до перекрестка void goToCrossroad(int speed) { // Задаём пропорциональный коэффициент П-регулятора int Kp = 10; // Считываем показания датчиков линии int leftLine = readLineL(); int rightLine = readLineR(); // Следуем по линии, пока не встретим перекрёсток while (leftLine > LINE_BLACK && rightLine > LINE_BLACK) { // Считываем показания датчиков линии leftLine = readLineL(); rightLine = readLineR(); // Рассчитываем ошибку int err = leftLine - rightLine; // Формируем управляющий сигнал int out = Kp * err; // Ограничиваем максимальное и минимальное значения управляющего сингала out = constrain(out, -100, 100); // Регулируем скорость моторов в соответствии с управляющим сигналом motorsDrive(speed + out, speed - out); } // Даем роботу немного времени заехать за линию motorsDrive(speed, speed); delay(500); // Останавливаем моторы motorsDrive(0, 0); } // Функция для звукового сигнала void beep(int n) { // Включаем и выключам пищалку в цикле n раз for (int i = 0; i < n; i++) { // Включаем пищалку на 250 миллисекунд digitalWrite(PIN_BUZZER, HIGH); delay(250); // Выключаем пищалку на 250 миллисекунд digitalWrite(PIN_BUZZER, LOW); delay(250); } } void scan(int i, bool cubes[5]) { // Поворачиваемся к ячейке turnR(TURN_SPEED, TURN_WAIT); // Немного ждём, пока показания дальномера стабилизируются delay(500); // Проверяем, есть ли в ячейке кубик if (readDistIRR() < 30) { // Сигнализируем, что обнаружили кубик beep(1); // Запоминаем, что в этой ячейке есть кубик cubes[i] = 1; } // Поворачиваемся обратно к линии turnBackR(TURN_SPEED, TURN_WAIT); } void goToB() { // Доезжаем до верхнего угла трассы goToCrossroad(DRIVE_SPEED); // Поворачиваем направо turnR(TURN_SPEED, TURN_WAIT); // Доезжаем до нижнего угла трассы goToCrossroad(DRIVE_SPEED); // Поворачиваем к ряду B turnR(TURN_SPEED, TURN_WAIT); } void loadCube() { // Поворачиваем к ячейке turnR(TURN_SPEED, TURN_WAIT); // Доезжаем до кубика goToCrossroad(DRIVE_SPEED); // Опускаем захват gripDown(); // Разворачиваемся на 180° turn180R(TURN_SPEED, TURN_WAIT); // Едем обратно к линии goToCrossroad(DRIVE_SPEED); // Выезжаем на линию turnL(TURN_SPEED, TURN_WAIT); } void deliverCube(int i, int n) { // Едем до угла трассы for (int j = 0; j < n - i + 1; j++) { goToCrossroad(DRIVE_SPEED); } // Поворачиваем налево turnL(TURN_SPEED, TURN_WAIT); // Едем до верхнего угла goToCrossroad(DRIVE_SPEED); // Поворачиваем налево turnL(TURN_SPEED, TURN_WAIT); // Едем к нужной ячейке for (int j = 0; j < n - i + 1; j++) { goToCrossroad(DRIVE_SPEED); } } void unloadCube() { // Поворачиваем к ячейке turnL(TURN_SPEED, TURN_WAIT); // Доезжаем до места установки кубика goToCrossroad(DRIVE_SPEED); // Опускаем захват gripDown(); // Разворачиваемся на 180° turn180R(TURN_SPEED, TURN_WAIT); // Едем обратно к линии goToCrossroad(DRIVE_SPEED); // Выезжаем на линию turnR(TURN_SPEED, TURN_WAIT); } void goBack(int i, int n) { // Едем до угла трассы for (int j = 0; j < n - i + 1; j++) { goToCrossroad(DRIVE_SPEED); } // Поворачиваем направо turnR(TURN_SPEED, TURN_WAIT); // Доезжаем до нижнего угла трассы goToCrossroad(DRIVE_SPEED); // Поворачиваем направо turnR(TURN_SPEED, TURN_WAIT); // Едем к исходной ячейке for (int j = 0; j < n - i + 1; j++) { goToCrossroad(DRIVE_SPEED); } } void moveCube(int i, int n) { // Забираем кубик loadCube(); // Везём кубик к нужной ячейке deliverCube(i, n); // Выгружаем кубик unloadCube(); // Возвращаемся на исходную позицию goBack(i, n); } void runRobot() { // Количество перекрёстков const int n = 5; // Расположение кубиков bool cubes[] = {0, 0, 0, 0, 0}; // Сигнализируем о начале выполнения алгоритма beep(2); // Отслеживаем текущий перекрёсток int i = 0; // Сканируем кубики в верхнем ряду while (i < n) { // Едем до перекрёстка goToCrossroad(DRIVE_SPEED); // Сканируем ячейку scan(i, cubes); // Переходим к следующему перекрёстку i++; } // Все перекрёстки просканированы — переходим к нижнему ряду goToB(); // Проезжаем перекрёстки в обратном порядке while (i > 0) { // Едем до перекрёстка goToCrossroad(DRIVE_SPEED); // Проверяем, нужно ли переместить кубик if (cubes[i - 1] == 0) { // Перемещаем кубик moveCube(i, n); } // Переходим к следующему перекрёстку i--; } // Все кубики на своих местах — возвращаемся в стартовую зону // Доезжаем до нижнего угла трассы goToCrossroad(DRIVE_SPEED); // Поворачиваем направо turnR(TURN_SPEED, TURN_WAIT); // Доезжаем до верхнего угла трассы рядом со стартовой зоной goToCrossroad(DRIVE_SPEED); // Сигнализируем о завершении алгоритма beep(3); }