Go TwinCAT: Контроль состояния сервотерминалов EL7201

Что если силовое питание не подано, ось не активна и вдруг разорвать сигнал обратной связи? Что если оторвать провод силового питания? Как будет реагировать NC? Возможно ли вообще отследить такой тип аварий и как сбросить ошибку? Копну глубже в контроль состояния компактных сервоусилителей EL72xx, и начну с обратной связи.

Изображение: Beckhoff Automation

Обратная связь

Сервоось недееспособна, если отсутствует сигнал обратной связи. Необходимо регулярно вызывать функцию Axis.ReadStatus(), чтобы понять, что с ней происходит. В момент потери сигнала будет выставлен флаг Axis.Status.DriveDeviceError.

Так как работа сервомотора без обратной связи невозможна, самостоятельно этот флаг не сбросится и не "рассосется". После устранения причины аварии, ошибку нужно будет сбросить с помощью стандартной функции MC_Reset. Но это всё на случай, если ПЛК и сервомодули нельзя обесточивать на время ремонта, обслуживания или замены оборудования. Безопаснее выключить и включить снова.

Силовое питание

Потерю силового питания нельзя рассматривать как аварийную ситуацию, так как питание логики и силового питания подаются раздельно. Силовое питание можно подать когда-нибудь позже, в то время как "мозги" должны быть запитаны с самого начала работы.

Ни один флаг не пошевелится, если силовое питание отсутствует, а NC-ось неактивна. Но стоит только разблокировать ось и дать ей команду на старт, как NC тут же взбрыкнет ошибкой. Необходимо научиться выявлять такие ситуации независимо от состояния оси и наличия или отсутствия команд.

Слово состояния сервомодуля (6010:10 — Statusword) содержит два бита, отвечающие за ошибки и аварии. Это бит #3 Fault и бит #7 Warning: стр. 143, Index 6010 DRV Inputs. Поле Fault отвечает за аварии. Этот бит транслируется подсистемой NC в поле состояния оси DriveDeviceError. Именно его мы отслеживали в главе про обратную связь. Потеря обратной связи — это авария, но(!) отсутствие силового питания — это еще не авария, а просто ситуация требующая особого внимания. Поэтому — Achtung, т. е. Warning.

Флаг Warning также транслируется в NC, но он не доступен через параметры состояния NC-оси. Причина этого для меня не понятна, но я попробую добраться и до этого флага.

Начнем с того, что Statusword передается в PDO сервомодуля. Затем, оно автоматически линкуется с NC параметрами Axis.Drive.Inputs.In.[nState1..nState2], попутно разбиваясь на старший и младший байты. И всё. Далее эти байты используются где-то внутри подсистемы NC и недоступны разработчику.

Чтобы получить доступ к слову состояния, можно непосредственно постучаться в сервомодуль, то есть прочитать параметры через функции CANopen. Можно и по другому. Если немного погуглить (а это более качественный способ поиска информации по справочной системе), то обнаруживается интересная таблица с индексами:

TwinCAT Connectivity → ADS-Device-Documentation → ADS Interface NC → Specification "Index group" for NC ( ID [0x01...0xFF] ) → Specification Drive → "Index offset" specification for cyclic drive process data (Index group 0x7300 + ID).

Из таблицы не совсем понятно зачем всё это необходимо, но судя по названиям...

Читаем другие параметры

Читать будем через ADS. Для чтения параметров, перечисленных в "таблице", мы воспользуемся функцией ADSREAD из библиотеки системных функций Tc2_System. Для запуска функции понадобятся:

NETID — пустая строка, если читаем с того же локального ПЛК.
PORT — это стандартный порт NC-Task SAF = 501.
IDXGRP — индекс группы из таблицы = 0x7300 + ID, где ID - это номер оси NC; нумерация осей начинается с единицы, то есть первая ось получит индекс = 16#7301.
IDXOFFS — смещение из таблицы = 16#80.

В таблице доступ к параметру 16#80 помечен как "Write", но все относительно и зависит от точки зрения, поэтому я буду из него "Read". Осталось решить куда прочитать данные. По идее необходима некая структура данных, но есть ли она в стандартных библиотеках мне не известно, поэтому я создал парочку своих собственных DUT. Выглядят они почти как в той самой таблице из справочной системы:

