Робот-следящий за строкой - ПИД-управление - Настройка Android

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

	IDE Arduino
	MIT App Inventor 2

Об этом проекте

Целью этого проекта является создание робота-следящего за линией с ПИД-регулированием. Мы также будем использовать устройство Android, чтобы легко настроить основные параметры управления для лучшей и быстрой настройки.

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

Ниже приведено видео, на котором робот движется по линейной цепи:

Шаг 1. Спецификация материалов

Список необходимых материалов очень прост, а готовый робот стоит очень дешево (около 75 долларов США):

Кузов (может быть адаптирован под ваши нужды):

• 2 деревянных квадрата (80 x 80 мм)

• 3 зажима для папок

• 2 деревянных колеса (диаметр:50 мм)

• 1 заклинатель шаров.

• 9 резинок

• Полоса кадров команд 3M

• Пластиковые соединения для крепления датчика

• Доска для хлеба и электропроводка

• 2 комплекта из 4 никель-металлгидридных батарей (5 В каждый комплект).

• 2 X SM-S4303R пластиковый сервопривод непрерывного вращения на 360 градусов

• Ардуино Нано

• Модуль Bluetooth HC-06

• 5 линейных датчиков (4-канальный инфракрасный модуль слежения за линией TCRT5000 + 1 независимый путевой датчик)

• 1 светодиод

• 1 кнопка

Шаг 2. Настройка двигателей

Для двигателей использовались 2 сервопривода непрерывного действия (SM-S4303R). Они будут «склеены» вместе, образуя цельный блок, как вы можете видеть на фотографии (используйте полосу 3M Command, клей или двусторонний скотч). Эти сервоприводы будут работать с заданной скоростью, определяемой шириной импульса, полученной при вводе данных. Для этого конкретного сервопривода ширина импульса изменяется от 1,0 мс (1000 микросекунд) до 2,0 мс (2000 микросекунд). Другие сервоприводы могут работать с другой шириной импульса.

Подробно:

• Импульс 1,5 мс установит сервопривод в нейтральное положение или «остановлен».

• Импульс 1,0 мс заставит сервопривод набрать полную скорость (около 70 об / мин) в одном направлении.

• Импульс 2,0 мс на полной скорости в обратном направлении.

• Импульс от 1,0 до 1,5 мс или от 1,5 до 2,0 мс генерирует пропорциональную скорость.

После того, как оба сервопривода физически подключены, следуйте приведенной выше схеме, чтобы подать их (внешние 5 В или 6 В) и подать на них сигнал Arduino:

• Левый сервопривод:вывод 5 Arduino

• Правый сервопривод:вывод 3 Arduino

После того, как все подключено, первое, что необходимо сделать, это отправить импульс 1,5 мс, чтобы проверить, остановлены ли двигатели (не работают). В противном случае сервоприводы должны быть отрегулированы до полной остановки (ищите желтый болт, расположенный под сервоприводом).

ПРИМЕЧАНИЕ :Если ваш сервопривод не имеет этой физической регулировки, попробуйте изменить параметр "1500" микросекунд внутри функции (вверх или вниз), пока не получите полную остановку.

Приведенный ниже код Arduino может выполнить эту работу:

  #include  // Серво-библиотека Servo leftServo; Servo rightServo; Void setup () {leftServo.attach (5); rightServo.attach (3); leftServo.writeMicroseconds (1500); rightServo.writeMicroseconds (1500);} void loop () {}  

Шаг 3. Соберите корпус и двигатели для проверки движения

• С помощью планки рамы 3M Command прикрепите 2 сервопривода к одной из квадратных деревянных деталей.

• Прикрепите второй квадрат дерева к предыдущему с помощью зажимов для бумаги. Отрегулируйте длину платформы в соответствии с вашими потребностями.

• Закрепите шарнирный фиксатор с помощью зажима для бумаги.

• Электропитание двигателей будет осуществляться от одной из 5-вольтовых батарей. Этот комплект батарей будет установлен между макетной платой и корпусом.

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

• Подключите Arduino Nano к макетной плате.

• Подключите GND Power GND к Arduino GND.

• Подключите сервоприводы к Arduino:ВЛЕВО ==> Контакт 5; ВПРАВО ==> Контакт 3

• Подключите светодиод к контакту 13 Arduino.

