Интерфейс приводов Herkulex

===========================

Требования

Подсистема взаимодействия с приводами должна удовлетворять следующим целям:

  1. Доступ к приводу по имени, преобразования внутренних единиц измерения в единицы СИ.
  2. Настройки приводов: передача параметров конфигурации в память приводов.
  3. Мониторинг: считывание регистров, статус привода, сброс статуса и пр. по требованию пользователя.
  4. Считывание позы при инициализации системы управления, возможность ее считывания в (?)
  5. Взаимодействие в реальном времени: периодическая посылка задания всем или группе приводов, считывание параметров заданной группы приводов. Интерфейсы мониторинга и настройки не должны мешать работе интерфейса реального времени.

Желательно обеспечить максимальное повторное использование кода, поддержку нескольких веток приводов на разных последовательных портах.

Структура системы взаимодействия с приводами

На основе требований выделяется и характера протокола выделяется 4 элемента:

  1. Канальный уровень: прием/посылка и проверка целостности кадров. (herkulex_driver)

  2. Реализация уровня представления (протокол): именование, кодирование/декодирование кадров. Например, преобразование операции назначения/чтения параметра привода в сообщение, преобразование задания на приводы в сообщение, декодирование ответа привода.

    В настоящий момент он реализован в виде программных объектов типа HerkulexServo.

  3. Подсистема реального времени (РВ): передача целевой позиции, считывание состояния (скорость, позиция и т.п.) группы приводов с заданной частотой и минимальной задержкой, кэширование регистров приводов и т.п.

  4. Подсистема настройки и мониторинга (НиМ): назначение произвольного набора параметров, чтение произвольного набора параметров, опрос статусов, считывание состояния группы приводов.

    В настоящий момент он реализован в виде компонента herkulex_array.

Принцип разделения времени. Система управления работает по циклам. В начале каждого цикла подсистеме РВ выделяется интервал времени, в течении которого она осуществляет обмен. По завершения обмена, либо по истечению времени канал передается подсистеме настройки и мониторинга, которая использует оставшийся промежуток времени. За осуществление этого разделения отвечает планировщик. Его логично сделать частью подсистемы РВ из-за тесной связи между ними: цикл планировщика тождественен циклу управления, набор операций РВ за цикл фиксирован.

Для реализации разделения по времени необходимо иметь верхнюю оценку на полного времени выполнения операции, включающего в себя: подготовку сообщения, посылку, обработку на стороне клиента, посылку ответа, обработку ответа. Выполнение очередной операции не начинается, если для нее не хватает времени в выделенном интервале.

Канальный уровень вынесен в отдельный компонент. Решение относительно спорное, т.к. при использовании синхронных операций ничем не отличается от вызова функций.

В случае нескольких портов каждый из них должен снабжаться своим herkulex_driver и планировщиком.

Альтернативные варианты реализации

Разделение интерфейса РВ и НиМ

Для работы подсистемы РВ и НиМ требуется реализация протокола. Однако вынести реализацию протокола в отдельный компонент затруднительно из-за слишком тесной связи с обоими подсистемами. Возможны реализации:

  1. Один компонент для подсистемы РВ, подсистемы НиМ, протокола и планировщика.
  2. Два компонента с двумя копиями протокола:
  3. Подсистема РВ, планировщик, протокол.
  4. Подсистема НиМ, протокол.
  5. Два компонента с одной копией протокола:
  6. Подсистема РВ, планировщик, требует операции кодирования JOG и запроса состояния, декодировании ответа на запрос состояния.
  7. Подсистема НиМ, протокол, предоставляет операции кодирования JOG и запроса состояния, декодировании ответа на запрос состояния.
  8. Два статически связанных компонента с одной общий копией протокола:
  9. Подсистема НиМ, протокол, в конструкторе создается еще один TaskContext для подсистемы РВ.
  10. Подсистема РВ, планировщик, доступ к протоколу осуществляется через ссылку на его класс.

Первый вариант потребует нескольких нитей (ExecutionEngine) в одном компоненте и их ручной синхронизации. Фактически это будет два компонента в одной обертке.

Второй вариант приводит к двум экземплярам реализации протокола. Это означает, что часть настройки (списки приводов, аппаратные ID, параметры калибровки) должны дублироваться. С другой стороны, могут быть параметры, используемые только в одной конфигурации.

