Робот, ездящий по линии под управлением Arduino

Существует множество подходов для решения задачи следования по линии. Выбор одного из них зависит от конкретной конструкции робота, от количества сенсоров, их расположения относительно колёс и друг друга.

В нашем примере будет собран робот на лёгкой платформе с двумя колёсами и двумя датчиками линии, расположенными на днище робота перед колёсами.

В результате выглядеть он будет так:

Для нашего примера понадобятся следующие детали:

• Arduino Uno
• Двухколёсная платформа miniQ
• Motor Shield
• Troyka Shield
• Пара датчиков линий
• Несколько соединительных проводов и болтов и гаек для крепления датчиков и Arduino Uno
• Кабель питания от батарейки Крона и сама батарейка

Вообще говоря, лучше было бы использовать NiMH-аккумуляторы: они лучше отдают ток и значительно дольше держат напряжение, но для целей этого проекта одной батарейки на 9 В вполне хватило.

Сначала соберём робота, установим всю механику и электронику.

Для начала прикрепим колёса к моторам.

Затем с помощью пластиковых П-образных креплений прикручиваем моторчики к платформе. Обратите внимание на взаимное расположение крепления и моторчики: в креплении есть небольшие углубления, так что если всё соединить правильно, то моторчики будут крепко держаться и никуда не выскочат.

Теперь крепим балансировочный шар.

Отлично! Платформа собрана. Если вам кажется, что колёсам отведено слишком мало места и они трутся о платформу, то скорее всего вам нужно посильнее надавить на колёса, чтобы они плотнее сели на вал мотора.

Закрепим их, как показано на фото:

Можно было бы выбрать и другое место. Это могло бы сделать контроль проще или сложнее, а самого робота более или менее эффективным. Оптимальное расположение — вопрос серии экспериментов. Для этого проекта просто был выбран такой способ крепления.

Arduino закрепим с противоположной стороны двумя винтиками и гайками.

Опять же, можно выбрать и другое место. Например над колёсами, если приподнять Arduino на латунных стойках. Это изменило бы положение центра масс и повлияло бы на эффективность робота в лучшую или худшую сторону.

Установим Motor Shield на Arduino и подсоединим соединительные провода. Обратите внимание, чтобы соотвествовать программному коду из примера ниже, моторчики соединены с Motor Shield так: правый — к клеммам M1 с прямой полярностью (плюс к плюсу), а левый — к M2 с обратной (плюс к минусу).

В этом проекте, для экономии времени концы соединительных проводов просто скручены с контактами моторов. При работе «начисто» стоит жёстко припаять провода к моторам.

Присоединяем сверху Troyka Shield и подключаем датчики к 8 и 9 цифровым контактам. В итоге получаем следующую конструкцию:

Теперь напишем программу, которая заставит собранную конструкцию двигаться по нарисованной линии. В проекте мы будем использовать чёрную линию, напечатанную на белых листах бумаги.

Пусть у нас усть белое поле, и на нём чёрным нарисован трек для нашего робота. Используемые датчики линии выдают логический ноль, когда «видят» чёрное и единицу, когда «видят» белое.

На прямой робот должен пропускать трек между сенсоров, то есть оба сенсора должны показывать единички.

При повороте траектории направо, правый сенсор наезжает на трек и начинает показывать логический ноль. При повороте налево, ноль показывает левый сенсор.

Таким образом получаем простую систему с тремя состояниями:

• STATE_FORWARD — нужно ехать вперёд
• STATE_RIGHT — нужно поворачиваться направо
• STATE_LEFT — нужно поворачиваться налево

На вход системы поступает информация с сенсоров. Получаем следующую логику переходов:

LineRobot_v1.ino

