roboforum.ru

Алгоритм описывает работу программы, а программа в робототехнике привязана к «железу», поэтому сперва немного о железе и принципах работы. Я не буду углубляться слишком глубоко, будем считать, что читателю знакомо понятие «соревнование по лайнтрейсу».
Итак, нужен датчик, который умеет определять цвет линии перед собой: черный или белый (или оттенки серого). По изменению показаний датчика должно быть понятно, как именно менялся цвет (то есть нужно знать направление градиента) – это нужно для поворота робота в нужную сторону.
Традиционно в качестве таких датчиков используются отражательные оптроны – у них и светодиод, и фототранзистор находятся за окошками, а между ними нет прямой видимости. То есть на фототранзистор свет попадает только отраженным, отсюда и название. От белой поверхности свет отражается хорошо, а от черной – плохо. Теперь понятно, как должна быть устроена схемотехника датчика. Для каждого оптрона нужно два резистора: один для светодиода (будет задавать ток через него), другой в коллектор фототранзистора. Выходной сигнал снимается с коллектора. При нахождении над белым выходное напряжение будет низким (транзистор освещен сильнее, открыт больше), при нахождении над черным выходное напряжение будет высоким (транзистор освещен слабо и открыт чуть-чуть). Манипулируя номиналами резисторов, можно получить уровни напряжения, равные логическим нулю и единице (но это не обязательно). Желательно настроить датчик на максимальный диапазон изменения напряжения.
Сколько нужно датчиков и в каком виде принимать сигнал?
Допустим, у нас новичок в роботостроении, который хочет робота без премудростей (без МК и программ). Тогда ему хватит одного датчика с цифровым выходом.
При старте робота ставим (например) на белое слева от линии. Настраиваем аппаратную логику: с датчика приходит ноль – включить левый мотор, выключить правый; с датчика приходит единица – сделать наоборот. Робот по очереди переключает моторы и медленно едет зигзагом (чтобы не улететь с линии).
Следующим этапом можно поставить два таких датчика: один на белое, другой на черное. Например, левый на белое, правый на черное. Аппаратная логика следующая: левый мотор едет, если левый датчик на белом, правый мотор едет, если правый датчик на черном. Допустим, линия ушла влево. Левый мотор выключится, потому что под левым датчиком стало черное, правый будет поворачивать. Если ехать быстро – можно не успеть повернуть и встать в положение обоих обесточенных моторов.
Можно поставить много таких датчиков, подключить к ним приоритетный шифратор – вероятность потерять линию сильно уменьшится. Такие датчики есть и готовые, количество оптронов на них доходит до 13 (максимум, который я видел в продаже). Но все равно с таким подходом в linetrace не гонять.
Какие преимущества дает аналоговый датчик (то есть напряжение с оптрона оцифровываем АЦП, обычно встроенным)? Можно иметь больше градаций, оттенков серого. Допустим, такой датчик светит на левую границу линии. В световое пятно попадает половина белого цвета и половина черного цвета – с АЦП приходит код примерно из середины диапазона. По изменению этого кода можно понять, куда движется линия (система отсчета привязана к роботу). Но есть нюанс – нельзя терять эту границу, иначе можно попасть на правую границу, а она по коду абсолютно идентична левой, но алгоритм управления для правой границы инверсен по отношению к алгоритму для левой границы. Определить, что был переход на другую границу, невозможно. Вот мы движемся от левой границы к правой: код растет (мы же через черное движемся), а потом падает. А вот мы двигались сначала от левой границы, а потом снова к ней: код растет, а потом падает. Нужно различать границы, а с одним датчиком это невозможно.
Добавляем еще один аналоговый датчик. Размещаем их ровно над границами, с обоих датчиков приходит код. И вот теперь с этим кодом уже можно работать.
Я назвал датчик «вычитательным» неспроста. Будем вычитать коды с датчиков, например из кода от левого датчика будем вычитать код правого датчика. Как будет меняться эта разность? Пусть оба датчика находятся на белом слева от линии, а потом будем двигать датчики к линии, а потом оба уйдут за линию. Сигналы будет следующие:
– оба датчика на белом, два маленьких числа, вычитаем и получаем ноль;
– левый на белом, правый на черном, из маленького вычитаем большое – получаем отрицательное число;
– левый на черном, правый на белом, из большого числа вычитаем маленькое – получаем положительное число;
– оба датчика на белом с другой стороны линии – снова получаем ноль.
С одной стороны кажется, что у датчика есть рабочая область (отрицательная-ноль-положительная) и два нуля по краям. И эти три нуля никак не отличить, поэтому все проблемы цифровых датчиков вычитательному датчику также присущи. Но это не так: ноль посередине получается при вычитании кодов из середины диапазона АЦП, а нули по краям получаются при вычитании кодов из нижней границы диапазона.
Ну вот, вступление заканчивается, пора переходить к алгоритму.
Очень желательно, чтобы информация с датчиков появлялась бы сама, без участия главного цикла. А это на МК можно сделать только с помощью прерываний. Заводим оба сигнала с датчика на АЦП. Используемые переменные:
– байт для кода с левого датчика;
– байт для кода с правого датчика;
– флаг активного датчика;
– байт текущего результата вычитания;
– байт «захвата линии» (если датчики на линии – 0, если датчики слева от линии – 1, если датчики справа от линии – 2).
При инициализации АЦП настраивается на работу с левым датчиком (например), устанавливается флаг активного датчика (левый); разрядности АЦП достаточно 8 бит. Разрешается прерывание по готовности результата. Запускается преобразование. Дальнейшая работа происходит в подпрограмме обработки прерывания.
Когда произошло прерывание, считываем код из АЦП. Затем смотрим, какой датчик был активным – сохраняем результат в нужную ячейку озу. Меняем флаг активного датчика; делаем вычитание, сохраняем результат в озу. После этого нужно проанализировать полученные данные и определиться, где находятся датчики. Делается это следующим образом:
– если оба кода выше границы (о ней далее), то датчики находятся над линией;
– если оба кода ниже границы – датчики вышли за линию, в какую сторону – не известно;
– если один из кодов ниже границы, а другой выше, то робот находится с той стороны линии, с какого датчика код ниже границы;
– устанавливается соответствующий флаг.
Запускаем преобразование и выходим. Разность кодов АЦП у нас будет в дополнительном коде (если старший бит равен единице, то надо число вычесть из нуля – получим его модуль).
Как программа главного цикла будет понимать данные с датчика. Знак говорит о стороне смещения (минус – влево, плюс – вправо). Модуль говорит об величине отклонения. По флагу можно понять, не начали ли мы терять линию, а если начали, то с какой стороны она находится.
Если линия не потеряна, то отклонение от нуля можно использовать как отклонение (ошибка, невязка) для ПИД-регулятора робота. Если линия начала теряться – надо срочно ее найти, пока не потеряли окончательно.
Теперь к вопросу о границах определения положения линии. Их нужно определять экспериментально. Для этого желательно на роботе иметь устройство вывода (индикатор или связь с ПК, проводная или беспроводная). Ставим робота на линию, получаем коды с двух каналов АЦП, двигаем робота, смотрим, назначаем границу.
Оптроны датчика могут иметь разброс (или просто быть разными), соответственно разность кодов будет равна нулю не всегда на середине линии. Если хочется ехать именно посередине, то без калибровки не обойтись. Для этого робота надо поставить ровно на линию, получить разность кодов АЦП. Эту разность вычитаем из нуля, а результат будет поправкой, которую при работе нужно будет прибавлять. Соответственно появится еще одна переменная байтового размера. Но калибровка в данном случае – для эстетов, работать алгоритм будет и без нее.
Теперь прикинем, сколько времени нужно для появления данных. Если тактовую частоту АЦП настроить на 125 кГц и работать в режиме однократного преобразования (это самое медленное), то при этом время одного преобразования составит 200 мкс. Алгоритм не предусматривает сперва измерения с двух каналов, а потом вычитания (в этом нет смысла, робот едет и данные устаревают тут же). Так часто получать данные не обязательно, поэтому есть возможность пропустить поступающие данные через цифровой фильтр. Например такой: данные с АЦП попадают на конвейер (первым зашел – первым вышел) размером в степень двойки (например, восемь). Новое данное задвигается в конвейер, самое старое пропадает. Потом делается среднее арифметическое с округлением: суммируем все ячейки, сдвигаем результат вправо два раза, смотрим на младший бит. Если он равен единице – сдвигаем еще раз вправо и прибавляем единицу; если равен нулю – просто сдвигаем вправо. Глубина фильтра определяется скоростью робота, чтобы реальные повороты трассы не сгладились слишком сильно.
Как это прикинуть? Очень просто. У вас есть мотор с редуктором, у него есть передаточное число, у мотора есть максимальная скорость вращения вала. Допустим, после редуктора скорость вала 100 об/мин. У колеса есть радиус/диаметр – вычисляем длину окружности. Сразу пример: колесо диаметром 42 мм, длина окружности 132 мм; за минуту робот проедет 132*100 = 13200 мм/мин = 220 мм/сек. Получение новой информации от датчика занимает 200 мкс, за это время робот проедет 0.044 мм. Дадим ему возможность проехать 1 мм, это значит в 23 раза больше. Берем ближайшее число степень двойки – это 16. Значит можно организовать два конвейера на 16 элементов, да и делить на 16 можно перестановкой тетрад.
Есть ли смысл увеличивать количество датчиков? Я его не вижу. Линия имеет две границы, поэтому и стоят два датчика. Остальные датчики попадут либо на линию, либо за линию справа и слева. Вычитающий алгоритм тут уже не будет работать. Тут нужно будет оцифровывать все оптроны и строить своеобразный видеокадр. Он будет похож на гистограмму. По этим данным можно выделить полосу линии (но разрешение будет невысоким), в этой линии выделить центр масс. Это уже сложная математика, затратные по времени вычисления, которые грешно помещать в подпрограмму обработки прерывания. Нам нужно в первую очередь получать информацию быстро, а уж потом точность стоит. И чтоб получать ее было просто. Именно поэтому я считаю, что слишком много датчиков – это уже плохо.
Вот такой несложный алгоритм позволит совершенно не думать, как рассчитать свое положение на линии – все сделается само в прерывании. Ничего не будет отвлекать от работы в главном цикле ПИД-регулятора.