Третий вариант осуществим только с реализацией подсистемы НиМ в виде компонента OROCOS с приоритетом РВ или с версиями операций ClientTread.

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

Далее описан четвертый вариант.

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

Альтернатива --- снабжение приводов полем port (или начинать иерархию параметров с имени порта, создавать структуру HerkulexArray для каждого порта), herkulex_array порождает несколько herkulex_sched.

В обоих случаях экземпляр agregator_gait может быть использован для полного определения позы.

Взаимодействие с канальным уровнем

  1. Канальный уровень реализуется в виде библиотеки и становится частью РВ подсистемы. Обращение --- вызовы функций.
  2. Канальный уровень отдельный компонент, взаимодействие синхронное --- операции.
  3. Канальный уровень отдельный компонент, взаимодействие асинхронное --- порты или операции.

В первых двух вариантах обращение к канальному уровню выглядит как вызов функции, блокирующийся до завершения операции. Эта способ хорошо согласуется с текущей реализацией. Использование библиотеки быстрее, но менее универсально.

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

Компонент herkulex_driver

Реализация канального уровня: формирование и отправка пакета по интерфейсу.

Входные порты

Нет.

Выходные порты

Порт публикует пакеты, например, для целей мониторинга. Возможность опциональна.

  1. port_name (string) --- имя устройства порта.
  2. baudrate (uint) --- скорость передачи данных.
  3. timeout (double) --- время ожидания ответного пакета, c (опционально при реализации cmdSync).

Операции

  1. Предоставляет: void sendPacketDL(HerkulexPacket req) (OwnThread) --- получить асинхронную команду, передать ее в сеть.
  2. Требует: void receivePacketDL(HerkulexPacket ack) --- известить о получении корректного пакета.

Опционально:

  1. bool cmdSync(HerkulexPacket& req, HerkulexPacket& ack) (OwnThread) --- послать запрос, дождаться ответа, вернуть результат.
  2. uint estimate(uint data_size, bool ack_rek) (ClientTread) --- верхняя оценка времени обмена (опционально).

Семантика исполнения

Конфигурация. Открытие порта, настройки.

cmdReqDL/cmdAckDL. Асинхронный механизм, фактически реализация портов на операциях. Интерфейс унифиурован со способом взаимодействия herkulex_array <--> herkulex_sched, что позволяет использовать использовать связку herkulex_array <--> herkulex_driver без планировщика и нтерфейса РВ. Основной недостаток --- компоненты высшего уровня сами должны заботиться о timeout.

cmdSync. Синхронный запрос. Запускает таймер. Формирует кадр по HerkulexPacket, посылает его, ожидает целостного кадра-ответа. Возврат по успеху или истечению таймера. Некорректно сформированные данные игнорируются.

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

Замечание: Компонент не осуществляет контроль за соблюдением политики ACK (следует ли ожидать ответа от привода на данный пакет). Соответствующие коллизии должен предотвращать протокол.

Детали реализации.

Версия без cmdSync элементарно реализуется на базе FileDescriptorActivity.

Версия с cmdSync сложнее, т.к. внутри операции надо ждать файлового ввода/вывода. Если не использовать asio, предлагается завести отдельную нить FileDescriptorActivity для чтения. Разбор входа --- стандартный подход на базе конечного автомата. Автомат сбрасывается по breakLoop.

Взаимодействие нитей DataObject или BufferLockFree и условная переменная или светофор.

Исключения и ошибки

Ошибки: 1. Аппаратные ошибки ввода-вывода.

Предупреждения: 2. Некорректный запрос. 3. Некорректный ответ. 3. Лишний или потерянный ответ.

Компонент herkulex_array

Подсистема настройки и мониторинга.

Входные порты

Отсутствуют.

Выходные порты

  1. joints (JointState) --- позиция и скорость приводов. Опрашивается и публикуется по запросу для целей первичной инициализации.

Параметры

  1. servos --- динамическая структура (PropertyBag) с информацией приводах в системе. Приведены основные опции.
    |- servo1_name