// Моторы подключаются к клеммам M1+,M1-,M2+,M2-   // Motor shield использует четыре контакта 6,5,7,4 для управления моторами  #define SPEED_LEFT       6 #define SPEED_RIGHT      5  #define DIR_LEFT         7 #define DIR_RIGHT        4 #define LEFT_SENSOR_PIN  8 #define RIGHT_SENSOR_PIN 9   // Скорость, с которой мы движемся вперёд (0-255) #define SPEED            35   // Коэффициент, задающий во сколько раз нужно затормозить // одно из колёс для поворота #define BRAKE_K          4   #define STATE_FORWARD    0 #define STATE_RIGHT      1 #define STATE_LEFT       2   int state = STATE_FORWARD;   void runForward()  {     state = STATE_FORWARD;       // Для регулировки скорости `SPEED` может принимать значения от 0 до 255,     // чем болше, тем быстрее.      analogWrite(SPEED_LEFT, SPEED);     analogWrite(SPEED_RIGHT, SPEED);       // Если в DIR_LEFT или DIR_RIGHT пишем HIGH, мотор будет двигать соответствующее колесо     // вперёд, если LOW - назад.     digitalWrite(DIR_LEFT, HIGH);     digitalWrite(DIR_RIGHT, HIGH); }   void steerRight()  {     state = STATE_RIGHT;       // Замедляем правое колесо относительно левого,     // чтобы начать поворот     analogWrite(SPEED_RIGHT, SPEED / BRAKE_K);     analogWrite(SPEED_LEFT, SPEED);       digitalWrite(DIR_LEFT, HIGH);     digitalWrite(DIR_RIGHT, HIGH); }   void steerLeft()  {     state = STATE_LEFT;       analogWrite(SPEED_LEFT, SPEED / BRAKE_K);     analogWrite(SPEED_RIGHT, SPEED);       digitalWrite(DIR_LEFT, HIGH);     digitalWrite(DIR_RIGHT, HIGH); }     void setup()  {     // Настраивает выводы платы 4,5,6,7 на вывод сигналов      for(int i = 4; i <= 7; i++)         pinMode(i, OUTPUT);       // Сразу едем вперёд     runForward(); }    void loop()  {      // Наш робот ездит по белому полю с чёрным треком. В обратном случае не нужно     // инвертировать значения с датчиков     boolean left = !digitalRead(LEFT_SENSOR_PIN);     boolean right = !digitalRead(RIGHT_SENSOR_PIN);       // В какое состояние нужно перейти?     int targetState;       if (left == right) {         // под сенсорами всё белое или всё чёрное         // едем вперёд         targetState = STATE_FORWARD;     } else if (left) {         // левый сенсор упёрся в трек         // поворачиваем налево         targetState = STATE_LEFT;     } else {         targetState = STATE_RIGHT;     }       if (state == targetState) {         // мы уже делаём всё что нужно,         // делаем измерения заново         return;     }       switch (targetState) {         case STATE_FORWARD:             runForward();             break;           case STATE_RIGHT:             steerRight();             break;           case STATE_LEFT:             steerLeft();             break;     }       // не позволяем сильно вилять на прямой     delay(50); }

Однако если выставить скорость моторов побольше, мы столкнёмся со следующей проблемой: наш робот будет вылетать с трека, не успевая отреагировать на поворот. Это связано с тем, что наши моторчики не умеют тормозить мгновенно.

В этом легко убедиться поставив следующий эксперимент: с заданной скоростью робот будет двигаться по поверхности, и в некоторый момент будет установлена нулевая скорость и измерен тормозной путь робота. Пусть робот разгоняется по монотонной поверхности и тормозится при фиксировании импровизированной стоп-линии.

Эксперимент проведём для разных скоростей. Код программы для эксперимента таков:

stopping_distance_experiment.ino

#define LEFT_SENSOR_PIN   8 #define RIGHT_SENSOR_PIN  9 #define SPEED_LEFT        6 #define SPEED_RIGHT       5  #define DIR_LEFT          7 #define DIR_RIGHT         4   // Для того чтобы убедиться, что именно тормозной путь долог, а не команда остановиться  // приходит слишком поздно, будем включать светодиод, когда отдаётся команда. #define LED_PIN           13   int currSpeed = 40; void setup() {     for(int i = 4; i <= 7; ++i)         pinMode(i, OUTPUT);       analogWrite(SPEED_RIGHT, currSpeed);     digitalWrite(DIR_RIGHT, HIGH);       analogWrite(SPEED_LEFT, currSpeed);     digitalWrite(DIR_LEFT, HIGH);           pinMode(LED_PIN, OUTPUT); }   void loop() {     if (currSpeed > 120)         return;       boolean white[] = {         !digitalRead(LEFT_SENSOR_PIN),          !digitalRead(RIGHT_SENSOR_PIN)     };       if (white[0] && white[1]) {         // едем пока не упрёмся         return;     }       // зажигаем светодиод, останавливаем моторы     // и наблюдаем     digitalWrite(LED_PIN, HIGH);     analogWrite(SPEED_RIGHT, 0);     analogWrite(SPEED_LEFT, 0);     delay(5000);       // повторяем эксперимент, увеличивая скорость     // на 10 пунктов     currSpeed += 10;     if (currSpeed > 120)         return;       digitalWrite(LED_PIN, LOW);     analogWrite(SPEED_RIGHT, currSpeed);     analogWrite(SPEED_LEFT, currSpeed);     }

