Сергей Коба

Сергей Коба

Веб, блокчейн, мобильная разработка и интернет вещей

Веб тимлид в MobiDev. Цель: изучать и учить чему-то новому без остановки. Основные языки: PHP и Ruby. Также интересно: блокчейн, мобильная разработка, IoT и DevOps. Жизненное кредо: я жив, пока я учу что-то новое.

Строим WiFi робота c камерой (Часть 2 - Первая поездка)

30 июня 2017 11:00

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

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

Пришло время взяться за обучение Pluto по-серьезному! Вот список комманд, которые мы реализуем в ближайшее время:

  • ехать вперед;
  • ехать назад;
  • поворот влево;
  • поворот вправо;
  • остановка;
  • изменение скорости.

Если вы еще недостаточно вдохновлены этим перечнем, то спешу сообщить, что пришла пора придать нашему роботу его первым облик. А именно установить его разрозненные компоненты на платформу. Начнем с моторов и колес...

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

Далее, не особо выбирая размещение, прикрутим Arduino и драйвер моторов, не забудьте оставить место для аккумуляторов. Я уверен, что в дальнейшем Вы или точнее Мы еще ни раз поменяем расположение компонент. Не знаю как повезло Вам, но нам китайцы прислали в наборе такую платформу с отверстиями, что ни один компонент не удалось установить на нее параллельно краю. Поэтому не особо удивляйтесь, что Arduino и драйвер моторов стоят под углом.

Все готово к тому, чтобы продолжить апгрейд "мозга" Pluto. Модифицируем его программу таким образом, чтобы мы могли посылать команды в последовательный порт (COM-порт), а робот "слушал" порт и выполнял поступающие команды.

#define MotorLeftSpeedPin    5  // Левый (А) мотор СКОРОСТЬ — ENA
#define MotorLeftForwardPin 11 // Левый (А) мотор ВПЕРЕД — IN1
#define MotorLeftBackPin 9 // Левый (А) мотор НАЗАД — IN2
#define MotorRightForwardPin 8 // Правый (В) мотор ВПЕРЕД — IN3
#define MotorRightBackPin 7 // Правый (В) мотор НАЗАД — IN4
#define MotorRightSpeedPin 6 // Правый (В) мотор СКОРОСТЬ — ENB
/* Состояния робота */
#define FORWARD 0 /* Едем вперед */
#define BACKWARD 1 /* Едем назад */
#define LEFT 2 /* Поворот влево */
#define RIGHT 3 /* Поворот вправо */
#define STOP 4 /* Остановка */
void setup() {
pinMode(MotorLeftForwardPin, OUTPUT);
pinMode(MotorLeftBackPin, OUTPUT);
pinMode(MotorLeftSpeedPin, OUTPUT);
pinMode(MotorRightForwardPin, OUTPUT);
pinMode(MotorRightBackPin, OUTPUT);
pinMode(MotorRightSpeedPin, OUTPUT);
}
void loop() {
if (Serial.available() > 0) {
handleCommand(Serial.readString());
}
}
/*
* Список команд:
* '0' - Остановить моторы
* '1,dir' - Запустить моторы
* '2,speed' - Установить скорость
*/
void handleCommand(String cmd) {
String params[10];
char delimeter = ',';
int paramsCount = 0;
int dir = 0;
int speed = 0;
/* Разбиваем стркоу с командой на массив */
if(cmd.indexOf(delimeter) > 0) {
while(cmd.indexOf(delimeter) > 0) {
params[paramsCount] = cmd.substring(0, cmd.indexOf(delimeter));
paramsCount++;
cmd = cmd.substring(cmd.indexOf(delimeter)+1, cmd.length()+1);
}
params[paramsCount] = cmd;
paramsCount++;
} else {
params[paramsCount] = cmd;
paramsCount++;
}
switch(params[0].toInt()){
case 0:
/* Остановить моторы */
break;
case 1:
dir = params[1].toInt();
/* Запустить моторы, dir -указывает направление вращения */
break;
case 2:
speed = params[1].toInt();
/* Установить скорость вращения speed */
break;
}
}

В цикле loop мы проверяем наличие данных в Serial порте (Serial.available() > 0). В случае положительного ответа читаем строку (Serial.readString()) и передаем ее в функцию handleCommand, которая обрабатывает строку с командой. Для тестирования команд можно использовать инструмент Arduino IDE под названием Serial Monitor. На скриншоте ниже мы посылаем роботу команду "2,160", которая будет означать установить скорость вращения колес равной 160 ("0" - остановить моторы, "1,0" - ехать вперед и т.д.).

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

