Пример управляющей программы для мобильной платформы для соревнований РобоКодер?
1. Параллельные процессы и реальное время.
Рассмотрим набор задач соревнований РобоКодер? на примере типовой мобильной платформы компании "Техновижн" с распределенной системой управления на базе контроллеров ML2313?-485 и PS-M64-V02.
Каждая из этих задач может быть разбита на подзадачи/процессы, причем многие из подзадач будут универсальными, т.е. входить во все задачи. Например:
- проезд на заданное расстояние по прямой
- остановиться при срабатывании любого ИК датчика препятствий
- при проезде белой линии скорректировать текущие координаты тележки
- при обнаружении препятствия по ультразвуковому датчику на расстоянии менее 1 метра начать маневр «Объезд препятствия»
Характерные черты этих подзадач – параллельность и работа в реальном времени, т.е. решаться они должны одновременно и в пределах заданного интервала времени. А аппаратные средства, на базе которых должны быть решены эти задачи, т.е. процессоры – устройства последовательные, выполняющие каждый момент одно действие. Значит, необходимо так выстроить программы, реализующие эти процессы, чтобы на последовательном устройстве они работали тем не менее параллельно. В принципе это обеспечивается при использовании операционных систем реального времени, но в данной статье мы покажем пример реализации достаточно простой системы, обеспечивающей параллельное выполнение, на примере которой можно разобраться, что это такое – параллельное выполнение, реальное время и как разрабатывать алгоритмы, работающие в реальном времени. Тем более что «настоящие» ОС РВ – вещь сложная и специфическая. Достаточно сказать, что ни Linux, ни тем более Windows ОС РВ не являются.
Для параллельности алгоритм решения задач должен строиться как автомат, переходящий из состояния в состояние по определенным условиям, который на каждом переходе выполняет определенные действия. Тогда общий алгоритм работы будет представлять циклическую проверку наборов условий для определенных состояний, выполнение определенных действий и переход к новым состояниям.
В этой статье мы рассмотрим примеры решений и соответствующие программы, дающие некоторый базовый набор средств для решения задач соревнований.
В приложении дается полный текст программы Монитор.
Приводимые решения и программы не являются идеальными, готовыми и полными, и скорее дают новичкам начальную информацию для собственного поиска и разработок.
2. Пример реализации через набор состояний задачи «Выезд на белую линию разметки»
Задача «Выезд на белую линию разметки», может быть представлена набором состояний:
- исходное состояние 1 «Не на линии»
- состояние 2 «Включены оба двигателя»
- состояние 3 «На линии левый датчик»
- состояние 4 «На линии правый датчик»
- состояние 5 «На линии оба датчика»
и реализована циклическим выполнением следующих действий:
- В состоянии 1 ( С1 )включаем оба двигателя и переходим в С2
- В С2, если сработал левый датчик линии, выключаем левый двигатель и переходим в С3
- В С2, если сработал правый датчик линии, выключаем правый двигатель и переходим в С4
- В С3, если сработал правый датчик линии, выключаем правый двигатель и переходим в С5
- В С4, если сработал левый датчик линии, выключаем левый двигатель и переходим в С5
Теперь, допустим, что у нас есть сирена и старт и финиш мы хотим сопроводить сигналом длительностью 1 сек. Тогда добавляются состояния:
- З1 – сирена включена
- З2 – сирена выключена
- С6 – линию нашли и сигналим
- С7 – линию нашли и сигнал закончили
и цикл дополнится:
- В С1 включаем оба двигателя, включаем сирену, включаем отсчет 1 сек. и переходим в С2 и З1
- В С2, если сработал левый датчик линии, выключаем левый двигатель и переходим в С3
- В С2, если сработал правый датчик линии, выключаем правый двигатель и переходим в С4
- В С3, если сработал правый датчик линии, выключаем правый двигатель и переходим в С5
- В С4, если сработал левый датчик линии, выключаем левый двигатель и переходим в С5
- В З1, если кончился отсчет 1 сек, сирену выключаем и переходим в З2
- В С5, включаем отсчет 1 сек. и переходим в С6 и З1
- В С6, если З1 и кончился отсчет 1 сек, сирену выключаем и переходим в С7 и З2
Как видите, у нас получились два параллельных процесса –
ищем линию и сигналим. Некоторые состояния лишние и схему можно упростить, это не суть важно, важно то, что мы в этот цикл можем добавить еще много процессов со своими наборами состояний/действий и все они будут в каком-то приближении выполняться параллельно. Основными критериями должны быть два времени:
- время выполнения этого цикла
- минимальное из допустимых времен задержки действий
Например, пусть двигатель надо выключать не более чем через 10 мс после обнаружения линии (иначе платформа с нее съедет). А время выполнения цикла – 200 микросекунд. Тогда в наш цикл мы еще можем добавить в 50 раз больше действий, не нарушая параллельности процессов.
Вот часть программы, выполняющая «Выезд на белую линию разметки»:
if(mode_send_cmd==5) // Если оператор выбрал тест «Выезд на линию»
{
if(state==1) // В состоянии 1 включаем оба двигателя
{
copy_str_with_tx(&buffer_cmd[ptr_in_cmd],cmd_forward_80);
state=2;
}
if(state==2) // В состоянии 2 ожидаем срабатывания датчиков линии
{
if((PINA&1)==0)
{
// Левый двигатель - СТОП
copy_str_with_tx(&buffer_cmd[ptr_in_cmd],cmd_stop1);
state=3;
}
if((PINA&2)==0)
{
// Правый двигатель - стоп
copy_str_with_tx(&buffer_cmd[ptr_in_cmd],cmd_stop2);
state=4;
}
}
if(state==3) // В состоянии 3 ожидаем срабатывания правого датчика линии
{
if((PINA&2)==0)
{
// Правый двигатель - стоп
copy_str_with_tx(&buffer_cmd[ptr_in_cmd],cmd_stop2);
state=5;
}
}
if(state==4) // В состоянии 4 ожидаем срабатывания левого датчика линии
{
if((PINA&1)==0)
{
// Левый двигатель - СТОП
copy_str_with_tx(&buffer_cmd[ptr_in_cmd],cmd_stop1);
state=5;
}
}
}
3. Управление памятью.
В «настоящих» ОС РВ (как и в обычных многозадачных и многопользовательских) немаловажная часть – управление памятью. Память выделяется, память освобождается, на нее стоят в очереди процессы, убирается «мусор». Для небольших процессоров типа однокристалок Atmel можно от этого отказаться и память распределять статически, как глобальные переменные, это сильно упрощает разработку. А если памяти не хватит – проще поставить более мощный процессор.
При необходимости можно одну и ту же память использовать для «непересекающихся» задач. Например, в нашем случае задача «Прием файла по Х-модему» использует:
int crctable[256];
char ReceiverBuffer[128];
Поскольку когда она работает, то остальное все не работает и наоборот, то эти же массивы можно использовать в других подпрограммах, преобразовав их в указатели.
Второй важный аспект – совместное использование переменных, в настоящих ОС РВ применяют флаги, семафоры, права на запись/чтение, в простых системах обычно достаточно просто выстроить алгоритмы так, чтобы процессы одновременно не могли записывать в одни и те же переменные.
4. Передача команд в контроллер двигателя КД.
У нас распределенная система управления, по сути – сеть из двух контроллеров. Поскольку скорость последовательного канала выбрана 9600, а команда занимает около 10 байт, то на передачу команды затрачивается около 10 миллисекунд, что очень много, поэтому команды надо передавать обязательно параллельно и по прерыванию от последовательного канала. Программа-источник записывает команду в кольцевой буфер по указателю записи, а в прерывании передачи последовательного канала выбирается следующий байт из буфера по указателю чтения. В принципе необходимо контролировать «переполнение буфера» и обрабатывать эту ошибку, но если алгоритм спроектирован так, что темп поступления команд гарантированно ниже, чем темп их вывода, то можно и не контролировать.
В нашей программе, например, команды для КД «порождаются» процессами, «вписанными» в 100-миллисекундное прерывание, не более 1-й команды за 100мс тик, поэтому буфер команд гарантированно будет пуст к началу следующего тика.
Пример программы, посылающей команду на вращение обоих двигателей:
Заготовка команды:
const char cmd_forward_80[]=":M0PF8000"; // оба двигателя вперед без стабилизации скорости
Запись команды в буфер команд:
copy_str(&buffer_cmd[ptr_in_cmd],cmd_forward_80); // Копируем команду в буфер
ptr_in_cmd=ptr_in_cmd+10;
if(ptr_in_cmd>=250) ptr_in_cmd=0;
if(flag_tx1==0) UDR1 = 0; // Активируем прерывание передатчика
Выдача команд в последовательный канал в прерывании передатчика:
void interrupt_tx1(void)
{
SREG |= 0x80; // Разрешаем прерывания
if(ptr_in_cmd!=ptr_out_cmd) // Проверяем наличие команды
{
UDR1 = buffer_cmd[ptr_out_cmd]; // Выбираем из буфера следующий байт
// и передаем его
++ptr_out_cmd; // Инкрементируем указатель кольцевого буфера
if(ptr_out_cmd==250) ptr_out_cmd=0;
flag_tx1=1;
}
else flag_tx1=0;
}
5. Планирование времени. Таймеры.
Все процессы должны быть распланированы по времени, если действие/реакция должны быть выполнены за определенное время, то система должна гарантировать это.
Самые «неотложные» действия должны выполняться по прерыванию. Например, передача байта команды в последовательный канал, обнаружение прихода импульса с энкодера.
Следующий уровень – действия, которые надо выполнять каждый временной интервал - тик. Для наших задач выбран тик 100 мс, и в таймерном прерывании 100 мс каждый раз выполняются требуемые действия, например – переходы по состояниям/действия из п.2. В принципе можно дополнить и другими временными интервалами. Следующий уровень – фоновые процессы, которые работают в «главном цикле». Для них нет жестких ограничений по времени. В программе монитор, например, это меню, вывод телеметрии.
Важный элемент любого процесса – временные задержки и timeout. Реализуются они следующим образом:
- выделяется переменная timer
- процесс при необходимости взводит timer, записывая в переменную требуемое кол-во тиков 100 мс и переходит в состояние С1
- в прерывании таймера 100 мс все ненулевые таймеры декрементируются
- в состоянии С1 процесс проверяет – timer досчитал до нуля? Если да, то переходит в состояние С2
Таким образом, например, следующая программа включает выход PORTA_0 на 1 секунду:
if(state==4) { PORTA=PORTA | 1; timer=10; state=1; }
if(state==1) if(timer==0) { PORTA=PORTA &0xfe; state=2; }
и в прерывании таймера 100 мс:
if(timer!=0) –-timer;
6. Датчики и исполнительные механизмы.
Работу любой системы управления можно представить в виде циклического выполнения следующих действий:
- получение информации от датчиков
- обработка и выработка решений по управляющим воздействиям
- вывод управляющих воздействия на исполнительные механизмы
Для типовой мобильной платформы:
Датчики:
- четыре датчика белой линии
- четыре ИК датчика препятствий
- ультразвуковой измеритель расстояний до препятствий
- сигналы с энкодеров обоих колес
Исполнительные механизмы:
- два двигателя, вращающие колеса
Подключение датчиков к типовой платформе:
PORTA_0 –
PORTA_1 –
PORTA_2 –
PORTA_3 –
PORTA_4 –
PORTA_5 –
PORTA_6 –
PORTA_7 –
ADC_0 - УЗ датчик
PORTE_4 – первый дачик энкодера левого колеса
PORTE_5 – второй дачик энкодера левого колеса
PORTE_6 – первый дачик энкодера правого колеса
PORTE_7 – второй дачик энкодера правого колеса
7.1. Дискретные входы/выходы.
Первые 8 датчиков – дискретные, т.е. они дают либо 0, либо 1, в любой момент времени мы можем считать их значение с соответствующего бита соответствующего порта процессора, например:
if(PORTA&1) stop_motor();
если на бит 0 порта А заведен датчик препятствий и при срабатывании он выдает 1, то эта команда выключит двигатели при приближении к препятствию.
7.2. Аналоговые входы.
УЗ датчик выдает напряжение, пропорциональное расстоянию до препятствия, поэтому он подключается к аналоговому входу ADC_0. В АЦП включено в режиме прерывания с автозапуском, п/п обработки прерывания:
void interrupt_adc(void)
{
int i;
i=ADCL;
adc_data =(ADCH<<8) | i;
}
и в переменной adc_data в любой момент доступно значение от 0 до 1023, которое легко пересчитать в расстояние в соответствии с характеристикой датчика SICK UM30?-13113.
7.3. Подсчет импульсов энкодеров.
Система датчиков энкодеров обеспечивает разную очередность импульсов при разных направлениях вращения колес. Поэтому можно поступить следующим образом ( рассматриваем один энкодер ):
- один датчик заводится на прерывание
- в перывании считываем значение со второго датчика, если 0 – импульс прибавляем, если 1 – импульс отнимаем, например:
void interrupt_int4(void)
{
int i,j;
SREG |= 0x80; // Разрешаем прерывания
i=PINE&0x20; // Считыванием значение второго датчика
if(i!=0) ++pos_left; // Определяем направление вращение и пересчитываем
else --pos_left; // условную координату
}
и в переменной pos_left будет число, соответствующее текущему расстоянию (координате ), пройденному колесом. При необходимости оно пересчитывается, например, в миллиметры, зная диаметр колеса и то, что на один оборот энкодер выдает 120 импульсов.
Этот алгоритм можно улучшить, так как он, например, может давать лишний подсчитанный импульс при реверсах.
8. ЧПУ программы и их исполнение.
В ряде систем управления применяются специализированные языки управления, это, например, языки программирования станков с ЧПУ, тот же RoboBasic? для управления роботами и многие другие. Все они основаны на создании языка, удобного для описания/управления объектом управления, а команды этого языка обычно исполняются специальной программой, называемой интерпретатором.
В Мониторе есть блок, который позволяет написать простейшую программу из команд КД ML2313?-485 (см. описание команд КД) плюс две служебные команды:
Завершить выполнение ЧПУ программы:
=S
Продолжить выполнение программы с начала
=R
Каждая команда КД дополнена полем: временная пауза после выдачи этой команды, в тиках 100 мс. И тогда ЧПУ программа:
:M0PF8000? 050
:M0SF0000? 100
=R
обеспечит непрерывное выполнение последовательности действий:
- вращение обоих колес на половинной мощности в течении 5 секунд
- останов колес на 10 секунд
А если R заменить на S, то однократное выполнение этой последовательности.
ЧПУ программа может включать в себя комментарии после точки с запятой. Синтаксический/семантический контроль команд не проводится.
9. Некоторые полезные подпрограммы.
ЧПУ программу удобно готовить в текстовом редакторе на РС, поэтому в Монитор включены блоки загрузки файлов по X-modem и записи загруженной программы в ЕЕПРОМ. ( Таким образом размер ЧПУ программы ограничен размером ЕЕПРОМ, это около 200 команд ).
9.1. Запись/чтение ЕЕПРОМ
В 64-й однокристалке объем ЕЕПРОМ – 2 Кбайт. П/п записи/чтения байта обеспечивают запись байта по указанному адресу и чтение байта по указанному адресу.
9.2. Передача файлов по x-modem и запись в ЕЕПРОМ
П/п XModemReceive? Монитора обеспечивает прием заданного количества блоков по 128 байт по протоколу x-modem, что обеспечивает загрузку файлов с компьютера в любой терминальной программе, поддерживающей этот протокол, например Hiper Terminal.
10. Общее описание программы монитор.
Ведущий контроллер PS-M64-V02 подключается к РС с помощью конвертера 485-232. На РС необходимо запустить программу Hiper Terminal с настройками:
9600, 8 бит, 1 стоповый, без контроля четности, без управления потоком,
эмуляция ANSI терминала.
После включения питания тележки программа Монитор выводит меню:
R-control. 2006. v.1.3
Menu.
1. Manual command input.
2. Run Robo program.
3. Telemetry.
4. Count encoder.
5. Go to line.
6. Load Robo program.
7. Print Robo program.
=>
Нажатием 1-7 можно выбрать любой режим работы/тест.
10.1. Manual command input.
========================================================================
Набираемые символы транслируются по 485-у интерфейсу в ML2313?-485.
10.2. Run Robo program.
========================================================================
Запускается на исполнение ЧПУ программа. На дисплей выводится значение таймера команд.
10.3. Telemetry.
========================================================================
Выводится состояние всех датчиков тележки, а значение с АЦП используется как уставка мощности команд вращения двигателей.
10.4. Count encoder.
========================================================================
Выводится на дисплей:
Press: 1 - forward left, 2 - reverse left, 3 - forward right, 4 - reverse right
5 - stop left & right, ESC - exit
Forward left: Position left = 0000 Position right = 0000
Т.е. по 1 - левое колесо едет вперед, Position left считает в плюс.
по 2 - левое колесо едет назад, Position left считает в минус
( в шестнадцатиричном виде).
По 3, 4 - аналогично для правого
По 5 - оба колеса останавливаются.
10.5. Go to line.
========================================================================
Тест выезда на белую линию. Тележка начинает движение и останавливается в позиции: оба датчика линии сработали, т.е. обнаружили линию.
10.6. Load Robo program.
========================================================================
После нажатия 6 на экран идут приглашения С, надо в Hiper Terminal выбрать Передача, Отправить файл, установить протокол Xmodem, выбрать файл, нажать Отправить. После успешной загрузки ( иначе сообщается об ошибке ), программа записывается в ЕЕПРОМ с индикацией адреса:
=>
Ready to receive file.CCCCCC
Rx OK
Save program to EEPROM.
0000 byte's
0040 byte's
0080 byte's
......
10.7. Print Robo program.
========================================================================
На дисплей выводится текущая ЧПУ программа, в перекодированном виде, без комментариев.
11. Вариант программы Монитор для системы управления на базе PC.
Этот вариант разработан для распределенной системы управления на базе РС и контроллера ML2313?-485.
Он в основном повторяет вариант для PS-M64-V02, поэтому дадим только описание меню и режимов, а в остальном см. текст программы с комментариями.