На той поверхности, на которой проводился эксперимент, были получены следующие результаты:

Таким образом, начиная с некоторого момента у нашего робота нет никакой возможности успеть среагировать и остаться на треке.

Что можно сделать?! После того, как сенсоры улавливают поворот, можно остановиться и вернуться назад на некоторое расстояние, зависящее от скорости перед остановкой. Однако мы можем отдать команду роботу ехать с какой-то скоростью, но не можем приказать ему проехать какое-то расстояние.

Для того, чтобы понять зависимость расстояния при заднем ходе от времени, был проведён ещё один замер:

time_distance_rate.ino

#define SPEED_LEFT      6 #define SPEED_RIGHT     5  #define DIR_LEFT        7 #define DIR_RIGHT       4   void go(int speed, bool reverseLeft, bool reverseRight, int duration) {     analogWrite(SPEED_LEFT, speed);     analogWrite(SPEED_RIGHT, speed);     digitalWrite(DIR_LEFT, reverseLeft ? LOW : HIGH);      digitalWrite(DIR_RIGHT, reverseRight ? LOW : HIGH);      delay(duration);  }   void setup()  {     for(int i = 4; i <= 7; ++i)         pinMode(i, OUTPUT); }    void loop()  {      // Задержка 5 секунд после включения питания      delay(5000);        for (int i = 200; i <= 1000; i += 100) {         // Несколько сотен мс вперёд          go(50, false, false, 200);         go(0, false, false, 0);           // Задержка 5 секунд         delay(5000);      }       // Остановка до ресета или выключения питания      go(0, false, false, 0);       // Приехали     while (true)         ;  }

На скорости 50, например, робот проделывал путь, зависящий от времени следующим образом:

Полученные две зависимости были линейно аппроксимированы, затем была выведена формула зависимости времени, которое надо двигаться назад, от скорости перед остановкой.

Обратим внимание на то, что у вас значения могут оказаться другими: из-за особенностей сборки либо из-за поверхности, поэтому в общем случае лучше провести все измерения самостоятельно.

Перед финальным экспериментом произведём ещё несколько поправок.

Во-первых, нам необязательно давать команду ехать назад перед каждым поворотом, как мы помним, на маленькой скорости робот прекрасно справляется и без этого. К тому же лучше ему двигаться не прямо назад, а немного поворачивая, всё-таки робот находится перед поворотом.

Во-вторых, нам стоит различать состояния робота: когда он движется по прямой, и ничто ему не мешает ускоряться; и когда робот входит в поворот. В первом случае действительно будем увеличивать скорость робота для более динамичного прохождения трека, во втором случае будем сбрасывать скорость до значения, достаточного для успешного прохождения поворота, и будем держать эту скорость ещё какое-то время.

В итоге наш код будет выглядит следующим образом:

Robot_v02.ino

// Моторы подключаются к клеммам M1+,M1-,M2+,M2-   // Motor shield использует четыре контакта 6,5,7,4 для управления моторами  #define SPEED_LEFT       6 #define SPEED_RIGHT      5  #define DIR_LEFT         7 #define DIR_RIGHT        4 #define LEFT_SENSOR_PIN  8 #define RIGHT_SENSOR_PIN 9   // Скорость, с которой мы движемся вперёд (0-255) #define SPEED            100   // Скорость прохождения сложных участков #define SLOW_SPEED       35   #define BACK_SLOW_SPEED  30 #define BACK_FAST_SPEED  50   // Коэффициент, задающий во сколько раз нужно затормозить // одно из колёс для поворота #define BRAKE_K          4   #define STATE_FORWARD    0 #define STATE_RIGHT      1 #define STATE_LEFT       2   #define SPEED_STEP       2   #define FAST_TIME_THRESHOLD     500   int state = STATE_FORWARD; int currentSpeed = SPEED; int fastTime = 0;   void runForward()  {     state = STATE_FORWARD;       fastTime += 1;     if (fastTime < FAST_TIME_THRESHOLD) {         currentSpeed = SLOW_SPEED;     } else {         currentSpeed = min(currentSpeed + SPEED_STEP, SPEED);     }       analogWrite(SPEED_LEFT, currentSpeed);     analogWrite(SPEED_RIGHT, currentSpeed);       digitalWrite(DIR_LEFT