/* Остановить моторы */
digitalWrite(MotorRightForwardPin, LOW);
digitalWrite(MotorLeftForwardPin, LOW);
digitalWrite(MotorRightBackPin, LOW);
digitalWrite(MotorLeftBackPin, LOW);
/* Запустить моторы, dir -указывает направление вращения */
dir = params[1].toInt();
switch(dir) {
case FORWARD:
digitalWrite(MotorRightForwardPin, HIGH);
digitalWrite(MotorLeftForwardPin, HIGH);
digitalWrite(MotorRightBackPin, LOW);
digitalWrite(MotorLeftBackPin, LOW);
break;
case BACKWARD:
digitalWrite(MotorRightForwardPin, LOW);
digitalWrite(MotorLeftForwardPin, LOW);
digitalWrite(MotorRightBackPin, HIGH);
digitalWrite(MotorLeftBackPin, HIGH);
break;
case LEFT:
digitalWrite(MotorRightForwardPin, HIGH);
digitalWrite(MotorLeftForwardPin, LOW);
digitalWrite(MotorRightBackPin, LOW);
digitalWrite(MotorLeftBackPin, LOW);
break;
case RIGHT:
digitalWrite(MotorRightForwardPin, LOW);
digitalWrite(MotorLeftForwardPin, HIGH);
digitalWrite(MotorRightBackPin, LOW);
digitalWrite(MotorLeftBackPin, LOW);
break;
}
/* Установить скорость вращения speed */
speed = params[1].toInt();
analogWrite(MotorRightSpeedPin, speed);
analogWrite(MotorLeftSpeedPin, speed);

Когда я попробовал данную сборку робота в реальных условиях, то обнаружил ожидаемый баг. Робот не может ехать прямо вперед и прямо назад. Его неизбежно косит влево или вправо. Этот эффект возникает из-за того, что китайские моторы крутяться с разной скоростью, даже если на них подать одинаковое напряжение (просто моторы у китайцев вышли разные :) ).

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

Решить эту задачку без дополнительного оборудования не представляется возможным. Нам нужно каким-то образом знать, с какой скорость крутиться колесо в данный момент, чтобы реагировать на изменяющуюся ситуацию. Для этого мы будем использовать оптические энкодеры B83609

Это специальные модули для измерения скорости и самое радостное, что на платформе есть под них специальные вырезы, которые даже подходят ;) Устанавливаем их на платформу и подсоединяем к Arduino Shield следующим образом:

ЦветНазначениеArduino Pin (контакт)
КоричневыйСигнал от правого энкодера (OUT на энкодере)2 (S) 
БелыйПитание для правого энкодера (5V на энкодере)2 (V)
ЧерныйЗемля для правого энкодера (GND на энкодере)2 (G)
ФиолетовыйСигнал от левого энкодера (OUT на энкодере)3 (S) 
ЗеленыйПитание для левого энкодера (5V на энкодере)3 (V)
СинийЗемля для левого энкодера (GND на энкодере)3 (G)

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

Код обработки прерываний от энкодеров может выглядеть так:

#define LeftMotorSpeedSensorPin  3  /* пин левого энкодера */
#define RightMotorSpeedSensorPin 2 /* пин правого энкодера */
void handleLeftSpeedSensor() {
/* обработка прерывания от левого энкодера */
}
void handleRightSpeedSensor() {
/* обработка прерывания от правого энкодера */
}
void setup() {
pinMode(LeftMotorSpeedSensorPin, INPUT_PULLUP);
pinMode(RightMotorSpeedSensorPin, INPUT_PULLUP);
/* назначаем обработчики прерываний от энкодеров */
attachInterrupt(digitalPinToInterrupt(LeftMotorSpeedSensorPin), handleLeftSpeedSensor, RISING);
attachInterrupt(digitalPinToInterrupt(RightMotorSpeedSensorPin), handleRightSpeedSensor, RISING);
}
void loop() {
if (Serial.available() > 0) {
handleCommand(Serial.readString());
}
}