|  |- hw_id (uint) --- идентификатор
|  |- servo_model (string) --- модель привода
|  |- registers_init
|  |   \- register_name (uint) --- значения регистров, инициализируемых при запуске. 
|  |- reverse (bool) --- флаг обращения направления на уровне протокола
|  \- offset (double) --- смещение нуля (на уровне протокола).
\- servo2_name

Открытый вопрос: хранить регистры в uint или преобразовывать в СИ.

  1. uint tryouts --- Число попыток обращения к приводу.
  2. timeout (double) --- время ожидания ответного пакета.

Операции

Внешний интерфейс настройки и мониторинга.

(Преимущественно OwnTread, в порядке важности, все не нужны).

  1. strings listRegisters(servo)
  2. uint getRegisterRAM(servo, reg) --- (что делать с ошибочным именем?)
  3. bool setRegisterRAM(servo, reg, val)
  4. printRegisterRAM(servo, reg)
  5. printAllRegistersRAM(servo)
  6. getStatus(servo)
  7. clearStatus(servo)
  8. printServoStatus(servo)
  9. printAllServoStatuses()
  10. printErrorServoStatuses()
  11. resetServo(servo) --- провести инициализацию привода заново.
  12. resetAllServos()
  13. publishState(servos) --- опросить позицию и скорость, опубликовать ее.
  14. writeRegProperties() --- обновить значения параметров в registers_init текущими значениями регистров (новые не создавать).
  15. addRegProperties() --- добавить новые параметры, инициализировать их регистрами.
  16. readRegProperties() --- перечитать параметры в регистры.

Интерфейс с herkulex_sched.

  1. Требует: void sendPacketCM(HerkulexPacket req) --- извещает herkulex_sched через BufferLockFree о новом запросе.
  2. Предоставляет: void receivePacketCM(HerkulexPacket req) (ClientTread) записывает в BufferLockFree результат запроса, посылает сигнал условной переменной.

Это механизм позволяет преобразовать синхронный вызов функции sendRequest в herkulex_array в асинхронный запрос на уровне herkulex_sched. Фактически он реализует семантику портов, используя операции. Порты невозможно использовать, т.к. OROCOS не пердоставляет возможности ожидания прихода сообщения в теле операции.

Доступ к протоколу.

herkulex_sched нуждается в средстве кодирования и декодирования пакетов. Они пердоставляются в виде операций ClientThread, которые будут исполняться с приоритетом планировщика. Коллизии при доступе к пмяти исключаются за счет неизменнности структур, представлющих привод. Прототипы опреций аналогичны соотвествтующим командам протокола.

  1. bool reqIJOG(HerkulexPacket& req, ServoGoal goal) сформировать групповую команду установки позиции.
  2. bool reqPosVel(HerkulexPacket& req, string servo)
  3. bool ackPosVel(HerkulexPacket req, string servo, double& pos, double& vel, Status& status)
  4. bool reqState(HerkulexPacket& req, string servo)
  5. bool ackState(HerkulexPacket req, string servo, double& pos, double& vel, Status& status)

Методы

  1. sendRequest(HerkulexPacket req, bool (* ack_callback)(HerkulexPacket ack)) --- синхронный запрос к приводам. Вызывает sendPacketCM, блокируется на условии до истечения timeout или вызова receivePacketCM. При получении пакета проверет его при помощи вызова ack_callback(). Если результат неудовлетворителен, то ждет ноавого пакета или повторяет запрос и т.д.

Семантика исполнения

Конфигурация. Считываются опции, создается HerkulexArray, содержащий объекты HerkulexServo. Проводится инициализация приводов и регистров по параметрам.

Исполнение. updateHook пуст, основной код в операциях. Операции транслируют запрос в вызовы sendRequest.

В некоторой степени открыт вопрос кеширования регистров и приводов. Для ускорения операций требующий чтения множества регистров они очень желательны.

Детали реализации.

Операции длительные и блокирующие. Следует предусмотреть реализацию breakLoop, прерывающую ожидание (сигнализирует условную переменную).

Взаимодействие c herkulex_sched

Механизм описывается псевдокодом ниже. Он позволяет обрабатывать "операцию" cmdSync в updateHook по усмотрению herkulex_sched.

В herkulex_array:

