Учимся программировать с Роботом Машинкой
Ехаем по линии.
Всё-таки банальный алгоритм следования по линии оказался довольно хорошей задачей для первых знакомств как с роботами, так и с программированием. Довольно простой алгоритм демонстрирует все основные концепты процедурного программирования: последовательные действия, проверка условия, цикличность, потом легко конвертируется в код на языке Си/С++. При этом запуск программы на живом роботе сразу даёт крайне интересный опыт столкновения идеального виртуального алгоритма с неидеальной физической реальностью: контакты на датчике коротят на металлическом уголке - нужно изолировать, черный скотч дает блик и не распознается датчиком - рисуем линию гуашью, линия слишком тонкая и датчик не успевает отреагировать - делаем толще, жесткие пластиковые колеса проскальзывают на поворотах - одеваем силиконовые покрышки, и так далее.
Участники Клуба изобретательства и робототехники ДОСААФ доблестно справились со всеми ожидаемыми и неожиданными проблемами и сейчас машинка катается по линии вполне надежно.
You can watch this video on www.livejournal.com
Робот Машинка едет по линии from 1i7 on Vimeo.
При подготовке этого урока всю главную работу выполнили Александр Теляшев (5а, школа 62): проект трассы, блок-схема алгоритма и программа для робота и Ксения Жукова (лицей 180) - отладка и улучшение алгоритма на живом роботе, доработка конструкции робота и трассы, краш-тест Робота Машинки по падению с метровой высоты.
Для предварительных приготовлений следуюет ознакомиться с материалами уроков Программирование микроконтроллеров для управления роботами и Робот Машинка: из цифровой модели в живого робота.
Постановка задачи
Итак, задача - сделать так, чтобы робот ехал по черной линии, нарисованной на нечерной поверности (белого или другого цвета): ехать прямо по прямой линии и поворачивать на поворотах.
Для того, чтобы робот мог отличать черный цвет от остальных, мы подарим ему зрение в виде специального датчика, например такого (для нашего алгоритма будем использовать два датчика). У датчика всего три провода: питание Vcc (5В), земля GDN и сигнал OUT. Датчик подаёт на сигнальную ножку положительное напряжение (т.е. логическую единицу), если поднести его к поверхности черного цвета, и устанавливает нулевое напряжение (т.е. логический ноль), если поднести его к поверхности любого другого цвета.
Тестовая трасса максимально простая - замкнутая кривая без перекрестков, без специальных маркеров для старта, без специальных маркеров для финиша и без любых других усложняющих элементов - только прямые участки и повороты. Машинка может начать движение с любого участка трассы и будет наматывать круги до тех пор, пока не сядут батарейки.
Алгоритм
Итак, задача поставлена, электроника к роботу подключена, можно приступать к решению. Но перед тем, как начинать писать программу, необходимо продумать, как именно робот будет распознавать разные ситуации на трассе и как он будет на них реагировать, т.е. придумать алгоритм.
В первую очередь, мы должны перечислить возможности, которые есть в распоряжении у нашего робота - дальше мы сможем оперировать ими и только ими. Итак, наш робот умеет:
- Ехать вперед
- Ехать назад
- Останавливаться
- Поворачивать налево
- Поворачивать направо
- Определять наличие линии на левом датчике
- Определять наличие линии на правом датчике
Вот и всё, из этих кубиков мы должны выстроить нужную нам последовательность действий. Для начала сформулируем её словами:
- Если ни левый, ни правый датчик не видят линии (линия между датчиками), едем вперёд.
- Иначе (линия есть под левым или под правым датчиком или сразу под обоими) проверяем, есть ли линия под правым датчиком.
- Если линия есть под правым датчиком, поворачиваем направо.
- Иначе (значит линия под левым датчиком) поворачиваем налево.
- Повторяем всё сначала
Мысленно прокрутите в голове результат поведения Робота Машинки, если он будет следовать предложенной схеме поведения в случае прямой линии и на поворотах налево и направо: вроде ничего не забыли, должно заработать. Стоит отметить, что вариант "линия есть и под левым и под правым датчиком" (например, перекресток или специально отмеченный тупик) нами специально не обрабатывается - в такой ситуации, следуя нашему алгоритму, робот будет просто поворачивать направо, игнорируя значение левого датчика.
Далее для наглядности и для того, чтобы привыкать к стандартам промышленной разработки программного обеспечения, озаботимся снабжением проекта понятной документацией в общепринятом формате. Другими словами, изобразим наше свободное словесное описание алгоритма в виде формальной блок-схемы.
Блок-схема состоит и различных блоков и стрелочек. Стрелки обозначают последовательность переходов между блоками, ход алгоритма можно представить как путешествие между блоками по стрелочкам. В овальных блоках отмечают начало и конец алгоритма. В прямоугольных блоках размещают действия, каждый блок с действием может иметь любое количество входов и только один выход. В ромбовидных блоках размещают проверку условий: если ответ на поставленный вопрос на момент проверки "да" (т.е. условие выполнено), то алготм далее пойдет по ветке с меткой "да", если ответ "нет" (т.е. условие не выполнено), то алгоритм далее пойдет по ветке с меткой "нет"; таким образом осуществится ветвление алгоритма. Блоки с проверкой условий могут иметь любое количество входящих стрелок и только две исходящие стрелки "да" и "нет".
Посмотрим, как предложенный выше алгоритм будет выглядеть в виде блок-схемы:
Как видим, в блоке-схеме есть только один блок "начало" и нет блока "конец". Это соответствует нашему решению, т.к. мы говорили о том, что машинка не должна прекращать работу по своей воле - она будет наматывать круги по трассе до тех пор, пока ее не отключат или у нее не сядет батарейка (для программ, написанных для микроконтроллеров, это типичная ситуация).
В блоки действий попали команды вперёд, поворот направо и поворот налево - это то, как робот может воздействовать на внешний мир. В блоки условий попали проверки на наличие линии на правом датчике и наличие линии на левом датчике - это то, что робот может узнавать о внешнем мире. Все эти возможности были перечислены выше в списке умений нашего робота, значит всё сходится - есть вероятность, что алгоритм заработает.
Из блок-схемы в программу
Теперь посмотрим, каким образом блок-схема из картинки будет превращаться в программу в нашем случае на языке Си/С++, чтобы потом отправиться на микроконтроллер управлять живым роботом.
Блоки действий
команда вперёд
mleft_forward();
mright_forward();
команда назад
mleft_backward();
mright_backward();
команда налево
mleft_backward();
mright_forward();
команда направо
mleft_forward();
mright_backward();
команда стоп
mleft_stop();
mright_stop();
Здесь вызовы mleft_forward, mleft_backward, mleft_stop и mright_forward, mright_backward, mright_stop - определенные нами для удобства подпрограммы, которые позволяют крутить левый и правый мотор в нужном направлении (подробнее смотреть в Программирование микроконтроллеров для управления роботами и Робот Машинка: из цифровой модели в живого робота). В выбранных именах (при желании их можно заменить на любые другие) приставка m означает motor (мотор), left/right - левый/правый мотор, forward/backward/stop - вперед/назад/стоп.
Блоки с условиями в общем случае конвертируются в конструкцию if/else (если/иначе):
if( условие ) {
// действия, если условие выполнено (ветка "да")
...
} else {
// действия, если условие не выполнено (ветка "нет")
...
}
Код для блоков действий для ветки "да" нужно помещать внутри фигурных скобок после if, для ветки "нет" - внутри фигурных скобок после else.
Внутри круглых скобок после ключевого слова if стоит выражение с условием. Выражение с условием должно принимать всего два значения: ПРАВДА или ЛОЖЬ - это базовые значения в двоичной логике, за подробностями обратитесь к булевой алгебре. Если выражение внутри круглых скобок блока if принимает значение ПРАВДА, то в программе отработает код внутри фигурных скобок после блока if (если); если выражение принимает значение ЛОЖЬ, то в программе выполнится код внутри фигурных скобок после блока else (иначе).
Самый простой способ получить булево выражение с условием для блока if/else - это сравнить два значения при помощи специальных операторов, в языке Си/С++ это:
a == b : если a равно b, ПРАВДА, иначе ЛОЖЬ
a != b : если a не равно b, ПРАВДА, иначе ЛОЖЬ
a < b : если a меньше b, ПРАВДА, иначе ЛОЖЬ
a > b : если a больше b, ПРАВДА, иначе ЛОЖЬ
a <= b : если a меньше или равно b, ПРАВДА, иначе ЛОЖЬ
a >= b : если a больше или равно b, ПРАВДА, иначе ЛОЖЬ
Для составления сложных условий из нескольких простых используются специальные булевы операторы (a и b - булевы переменные):
a && b (логическое И): если a ПРАВДА И b ПРАВДА, ПРАВДА, иначе ЛОЖЬ
a || b (логическое ИЛИ): если a ПРАВДА ИЛИ b ПРАВДА, ПРАВДА, иначе ЛОЖЬ
!a (логическое НЕ): если a ЛОЖЬ, ПРАВДА, иначе ЛОЖЬ
Выражения для блоков условий в случае с нашими датчиками будут выглядеть следующим образом:
ПРАВДА, если левый датчик видит линию
( digitalRead(LINE_SENSOR_L) == 1 )
ПРАВДА, если левый датчик не видит линию
( digitalRead(LINE_SENSOR_L) == 0 )
ПРАВДА, если правый датчик видит линию
( digitalRead(LINE_SENSOR_R) == 1 )
ПРАВДА, если правый датчик не видит линию
( digitalRead(LINE_SENSOR_R) == 0 )
Программа для робота
Вся прошивка в одном файле: Robot Car/прошивки/follow_line/follow_line.ino
Вспомогательный код для управления левым и правым моторами, здесь на нем не останавливаемся, подробности смотрите в Программирование микроконтроллеров для управления роботами и Робот Машинка: из цифровой модели в живого робота.
// Робот Машинка едет по линии
// Ножки для моторов
#define MOTOR_LEFT_1 8
#define MOTOR_LEFT_2 9
#define MOTOR_LEFT_EN 11
#define MOTOR_RIGHT_1 4
#define MOTOR_RIGHT_2 3
#define MOTOR_RIGHT_EN 6
void mleft_forward() {
// задать направление
digitalWrite(MOTOR_LEFT_1, HIGH);
digitalWrite(MOTOR_LEFT_2, LOW);
// включить моторы
digitalWrite(MOTOR_LEFT_EN, HIGH);
}
void mleft_backward() {
// задать направление
digitalWrite(MOTOR_LEFT_1, LOW);
digitalWrite(MOTOR_LEFT_2, HIGH);
// включить моторы
digitalWrite(MOTOR_LEFT_EN, HIGH);
}
void mleft_stop() {
// выключить моторы
digitalWrite(MOTOR_LEFT_EN, LOW);
}
void mright_forward() {
// задать направление
digitalWrite(MOTOR_RIGHT_1, HIGH);
digitalWrite(MOTOR_RIGHT_2, LOW);
// включить моторы
digitalWrite(MOTOR_RIGHT_EN, HIGH);
}
void mright_backward() {
// задать направление
digitalWrite(MOTOR_RIGHT_1, LOW);
digitalWrite(MOTOR_RIGHT_2, HIGH);
// включить моторы
digitalWrite(MOTOR_RIGHT_EN, HIGH);
}
void mright_stop() {
// выключить моторы
digitalWrite(MOTOR_RIGHT_EN, LOW);
}
Два датчика линии подключены к 27 (левый) и 28 (правый) ножкам контроллера.
/**
* Сенсор линии: сенсор подключен к входной ножке и
* подает на нее сигнал:
* 1, если сенсор обнаружил линию (черный цвет),
* 0, если сенсор линию не видит (белый цвет).
*/
#define LINE_SENSOR_L 27
#define LINE_SENSOR_R 28
Приготовления контроллера к работе во время запуска: переводим ножки для моторов режим вывода OUTPUT, ножки для датчиков - в режим ввода INPUT.
void setup() {
Serial.begin(9600);
Serial.println("Start Robot Car - the line follower!");
pinMode(MOTOR_LEFT_1, OUTPUT);
pinMode(MOTOR_LEFT_2, OUTPUT);
pinMode(MOTOR_LEFT_EN, OUTPUT);
pinMode(MOTOR_RIGHT_1, OUTPUT);
pinMode(MOTOR_RIGHT_2, OUTPUT);
pinMode(MOTOR_RIGHT_EN, OUTPUT);
// пин сенсора в режим ввода
pinMode(LINE_SENSOR_L, INPUT);
pinMode(LINE_SENSOR_R, INPUT);
// остановить моторы при старте
mleft_stop();
mright_stop();
}
Собственно, весь описанный выше алгоритм в виде программы. Бесконечное выполнение (т.е. цикличность алгоритма) нам обеспечивает специальная подпрограмма loop, которая после завершения выполнения последней своей строчки запускается контроллером вновь и так до тех пор, пока на контроллер подается питание. В остальном, посмотрите, как блок-схема превратилась в код на языке Си/С++ с использованием приведенных выше ключевых блоков кода, самостоятельно.
Обратите внимание, как регулируется минимальный угол поворота робота (с ним машинка гораздо лучше ведет себя на поворотах трассы, это было выяснено уже во время тестов): после команды поворота его продолжительность задается задержкой в миллисекундах (в нашем случае 350 миллисекунд) при помощи системного вызова delay(350).
void loop() {
if( digitalRead(LINE_SENSOR_L) == 0 && digitalRead(LINE_SENSOR_R) == 0 ) {
// линии нет на обоих датчиках
//Serial.println("Proverka linii: linii net na 2x datchikah");
// едем вперед
mleft_forward();
mright_forward();
} else {
// линия есть хотябы на одном из датчиков
Serial.println("linia na odnom is datchikov");
if( digitalRead(LINE_SENSOR_R) == 1 ) {
// линия под правым датчиком
Serial.println("praviy datchik -> povorot napravo");
// ненадолго остановимся, чтобы собраться с мыслями
//mleft_stop();
//mright_stop();
//delay(1000);
// поворачиваем направо
mleft_forward();
mright_backward();
// поворачиваемся 350 миллисекунд, время получено эмпирически
delay(350);
} else {
// линии нет под правым датчиком, значит она под левым датчиком
Serial.println("leviy datchik -> povorot nalevo");
// ненадолго остановимся, чтобы собраться с мыслями
//mleft_stop();
//mright_stop();
//delay(1000);
// повернуть налево
mleft_backward();
mright_forward();
// поворачиваемся 350 миллисекунд, время получено эмпирически
delay(350);
}
}
}
Разные фотки
исходники занятия, подсветка синтаксиса