Кажется "счастье" близко и нам остается лишь непрерывно измерять фактическую скорость каждого колеса и синхронизировать моторы с помощью Arduino. Для этой цели как нельзя лучше подходит алгоритм с обратной связью под названием PID. Даже есть готовая реализация этого алгоритма для Arduino. Идея заключается в том, что на вход алгоритма приходит желаемая скорость вращения мотора (например 4 оборота в секунду), а также текущая фактическа скорость вращения, измеренная с помощью энкодеров. На выходе мы должны получить значение от 0 до 255, которое мы посылаем драйверу моторов для управления скоростью. Данный алгоритм является адаптивным и если мотор крутится медленнее, чем надо, то увеличивает значение на выходе и наоборот.

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

Не буду тянуть кота за хвост, мне так и не удалось с помощью PID алгоритма заставить Pluto стабильно ехать вперед. Я связал это со следующими причинами: первая - очень трудно подобрать коэффициенты для алгоритма, при этом робот слишком медленно реагирует на изменяющуюся ситуацию и в итоге вместо поездки прямо мы получаем зигзаго образное движения в заданном направлении, если повезет; вторая - дешевые колеса с китайской резиной иногда проскальзывают по гладкому полу и энкодеры регистрируют это как увеличение скорости (без фактического продвижения), что вводит алгоритм в заблуждение, что одно из колес провернулось слишком много и надо компенсировать это.

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

/* Количество прерываний, необходимое для совершения поворота */
#define TurnHolesCount 40
/* Возможные состояние робота */
#define FORWARD 0
#define BACKWARD 1
#define LEFT 2
#define RIGHT 3
#define STOP 4
volatile int state = 0; /* Текущее состояние */
volatile int currentSpeed = 100; /* Текущая скорость моторов */
volatile int course = 0; /* Разница между количеством прерываний от левог ои правого энкодеров */

void handleLeftSpeedSensor() {
if (state == STOP) {
return;
} else {
course--;
}
switch(state) {
case FORWARD:
if(course > 0) { rotateLeft(true); } else { rotateRight(true); }
break;
case BACKWARD:
if(course > 0) { rotateLeft(false); } else { rotateRight(false); }
break;
case LEFT:
if (abs(course) >= TurnHolesCount) {
motorsStop();
}
break;
}
}
void handleRightSpeedSensor() {
if (state == STOP) {
return;
} else {
course++;
}
switch(state) {
case FORWARD:
if(course < 0) { rotateRight(true); } else { rotateLeft(true); }
break;
case BACKWARD:
if(course < 0) { rotateRight(false); } else { rotateLeft(false); }
break;
case RIGHT:
if (abs(course) >= TurnHolesCount) {
motorsStop();
}
break;
}
}

По сути данный код считает прерывания от колес, и как только одно из колес прокрутилось больше, чем другое, он его останавливает и вращает противоположное колесо. Т.е. на самом деле колеса не вращаются непрерывно, а то оставливаются, то вновь запускаются. Из-за большой скорости вычислений этот эффект практически не заметен. Также из-за того, что колеса вращаются лишь малую долю времени они не успевают проскальзывать по скользкому полу. Как ни странно именно этот простой алгоритм позволил Pluto наконец-то стабильно ездить вперед и назад.
Функции rotateRight и rotateLeft запускают соответственно вращение левого или правого моторов. Параметр булевского типа указывает направление вращения: true - вперед, false - назад. Также данный код использует так называемый конечный автомат или машину состояний. Значение текущего состояния робота хранится в переменной state. Такой подход позволяет нам переводить Плуто в различные состояния (движение вперед, назад, поворот и т.д.) путем изменения одной переменной. Вот как это происходит в функции handleCommand:

void handleCommand(String cmd) {
/* ... см код в начале статьи ... */
switch(params[0].toInt()){
case 0:
state = STOP;
motorsStop(); /* остановка моторов */
break;
case 1:
dir = params[1].toInt();
motorsStop();
state = dir;
motorsStart(dir); /* включаем вращение моторов */
break;
case 2:
motorsSetSpeed(params[1].toInt()); /* устанавливаем скорость */
break;
}
}

Полный рабочий код вы можете найти здесь.

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

Все статьи о Плуто доступны по следующим ссылкам:

Строим WiFi робота c камерой (Часть 1 - Ходовая)

Строим WiFi робота c камерой (Часть 2 - Первая поездка)

Строим WiFi робота c камерой (Часть 3 - Веб сервер)

Строим WiFi робота c камерой (Часть 4 - Камера)

Назад