• Подключите кнопку к выводу 9 Arduino

Обратите внимание, что из-за того, как сервоприводы установлены (напротив), диапазон скоростей составляет:

• Скорость движения правого сервопривода увеличивается с 1500 мкс (остановлен) до 2000 мкс (полная скорость).

• Скорость движения левого сервопривода увеличивается с 1500 мкс (остановлен) до 1000 (полная скорость).

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

Также к выводу 9 подключена кнопка. Эта кнопка очень полезна в тестовых целях и для запуска робота.

Например:

  while (digitalRead (buttonPin)) {} motorTurn (LEFT, 500); motorTurn (RIGHT, 500);  

Обратите внимание, что две строки, которые будут давать команду роботу повернуть ВЛЕВО, подождать 500 мс и повернуть ВПРАВО, появятся только после того, как вы нажмете кнопку (buttonPin =0). Перед этим программа будет остановлена ​​в бесконечном цикле.

Приведенный ниже код можно использовать в качестве основы для полного теста двигателя (вперед, назад, полная остановка, поворот налево, поворот направо). При необходимости вы должны отрегулировать задержки для требуемого угла поворота в зависимости от ваших двигателей (также, иногда значения левого и правого импульса должны немного отличаться, чтобы компенсировать любую несбалансированность двигателей.

Шаг 4. Модуль Bluetooth (необязательно)

Модуль Bluetooth HC-06 должен быть установлен на макетной плате, как показано на чертеже. Будет использоваться библиотека Arduino SoftSerial.

Ниже выводов HC-06:

• Пин-код передатчика к пину 10 Arduino (Rx)

• Контакт RX к пину 11 Arduino (Tx)

• VCC / GND к Arduino 5V / GND

Робот будет работать с Bluetooth или без него. Код построен таким образом, что, если вы не активируете BT, робот будет использовать параметры по умолчанию. Так что не беспокойтесь, если вы предпочитаете не устанавливать модуль HC-06, код все равно будет работать нормально. В последней части этого руководства я изучу, как использовать приложение Android для отправки данных для лучшей настройки параметров робота и / или перемещения робота в ручном режиме. Я оставлю использование Bluetooth и приложения как необязательное на тот случай, если кто-то захочет узнать больше об использовании, например, робота-следящего за линией для соревнований.

Шаг 5:Добавление датчиков линии

Подключите кабели к контактам Arduino, как показано ниже:

• Датчик 0 =12

• Датчик 1 =18

• Датчик 2 =17

• Датчик 3 =16

• Датчик 4 =19

• Закрепите 5 датчиков на пластиковом стержне, как показано на фотографиях.

• Для тестирования рекомендуется маркировать датчики. Название датчиков изменяется от «0» (больше слева) до «4» (больше справа)

• Проложите кабели под рамой, закрепив их эластичными лентами. Будьте осторожны, чтобы не перепутать колеса или ролик.

• Закрепите второй комплект батарей на 5 В и подключите его к Arduino Vin.

В моем случае я использую модуль с 4 встроенными датчиками + 1 дополнительный. Все они совместимы. Для простоты я включил в схему 5 отдельных датчиков, соединенных вместе. Окончательные результаты одинаковы в обеих конфигурациях.

Шаг 6. Внедрение логики ИК-датчика

ИК-датчик состоит из отдельного ИК-светодиода и ИК-фотодиода. ИК-свет, излучаемый светодиодом, падает на поверхность и отражается обратно на ИК-фотодиод. Затем фотодиод генерирует выходное напряжение, пропорциональное уровню отражательной способности поверхности (более высокие значения для «светлых поверхностей» и более низкие для «черных / темных поверхностей»).

В случае используемых датчиков интегральная схема в модуле генерирует на выходе простой цифровой сигнал (ВЫСОКИЙ:темный; НИЗКИЙ:светлый). Потенциометр, установленный на модуле (см. Фото), отрегулирует правильный уровень света, который будет считаться «темным» или «светлым». Он работает таким образом, что когда цвет отраженного света является черным / темным, на его выходе генерируется ВЫСОКИЙ («1») цифровой уровень, а для другого более светлого цвета - НИЗКИЙ («0»). Я использовал здесь интегрированный модуль с 4 датчиками и дополнительный модуль с единственным датчиком (другая форма, но та же логика). Комбинация представляет собой массив из 5 датчиков, которые, как я обнаружил, хороши для приятного и плавного управления, как описано ниже.

Массив из 5 датчиков установлен таким образом, что, если только один датчик отцентрирован относительно черной линии, только этот конкретный датчик будет давать ВЫСОКИЙ уровень. С другой стороны, расстояние между датчиками должно быть рассчитано так, чтобы два датчика могли покрыть всю ширину черной линии одновременно, создавая также ВЫСОКИЙ уровень на обоих датчиках (см. Изображения выше).

Возможные выходные данные матрицы датчиков при следовании за линией:

• 0 0 0 0 1

• 0 0 0 1 1

• 0 0 0 1 0

• 0 0 1 1 0

• 0 0 1 0 0

• 0 1 1 0 0

• 0 1 0 0 0

• 1 1 0 0 0

• 1 0 0 0 0

Наличие 5 датчиков позволяет генерировать «переменную ошибки», которая поможет контролировать положение робота над линией, как показано ниже.

Предположим, что оптимальным условием является центрирование робота, при котором линия проходит чуть ниже «среднего датчика» (Датчик 2). Результатом массива будет:0 0 1 0 0, и в этой ситуации «ошибка» будет «нулем». Если робот начинает двигаться влево (линия «кажется, что движется« вправо »), ошибка должна увеличиваться с положительным сигналом. Если робот начинает двигаться вправо (линия« кажется движется влево »), в Таким же образом ошибка должна увеличиваться, но теперь с отрицательным сигналом.

Переменная ошибки, связанная с состоянием датчика, будет:

0 0 1 0 0 ==> Ошибка =0

• 0 0 0 0 1 ==> Ошибка =4

• 0 0 0 1 1 ==> Ошибка =3

• 0 0 0 1 0 ==> Ошибка =2

• 0 0 1 1 0 ==> Ошибка =1

• 0 1 1 0 0 ==> Ошибка =-1

• 0 1 0 0 0 ==> Ошибка =-2

• 1 1 0 0 0 ==> Ошибка =-3

• 1 0 0 0 0 ==> Ошибка =-4

Глядя на код Arduino, каждый из датчиков будет определен с определенным именем (учтите, что датчику следования по линии, находящемуся левее, нужно присвоить метку "0"):

  const int lineFollowSensor0 =12; const int lineFollowSensor1 =18; const int lineFollowSensor2 =17; const int lineFollowSensor3 =16; const int lineFollowSensor4 =19;  

Для хранения значений каждого датчика будет создана переменная массива:

  int LFSensor [5] ={0, 0, 0, 0, 0};  

Каждая позиция массива будет постоянно обновляться выходными данными каждого из датчиков:

  LFSensor [0] =digitalRead (lineFollowSensor0); LFSensor [1] =digitalRead (lineFollowSensor1); LFSensor [2] =digitalRead (lineFollowSensor2); LFSensor [3] =digitalRead (lineFollowSensor3); LFSensor3); LFSensor [4]; =digitalRead (lineFollowSensor4);  