TYPE EL7201_DriveInfoEx :
STRUCT
    nInData1    : DINT;
    nInData2    : DINT;
    StatusWord  : EL7201_StatusWord; // Axis.Drive.Inputs.In.[nState1..nState2]
    nStatus3    : BYTE;
    nStatus4    : BYTE;
    // optional : extended drive info, 40 bytes
    nInData3    : DINT;
    nInData4    : DINT;
    nInData5    : DINT;
    nInData6    : DINT;
    nStatus5    : BYTE;
    nStatus6    : BYTE;
    nStatus7    : BYTE;
    nStatus8    : BYTE;
    Reserved1   : DINT;
    Reserved2   : DINT;
END_STRUCT
END_TYPE

Для удобства использования сразу же разбиваю слово состояния на структуру из битовых полей:

TYPE EL7201_StatusWord  :
STRUCT
    ReadyToSwitchOn     : BIT;
    SwitchedOn          : BIT;
    OperationEnable     : BIT;
    Fault               : BIT;
    Reserved4           : BIT;
    QuickStop           : BIT; // inverse: true when switched off
    SwitchedOnDisabled  : BIT;
    Warning             : BIT; // Ex.: raise when Power Supply lost
    Reserved8           : BIT;
    Reserved9           : BIT;
    TxPDOToggle         : BIT; // selection/deselection via 0x8010:01
    InternalLimitActive : BIT;
    TargetValueIgnored  : BIT;
    Reserved13          : BIT;
    Reserved14          : BIT;
    Reserved15          : BIT;
END_STRUCT
END_TYPE

В таблице есть указание что структура EL7201_DriveInfoEx может быть длинной как в 12 байт, так и расширенная, длинною в 40 байт. В моем случае необходима расширенная структура. Остается прочитать данные:

axis               : AXIS_REF;
axDriveStatus      : EL7201_DriveInfoEx;
AdsReadDriveStatus : ADSREAD;

[...]

AdsReadDriveStatus(
    NETID    := '', 
    PORT     := 501,
    IDXGRP   := 16#7300 + axis.NcToPlc.AxisId,
    IDXOFFS  := 16#80,
    LEN      := SIZEOF(axDriveStatus), 
    DESTADDR := ADR(axDriveStatus), 
    READ     := TRUE);
 
IF NOT AdsReadDriveStatus.Busy THEN
    AdsReadDriveStatus(READ := FALSE);
END_IF

Примечание: Fault и Warning работают и соответственно устанавливаются/сбрасываются независимо друг от друга: один флаг никак не влияет на другой. Warning устанавливается и сбрасывается автоматически, поэтому нет способа повлиять на его состояние.

Уровень силового питания

Значение силового питания (12-50 Вольт) можно прочитать напрямую из сервомодуля. Адрес сервомодуля можно получить с помощью функции MC_ReadDriveAddress и структуры ST_DriveAddress:

Изображение: Beckhoff Automation

Значение уровня постоянно обновляется в параметре CoE 9010:12 — DC link voltage. Значение дается в милливольтах, поэтому 24 вольтам будет соответствовать значение 23932. Почему не 24000? Потому что — не точно.

Следующий кусок кода прочитает значение напряжения прямо из сервомодуля:

dcLinkValue : DINT;
AdsReadCoE  : ADSREAD;

[...]

AdsReadCoE(
    NETID    := '169.254.23.39.4.1', 
    PORT     := 1002,
    IDXGRP   := 16#F302,
    IDXOFFS  := 16#90100012,
    LEN      := SIZEOF(dcLinkValue), 
    DESTADDR := ADR(dcLinkValue), 
    READ     := TRUE);
 
IF NOT AdsReadCoE.Busy THEN
    AdsReadCoE(READ := FALSE);
END_IF

NETID — адрес EtherCAT мастера.
PORT — номер порта устройства, в данном случае — это сервомодуль EL7201.
IDXGRP — индекс группы сервиса ADS, отвечающего за работу с CANopen SDO.
IDXOFFS — индекс и смещение регистра CAN. Для упрощения в примере выше индекс не вычисляется, так как в шестнадцатеричной системе его легко сформировать вручную 9010-0012. Если интересно, чуть более подробно написано в посте Работа с CANopen из C# программы.

Когда пример готов, подключаю цифровой осциллограф к переменной dcLinkValue и получаю график "зарядки-разрядки". Здесь питание контроллера и силовое питание сервомодулей подается от одного 24 вольтового блока питания. Поэтому "потолок" на графике ~ 24000, а сервомодуль работает с половинной мощностью:

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