sendRequest(HerkulexPacket req, bool (*ack_callback)(HerkulexPacket ack)) {
   do {
      sendPacketCM(req)
      ждем условия: !buffer_ack_CP.Empty()
      buffer_ack_CM.Pop(ack)
      success = ack_callback(ack);
   } while( условие повторения )
} 
receivePacketCM(HerkulexPacket ack) {
   buffer_ack_CM.Push(ack)
   сигнализируем условие
}

В herkulex_sched (если планировщик не запущен, то он просто исполняет запросы):

updateHook() {
   ...
   if (!buffer_lock_free2.empty()) {
      buffer_lock_free2.Pop(req)
       обработка
       response(ack)
    }
    ...
}
sendPacketCM(HerkulexPacket req) {
    if (this->isRunning())  
        buffer_CM_req.Push(req)
    else {
        herkulex_driver.sendPacketDL(req);
    }
} 
receivePacketDL(HerkulexPacket ack) {
    if (this->isRunning())  
        buffer_DL_resp.Push(ack)
    else {
        herkulex_array.receivePacketCM(ack);
    }
}

В herkulex_driver:

sendPacketDL(HerkulexPacket req) {
    кодирование пакета
    запись в порт
} 
updateHook() {
    ...
    if (получен целостный пакет pkt) {
         cmdAckDL(pkt);
    }
}

Реализация протокола

Предлагается следующая иерархия классов. Параметры конструкторов являются списками полей-данных. Основное требование --- неизменность описания массива приводов при функционировании компонента. Предложенное решение предполагает возможность существования приводов разного типа.

Семантика протокола состояит в трансляции запросов в HerkulexPacket и обратном декодировании ответов в заданные структуры данных. Иного варианта быть не может: один объект-протокол используется одновременно двумя нитями, использующими разные методы обращения к приводам, что не позволяет удобно включить в протокол синхронное взаимодействие с приводом. Единственное решение --- передовать функциям-запросам сылку на cmdSync.

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

Однако такой подход неудобен тем, что нельзя прямо в протокол вписать последовательности действий, например, по инициализации привода. Их должен исполнять непосредственно herkulex_array, причем трудно сделать такую последовательность своей для каждого типа привода.

  • RegisterMapper(vector<HerkulexRegister>) --- описание набора регистров, преобразования адреса в имя и обратно в любом направлении.

  • RegisterValues : map<string, uint> --- используется для передачи и хранения состояние группы регистров.

  • HerkulexServo(name, hw_id, RegisterMapper, reverse, offset) Запись о существовании привода. Не хранит никаких состояний привода, кроме неизменных параметров: scale, offset, hw_id, reverse. Реализует типовые операции c приводом. Связывается c RegisterMapper по ссылке.

    Типовые методы. .convert* --- преобразование значений в СИ к внутреннему представлению привода и обратно (учет scale, reverse). .reqRead(HerkulexPacket& req, string register) --- запрос на чтение регистра .reqRead(HerkulexPacket& req, reg_start, reg_end) --- запрос на чтение регистров .reqRead_epp(HerkulexPacket& req, string register) --- запрос на чтение регистра .reqStatus(HerkulexPacket& req) bool .ackRead(HerkulexPacket& ack, RegisterValues regs) --- декодирование запроса, false при несовпадении типа или привода * ...

    Заглушки для методов чтения РВ. Не ясно, как сделать лучше: для привода с произвольным набором регистров нельзя гарантировать, что нужные регистры есть и их можно прочитать за одно обращение к приводам. .reqPosVel --- запрос скорости и позиции bool .ackPosVel --- декодирование результата в структуру .reqState --- запрос регистров 53--59 .ackState --- декодирование результата в структуру

  • HerkulexServoDRS0101(name, hw_id, reverse, offset) : HerkulexServo Наследник HerkulexServo: жестко задан RegisterMapper, реализованы жестко зависящие от набора регистров методы

    • .reqPosVel --- запрос скорости и позиции
    • bool .ackPosVel --- декодирование результата в структуру
    • .reqState --- запрос регистров 53--59
    • .ackState --- декодирование результата в структуру

  • HerkulexArray Агрегирует набор HerkulexServo (реально HerkulexServoDRS0101) доступ по имени, итерация по элементам

    • .reqIJOG(HerkulexPacket& req, HerkulexGoal& goal)
    • .reqSJOG(HerkulexPacket& req, HerkulexGoal& goal, playtime)
    • const HerkulexServo * GetServo(const string& name), const HerkulexServo * GetServo(uint hw_id) --- получить указатель на привод. <!--- * bool .ackRead(HerkulexPacket& ack, string name, RegisterValues regs) --- декодирование запроса, false при несовпадении типа или привода ->