Имея значение каждого из датчиков, необходимо реализовать логику для генерации переменной ошибки:

  if ((LFSensor [0] ==0) &&(LFSensor [1] ==0) &&(LFSensor [2] ==0) &&(LFSensor [3] ==0) &&(LFSensor [4] ==1)) error =4; else if ((LFSensor [0] ==0) &&(LFSensor [1] ==0) &&(LFSensor [2] ==0) &&(LFSensor [3] ==1) &&(LFSensor [4] ==1)) error =3; else if ((LFSensor [0] ==0) &&(LFSensor [1] ==0) &&(LFSensor [2] ==0) &&(LFSensor [3] ==1) &&(LFSensor [4] ==0)) error =2; else if ((LFSensor [0] ==0) &&(LFSensor [1] ==0) &&(LFSensor [2] ==1) &&(LFSensor [3] ==1) &&(LFSensor [4] ==0)) error =1; else if ((LFSensor [0] ==0) &&(LFSensor [1] ==0) &&(LFSensor [2] ==1) &&(LFSensor [ 3] ==0) &&(LFSensor [4] ==0)) error =0; else if ((LFSensor [0] ==0) &&(LFSensor [1] ==1) &&(LFSensor [2] ==1) &&(LFSensor [3] ==0) &&(LFSensor [4] ==0)) error =- 1; иначе if ((LFSensor [0] ==0) &&(LFSensor [1] ==1 ) &&(LFSensor [2] ==0) &&(LFSensor [3] ==0) &&(LFSensor [4] ==0)) error =-2; else if ((LFSensor [0] ==1) &&(LFSensor [1] ==1) &&(LFSensor [2] ==0) &&(LFSensor [3] ==0) &&(LFSensor [4] ==0)) error =-3; else if ((LFSensor [0] ==1) &&(LFSensor [1] ==0) &&(LFSensor [2] ==0) &&(LFSensor [3] ==0) &&(LFSensor [4] ==0)) error =-4;  

