Продолжение материала, начало можно найти здесь по ссылке и прочитать о сборке робота. Кажется, это уже третья статья.
Начнем со структурной схемы системы управления роботом для решения задачи движения по линии
Аналого-цифровой преобразователь (АЦП) считывает показания датчика, затем они нормируются, и после этого высчитывается ошибка позиционирования. Ошибкой считается разность между текущими показаниями датчиков и показаниями когда робот установлен строго по центру линии. Далее регулятор на основе ошибки вычисляет управляющие переменные, которые являются коэффициентами заполнения широтно-импульсной модуляции (ШИМ) для каждого из двигателей. Коэффициенты ШИМ используются для регулирования скорости двигателей, они поступают на вход модулятора, откуда уже модулированный сигнал подается на линии Pwm1 и Pwm2 драйвера двигателей. Рассмотрим поподробнее некоторые моменты.
Инициализация.
В ходе ее выполнения очищается ОЗУ и все регистры общего назначения, инициализируется указатель стека. Затем производится настройка портов ввода-вывода: порт B настраивается полностью на вывод, так как к нему подключены светодиоды датчика линии, а порт D частично на ввод (линия 7) и частично на вывод (линии 2-6). Далее инициализируются периферийные устройства. Поскольку АЦП в выбранном контроллере 10-битный, то есть возможность выбора выравнивания по левому (младшие 2 бита в ADCL, старшие 8 в ADCH) и правому (старшие 2 бита в ADCH, младшие 8 в ADCL) краю.
В программе используется выравнивание значений показаний по левому краю, так как это позволяет упростить расчеты. АЦП контроллера после каждого измерения помещает результат в регистровую пару ADCH:ADCL. Для АЦП устанавливается значение опорного напряжения равное напряжению питания контроллера (2,56 В) и выравнивание показаний по левому краю. Затем инициализируется модуль приемопередатчика UART. После инициализации, контроллер считывает из памяти ЭСППЗУ (EEPROM) параметры регулятора. Параметры регулятора в данном случае это – пропорциональный (P), интегральный (I) и дифференциальный (D) коэффициенты, а также значение базового коэффициента заполнения ШИМ двигателей (PWM Base). Они хранятся в ОЗУ (SRAM) в массиве sPidParams.
Затем контроллер проверяет необходимость запуска режима отладки . Выбор данного режима задается установкой высокого логического уровня на линии 0 порта ввода-вывода C. В случае запуска режима отладки выполняется процедура Monitor. После ее завершения, контроллер возвращается на процедуру загрузки параметров регулятора из ЭСППЗУ . Это сделано для обновления параметров в памяти ОЗУ. После этого производится выбор режима запуска.
Сбор данных с датчиков.
Сбор данных с датчиков осуществляется двумя подпрограммами: Get_line – для получения данных с датчика линии и Get_distance – для получения данных с датчика препятствий. При опросе датчика линии сначала происходит сброс текущего номера канала АЦП. Затем включается светодиод, соответствующий текущему номеру канала АЦП. После этого обновляется значение регистра выбора канала АЦП ADMUX, оно вычисляется путем выполнения логической операции ИЛИ номера текущего канала с магической константой 0xE0. Эта константа определяет значение уставки (2,56 В) АЦП и выравнивания результата по левому краю.
После переключения канала, АЦП запускается в режиме одиночного преобразования. Сброс флага ADSC в регистре ADSRA сигнализирует об окончании преобразования. Дождавшись сброса этого флага, контроллер считывает значение показаний из регистра ADCH. С целью уменьшения влияния помех и упрощения вычислений регистр ADCL, содержащий два младших бита показаний не используется. Измерение повторяется 8 раз, после чего вычисляется среднее значение всех измерений и нормируется. Таким образом, реализуется программное повышение точности измерений. Полученное значение сохраняется в массив sLine в ОЗУ (SRAM). Затем номер текущего канала АЦП увеличивается. После сравнения алгоритм повторяется, если количество итераций не достигло 7 (оно ограничено количеством светочувствительных элементов в датчике линии).
Таким образом, после выполнения процедуры Get_line, показания датчика линии будут храниться в массиве sLine в ОЗУ. Сбор данных с датчика препятствия инициализируется выводом логической единицы на 6 линию порта D микроконтроллера. Для того чтобы датчик произвел измерение, необходимо в течение 10 мс удерживать его линию Trig в состоянии высокого логического уровня. После этого линия 6 порта D контроллера переводится в состояние низкого логического уровня.
Датчик производит измерение и переводит линию Echo в состояние высокого логического уровня. Длительность удержания линии Echo в этом состоянии прямо пропорционально зависит от расстояния до обнаруженного препятствия. Контроллер измеряет ее и затем пересчитывает расстояние в миллиметры. Результат измерения хранится в массиве sDistance в ОЗУ (SRAM).
Алгоритм нахождения отклонения
E был реализован методом центра масс. В соответствии с этим методом положение робота находится по формуле
Вычитая из текущего положения робота его положение при котором, он находится строго по центру линии и выполнив нормирование, можно получить искомое отклонение E.
Управление двигателями.
ШИМ двигателей реализован программным путем в процедуре PWM_drive. Программная реализация ШИМ позволяет подключать двигатели к любым свободным линиям ввода-вывода. В данном случае это линии 2 и 5 порта D. Параметры ШИМ состоят из трех значений (массив sMotors в ОЗУ). Младшие два байта отвечают за коэффициенты заполнения ШИМ правого и левого каналов, а старший байт используется как регистр сравнения. Значение регистра сравнения циклически изменяется от 0 до 255 с шагом 1. Если значение левого или правого коэффициентов ШИМ меньше (больше) регистра сравнения, то в линию порта D, подключенную к соответствующему двигателю, выводится высокий (низкий) логический уровень напряжения.
Ну а далее управление реализуется согласной выбранному закону регулирования (релейный, П, ПД, ПИД) – все данные готовы. Целью в данном случае было реализовать не сам алгоритм управления, а предоставить обучающемуся набор базовых функций, которые в дальнейшем упростят решение типичных задач (в данном случае движение по линии и объезд препятствий). Приведу видео прохождения трассы (взята с одной из тем на Roboforum'e) с П-регулятором. Скорость установлена небольшая чтобы робот не улетел со стола :) В дальнейшем можно реализовать ПД-регулятор (интегральная составляющая тут не столь необходима) и проводить тесты на скорость :)
В заключение, хотелось бы, чтобы данный цикл статей рассматривался читателями не как инструкция по сборке, а как пример того, что любой ваш робот, скрученный и спаянный вами пусть даже на коленке – это прежде всего результат долгого творческого поиска, в чем я и желаю всем успехов :)