Для извещения о ошибках следует использовать исключения, однако надо помнить, что это не РВ средство, а средство извещения о ошибках, не являющихся нормальным состоянием системы. Поэтому о ошибке декодирования пакетов извещение происходит посредством возврата false.

Типовое использование:

 const HerkulexServo& servos = array->getServo(name); // исключения по имени привода
 servo.reqRead(req_packet, "velocity");  // исключение по имени регистра
 cmdSync(req_packet, ack_packet); 
 if (! servo->ackRead(ack_packet, cache))
     if (servo->getHWID() ~= ack_packet.hw_id) ....
     else ...
 }
 else {
     ...
 }

Исключения и ошибки

Уточнить 1. Привод не отвечает на STAT 2. Превышен лимит числа попыток обращения к приводу: привод не отвечает 2. Превышен лимит числа попыток обращения к приводу: привод не вернул плохой ответ. 4. Неверное имя привод/регистр.

Компонент herkulex_sced

Подсистема обмена реального времени.

Входные порты

  1. sync (TimerEvent, EventPort) --- тактирующий сообщения таймера
  2. goal (HerkulexGoal) --- задания на приводы

Выходные порты

  1. joints (JointState) --- позиции и скорости (прочитанные).
  2. states (HerkulexState) --- расширенное состояние привода.

Параметры

  1. period_RT (double) --- длительность периода РВ (секунды)
  2. period_CM (double) --- длительность периода НиМ (секунды)
  3. poll_list (strings) --- список приводов, которые опрашиваются.
  4. timeout (double) --- время ожидания ответного пакета (секунды).

Операции

Интерфейс с herkulex_array.

  1. Требует: void responseCM(HerkulexPacket req) передать подсистеме НиМ (configuration and monitoring) результат запроса.
  2. Предоставляет: void requestCM(HerkulexPacket req) (ClientTread) --- извещает herkulex_sched через BufferLockFree о новом запросе.

Интерфейс с herkulex_driver.

  1. Требует: void sendPacketDL(HerkulexPacket req) --- извещает канальный уровень (Data Link) herkulex_driver через о новом запросе.
  2. Предоставляет: void receivePacketDL(HerkulexPacket req) (ClientTread) записывает в BufferLockFree результат запроса, извещает компонент о получении ответа.

Методы

Семантика исполнения

Операция sendPacketCM. Поведение детализировано выше. В зависимоси от состояни компонента (запущен, остановлен) помещает запрос в буфер, либо передает его на исполнение канальному уровню немедленно.

Исполнение. updateHook():

  1. Ожидаем сообщение TimerEvent
  2. Запускаем таймер на длительность period_RT
  3. Читаем порт goal, посылаем JOG
  4. В цикле читаем poll_list и опрашиваем приводы.
  5. Выход из цикла: иcтек таймер или кончился список.
  6. Публикуем результат опроса.
  7. Запускаем таймер на длительность period_CM
  8. Ждем запроса через вызов cmdReqCP, обрабатываем его.
  9. Цикл и ожидание прерываем по истечению таймера.
  10. Переход в состояние 1.

По идее, можно опросить состояние всех приводов за несколько циклов.

Детали реализации.

Таймер timeout можно реализовать также средсвами herkulex_sched: запускать его при вызове sendRequestDL, по истечению делать trigger(). Аналогичным способом реализуется таймер слотов РВ и НиМ.

Ожидание запроса НиМ (sendPacketCM) и ответа приводов (receivePacketDL) можно осуществить выходом из updateHook. По событию (исполнение операции, или getActivity()->trigger()) снова начнет исполнятся пользовотельский код. Надо только проверить тип события: запрос, ответ, таймер. При такой реализации будет не цикл, а конечный автомат.

В OROCOS есть реализация таймера Timer, буфера BufferLockFree и средств синхронизации.

Исключения и ошибки

Уточнить 1. TimerEvent пришло раньше выхода из updateHook(). 2. Ошибка разбора ответа привода. 3. Имена приводов.