Шаг 7:Управление направлением (пропорциональное управление - P)

Идеально! На данный момент наш робот собран и готов к работе. Вы должны выполнить некоторые базовые тесты с двигателями, прочитать выходные данные датчиков и проверить их по линии. Не хватает настоящего «мозга», первых шагов «искусственного интеллекта». Мы получим это, реализовав логику управления, которая будет гарантировать, что робот будет продолжать следовать линии.

Простое пропорциональное управление:

Предположим, что робот перемещается по линии, а выходные данные массива датчиков: «0 0 1 0 0» . Соответствующая ошибка - «0». В этой ситуации оба двигателя должны вращаться вперед с постоянной скоростью.

Например:

Определение переменной: iniMotorSpeed ​​=250 ; означает, что ЛЕВОЙ сервопривод будет получать импульсы длительностью 1,250 мкс, а ПРАВЫЙ сервопривод 1750 мксек. С этими параметрами робот будет двигаться вперед с половинной скоростью. Помните, что скорость движения ПРАВОГО сервопривода будет изменяться с длительностью импульса от 1500 мкс (остановлен) до 2000 мкс (полная скорость), а ЛЕВОГО сервопривода - от 1500 мкс (остановлен) до 1000 мкс (полная скорость).

  rightServo.writeMicroseconds (1500 + iniMotorPower); leftServo.writeMicroseconds (1500 - iniMotorPower);  

Предположим теперь, что робот движется влево (это как «ЛИНИЯ идет вправо») и покрывает также датчик 3. Выходной сигнал массива будет: «0 0 1 1 0» и error =1 . В этой ситуации вам нужно повернуть робота вправо. Для этого вы должны уменьшить скорость ПРАВОГО сервопривода, что означает уменьшение длины импульса. Кроме того, скорость ЛЕВОГО серво должна увеличиваться, что означает уменьшение длины ЛЕВОГО сервоимпульса. Для этого нам нужно изменить функцию управления двигателем:

  rightServo.writeMicroseconds (1500 + iniMotorPower - ошибка); ==> Положительная ошибка:уменьшить скорость leftServo.writeMicroseconds (1500 - iniMotorPower - ошибка); ==> Положительная ошибка:увеличьте скорость  

Вышеупомянутая логика верна, но легко понять, что добавление или вычитание «1» микросекунды в длине импульса не приведет к необходимой коррекции за реалистичное время. Интуитивно понятно, что число, которое нужно добавить или вычесть, должно быть больше, например 50, 100 и т. Д. Чтобы получить это, «ошибку» нужно умножить на константу (назовем ее «K»). Как только влияние этой константы будет пропорционально ошибке, мы назовем ее «Пропорциональная константа:Kp» . .

Моторная функция будет:

  int Kp =50; rightServo.writeMicroseconds (1500 + iniMotorPower - ошибка Kp *); leftServo.writeMicroseconds (1500 - iniMotorPower - ошибка Kp *);  

Мы можем возобновить то, что произойдет с двигателями, как показано ниже:

• Массив датчиков: 0 0 1 0 0 ==> ошибка =0 ==> Длина импульса правого сервопривода =1,750 мкс ==> Длина импульса левого сервопривода =1,250 мкс (оба двигателя имеют одинаковую скорость)

• Массив датчиков: 0 0 1 1 0 ==> ошибка =1 ==> Длина импульса правого сервопривода =1,700 мкс (медленнее) ==> Длина импульса левого сервопривода =1,200 мкс (быстрее)

Если ситуация обратная и робот движется вправо, ошибка будет «отрицательной», и скорость сервоприводов должна измениться:

• Массив датчиков: 0 0 1 0 0 ==> ошибка =0 ==> Длина импульса правого сервопривода =1,750 мкс ==> Длина импульса левого сервопривода =1,250 мкс (оба двигателя имеют одинаковую скорость)

• Sensor Array: 0 1 1 0 0 ==> ошибка =-1 ==> Длина импульса правого сервопривода =1800 мкс (быстрее) ==> Длина импульса левого сервопривода =1300 мкс (медленнее)

На данный момент ясно, что чем больше робот будет отклонен в сторону, тем больше будет ошибка и быстрее он должен вернуться в центр. Скорость, с которой робот отреагирует на ошибку, будет пропорциональна ей. Это называется «Пропорциональный контроль» . , то есть компонент "P" более сложной сети управления, PDI (пропорциональный, производный, интегральный).

Шаг 8:ПИД-регулирование (необязательно)

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

Если решились, поехали!

ПИД-регулятор (пропорциональный, производный и интегральный) - одна из наиболее распространенных схем управления. В большинстве промышленных контуров управления используются некоторые разновидности ПИД-регулирования. Существует множество способов настройки контура ПИД-регулирования, включая ручной метод, используемый в этом примере.

Думайте о PID как о простой пружине. Пружина имеет исходную длину, которая при нарушении ее расширения или сжатия имеет тенденцию восстанавливать свою исходную длину в кратчайшие сроки. Точно так же алгоритм PID в системе имеет заданное значение определенной физической величины, которую нужно контролировать, называемую « заданной точкой . ’, При изменении которого по какой-либо причине система контролирует другие необходимые функции, чтобы вернуться к исходной уставке в кратчайшие сроки. ПИД-регуляторы используются везде, где необходимо контролировать физическую величину и приравнивать ее к заданному значению. Пример, круиз-контроллер в автомобилях, роботах, регуляторах температуры, регуляторах напряжения и т. д.

Как работает PID?

Система вычисляет « ошибку . »Или« отклонение ’Физической величины от заданного значения путем измерения текущего значения этой физической величины с помощью датчика (ов). Чтобы вернуться к заданному значению, эта « ошибка ’Следует свести к минимуму, а в идеале - сделать равным нулю. Также этот процесс должен происходить как можно быстрее. В идеале не должно быть запаздывания в реакции системы на изменение ее уставки.

Дополнительную информацию можно найти во многих книгах и на веб-сайтах, в том числе здесь.

Реализация PID

i) Срок действия ошибки (e):

Это равно разнице между заданным значением и текущим значением контролируемой величины. Ошибка = set_point - текущее_значение (в нашем случае это переменная ошибки, полученная из положения робота над линией

ii) Пропорциональный срок (P):

Этот член пропорционален ошибке.

P =ошибка

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

iii) Интегральный член (I):

Этот член представляет собой сумму всех предыдущих значений ошибок.

I =I + ошибка

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

iv) Дифференциальный или производный член (D):

Этот член представляет собой разницу между мгновенной ошибкой от заданного значения и ошибкой от предыдущего момента.

D =error - previousError

Это значение отвечает за замедление скорости изменения физической величины, когда она приближается к заданному значению. Производный термин используется для уменьшения перерегулирования или того, насколько система должна « сверх исправлять ".

Уравнение:

Значение PID =(Kp * P) + (Ki * I) + (Kd * D)

Где:

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

Одним из подходов к настройке цикла может быть метод пробной ошибки :

Установите для переменной Kd значение 0 . и сначала настройте только член Kp. Kp из 25 в нашем случае это хорошее место для начала. На последнем этапе мы использовали Kp 50, который очень хорошо работает с моим роботом. Как только робот ответит разумно, настройте производную часть контура управления ( Kd ). Сначала установите значения Kp и Kd, равные 1/2 значения Kp. Например, если робот отвечает разумно с Kp =50, тогда установите Kp =25 и Kd =25 для запуска. Увеличьте коэффициент усиления Kd (производная), чтобы уменьшить перерегулирование, уменьшите его, если робот станет нестабильным.

• Если робот реагирует слишком медленно, увеличьте значение.

• Если кажется, что робот быстро реагирует, становясь нестабильным, уменьшите значение.

Еще один компонент цикла, который следует учитывать, - это фактическая частота выборки / цикла . . Увеличение или уменьшение этого параметра может существенно повлиять на производительность робота. Это устанавливается задержкой операторы, которые у вас есть в вашем коде. Это метод пробной ошибки для получения оптимального результата.

На основе описанного выше подхода была реализована следующая функция:

  void calculatePID () {P =error; I =I + ошибка; D =ошибка-предыдущая ошибка; Значение PID =(Kp * P) + (Ki * I) + (Kd * D); previousError =error;}  

Простая константа Kp, использованная на последнем шаге, будет заменена на это более полное PIDvalue . :

  void motorPIDcontrol () {int leftMotorSpeed ​​=1500 - iniMotorPower - PIDvalue; int rightMotorSpeed ​​=1500 + iniMotorPower - значение PID; leftServo.writeMicroseconds (leftMotorSpeed); rightServo.writeMicroseconds (rightMotorSpeed);}  

Но учтите, что если у вас Kd и Ki =0 , значение PID на последнем шаге используется только ошибка Kp *.

Шаг 9. Последний код

На этом этапе робот может следовать постоянному циклу и будет делать это без остановки. Программа цикла будет выглядеть так:

  void loop () {readLFSsensors (); // считываем датчики, сохраняем значения в массиве датчиков и вычисляем "ошибку" calculatePID (); motorPIDcontrol ();}  

Но для более полной и реальной работы важно добавить хотя бы пару основных " команд "готово" со строкой ". Например, давайте представим новую переменную:" mode ". Мы определим 3 состояния для этой переменной:

Режим:

• #define STOPPED 0

• #define FOLLOWING_LINE 1

• #define NO_LINE 2

Если все датчики обнаружат черную линию, выходной сигнал массива датчиков будет:1 1 1 1 1. В этом состоянии мы можем определить режим как «ОСТАНОВЛЕННЫЙ», и робот должен выполнить « полную остановку . ".

  if ((LFSensor [0] ==1) &&(LFSensor [1] ==1) &&(LFSensor [2] ==1) &&(LFSensor [3] ==1) &&(LFSensor [4] ==1)) {mode =STOPPED;}  

Другая распространенная ситуация с роботами Follower Line - это когда он не находит " никакой линии " ", или выходной сигнал Sensor Array:0 0 0 0 0. В этом случае мы можем запрограммировать его так, чтобы он поворачивался назад на 180o или поворачивался на небольшие углы, пока линия не будет найдена и нормальное условие следования линии не возобновится.

  else if ((LFSensor [0] ==0) &&(LFSensor [1] ==0) &&(LFSensor [2] ==0) &&(LFSensor [3] ==0) &&( LFSensor [4] ==0)) {mode =NO_LINE;}  

Полный loop () будет:

  void loop () {readLFSsensors (); переключатель (режим) {case STOPPED:motorStop (); ломать; case NO_LINE:motorStop (); motorTurn (СЛЕВА, 180); ломать; case FOLLOWING_LINE:calculatePID (); motorPIDcontrol (); ломать; }}  

The real final code will integrated some additional logic and also some variables that must be initialized, etc. During the above explanation I left them out for simplicity, but everything should be clear looking at final code.

Below is the final Arduino Code:

Step 10:Tuning the PID control using the Android App

In the previous code, you can find at "robotDefines.h " tab the following definitions for the constants to be used with the PID control:

float Kp=50;float Ki=0;float Kd=0; 

As explained at previous step, the best way to define the correct constant to be used with a PID controller is using the "Try-error" methodology. The bad side of that is that you must re-compile the program each time that you must change it. One way to speed-up the process is to use the Android App to send the constants at the "Set-Up Phase" .

I developed an Android App exclusively for that. In short there are the traditional manual commands:

• FW, BW, Left, Right and Stop where the app will send respectively to the BT module:'f', 'b', 'l', 'r' and 's'.

Also 3 sliders were included, one for each PID constants:

• Kd:"d/XXX" where "XXX" it is a number from 0 to 100.

• Kp:"p/XXX"

• Ki:"i/XXX"

An extra button was included that will work exactly as the button connected on Arduino Pin9. You can use one or the other, it does not matter.

Below you can find the .aia file that can be modified at MIT AppInventor and the .apk file to be installed directly in your Android device.

Step 11:Changing the Code for PID remote tuning

During the Setup, we will introduce a loop where you can send the PID parameters to the Robot before you put him over the line:

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command =""; } checkPIDvalues(); mode =STOPPED; 

The manual command function will be:

void manualCmd(){ switch (command[0]) { case 'g':mode =FOLLOWING_LINE; break; case 's':motorStop(); //turn off both motors break; case 'f':motorForward(); break; case 'r':motorTurn(RIGHT, 30); motorStop(); break; case 'l':motorTurn(LEFT, 30); motorStop(); break; case 'b':motorBackward(); break; case 'p':Kp =command[2]; break; case 'i':Ki =command[2]; break; case 'd':Kd =command[2]; break; }}  

In the video, you can see some tests using the Android App. Below the final code including the PID setup via Android:

Step 12:Conclusion

This is the first part of a more complex project, exploring the potentiality of a line follower robot. In the next part, I will develop a Maze solve robot, based on this this project here. Hopefully I can contribute for others to learn more about electronics, robot, Arduino, etc.

The update files for this project can be found at GITHUB. For more tutorials, please visit my Blog:MJRoBot.org

Greetings from the south of the world!

Thanks

Marcelo

Код

• Code snippet #1
• Code snippet #2
• Code snippet #3
• Code snippet #5
• Code snippet #6
• Code snippet #10
• Code snippet #11
• Code snippet #12
• Code snippet #15
• Code snippet #17
• Code snippet #18

Code snippet #1Plain text

#include  // Servo library Servo leftServo;Servo rightServo;Void setup(){ leftServo.attach(5); rightServo.attach(3); leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500);}void loop(){}

Code snippet #2Plain text

while(digitalRead(buttonPin)) { }motorTurn (LEFT, 500);motorTurn (RIGHT, 500);

Code snippet #3Plain text

const int lineFollowSensor0 =12;const int lineFollowSensor1 =18;const int lineFollowSensor2 =17;const int lineFollowSensor3 =16;const int lineFollowSensor4 =19;

Code snippet #5Plain text

LFSensor[0] =digitalRead(lineFollowSensor0);LFSensor[1] =digitalRead(lineFollowSensor1);LFSensor[2] =digitalRead(lineFollowSensor2);LFSensor[3] =digitalRead(lineFollowSensor3);LFSensor[4] =digitalRead(lineFollowSensor4);

Code snippet #6Plain text

if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==1 )) error =4;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==1 )&&(LFSensor[4]==1 )) error =3; else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) error =2;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) error =1;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =0;else if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==1 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =- 1;else if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-2;else if((LFSensor[0]==1 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-3;else if((LFSensor[0]==1 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-4;

Code snippet #10Plain text

void calculatePID(){ P =error; I =I + error; D =error-previousError; PIDvalue =(Kp*P) + (Ki*I) + (Kd*D); previousError =error;}

Code snippet #11Plain text

void motorPIDcontrol(){ int leftMotorSpeed =1500 - iniMotorPower - PIDvalue; int rightMotorSpeed =1500 + iniMotorPower - PIDvalue; leftServo.writeMicroseconds(leftMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed);}

Code snippet #12Plain text

void loop (){ readLFSsensors(); // read sensors, storage values at Sensor Array and calculate "error" calculatePID(); motorPIDcontrol();}

Code snippet #15Plain text

void loop() { readLFSsensors(); switch (mode) { case STOPPED:motorStop(); break; case NO_LINE:motorStop(); motorTurn(LEFT, 180); break; case FOLLOWING_LINE:calculatePID(); motorPIDcontrol(); break; }} 

Code snippet #17Plain text

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command =""; } checkPIDvalues(); mode =STOPPED;

Code snippet #18Plain text

void manualCmd(){ switch (command[0]) { case 'g':mode =FOLLOWING_LINE; break; case 's':motorStop(); //turn off both motors break; case 'f':motorForward(); break; case 'r':motorTurn(RIGHT, 30); motorStop(); break; case 'l':motorTurn(LEFT, 30); motorStop(); break; case 'b':motorBackward(); break; case 'p':Kp =command[2]; break; case 'i':Ki =command[2]; break; case 'd':Kd =command[2]; break; }} 

Github

Схема