Управление движением по шине EtherCAT в среде разработки CODESYS
В этом примере мы реализуем функцию для управления движением SoftMotion на платформе Codesys по шине EtherCAT в качестве мастер-станции, а в качестве аппаратной платформы используем контроллер XINJE XSLH-30A32 и сервопривод Xinje DS5C1 — ведомомая станция.
Среда программирования — CodeSys 3.5 Sp16 patch 40.
- Создаем новый проект
- На каждое EtherCAT-устройство производитель дает XML-файл описания. Устанавливаем данный файл.
ГЛАВНОЕ МЕНЮ ==> ИНСТРУМЕНТЫ ==> РЕПОЗИТАРИЙ УСТРОЙСТВ



После окончания установки закрываем окно. Добавляем в проект мастер станцию EtherCAT Master.
Нажимаем ПКМ* ДОБАВИТЬ УСТРОЙСТВО ==> ПРОМЫШЛЕННЫЕ СЕТИ ==> EtherCAT Master SoftMotion
*ПКМ (здесь и далее) — Правая Кнопка Мыши

Устанавливаем соединение и переходим к настройкам компонента.

По нулевому порту у нас устанавливается соединение с компьютером, поэтому выбираем порт eth1.

Загружаем программу в ПЛК. ПКМ на компоненте Ethernet Master SoftMotion ==> Поиск Устройств
Все подключенные устройства, на которые установлены XML файлы описания отражаются в окне.
Выделяем нужное устройство и нажимаем кнопку КОПИРОВАТЬ В ПРОЕКТ. Можно скопировать в проект сразу все устройства, но для начала нам необходимо только одно.


Добавляем ось управления движением.
Выбираем ведомое устройство оси XINJE_DS5C1_CoE_Drive, кликаем ПКМ и выбираем Добавить ось SoftMotion CiA 402.

Для удобства переименуем ось. ПКМ на элементе ==> Рефакторинг


Добавляем в проект необходимые папки:
GVLs — для глобальных переменных;
POUs — для программных блоков;
VISUs — для визуализации;
В образованных папках создаем необходимые объекты.

Для дальнейшей работы нам необходимы библиотеки. Конкретно SM3_BASIC. Через менеджер библиотек добавляем.
Объявляем функциональные блоки.
Программу пишем на языке ST.
Пишем программу PLC_PRG
PROGRAM PLC_PRG
VAR
fbPower :MC_Power;
fbMoveRelative :MC_MoveRelative;
fbStop :MC_Stop;
fbReset :MC_Reset;
END_VAR
// Функциональный блок влючения
fbPower(
Axis:= X_Axis ,
Enable:= ,
bRegulatorOn:= ,
bDriveStart:= ,
Status=> ,
bRegulatorRealState=> ,
bDriveStartRealState=> ,
Busy=> ,
Error=> ,
ErrorID=> );
//Функциональный блок Относительное перемещение
fbMoveRelative(
Axis:= X_Axis ,
Execute:= ,
Distance:= ,
Velocity:= ,
Acceleration:= ,
Deceleration:= ,
Jerk:= ,
BufferMode:= ,
Done=> ,
Busy=> ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=> );
//Функциональный блок останова
fbStop(
Axis:= X_Axis,
Execute:= ,
Deceleration:= ,
Jerk:= ,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
// Фунциональный блок RESET
fbReset (
Axis:= X_Axis ,
Execute:= ,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
Добавляем визуализацию.
На каждый компонент разработан библиотечный блок визуализации. Это освобождает нас от рутинного труда по объявлению переменных и привязки их ко входам/выходам. В реальных проектах это не пройдет, но для демонстрации и понимания подходит.


Эти компоненты расположены во вкладке панель инструментов. На примере компонента MC_POWER разберемся, каким образом привязаться к переменным.

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

Элемент, который имитирует вращение мотора называется RotDrive. Его нужно привязать к оси X_Axis.
SoftMotion drive: scaling/mapping
Разбираемся в настройках ОСИ. Двойной клик на компоненте X_Axis. Переходим во вкладку Scaling/Mapping

Расшифровка значений:

Чтобы точно контролировать параметры движения, контроллер должен точно рассчитать положение серводвигателя. Для таких расчетов необходимы данные обратной связи от энкодера. Это необходимо и для того, чтобы не возникали ошибки от переполнения счетчиков импульсов от энкодера.
Часто возникает необходимость в получении точного абсолютного положения. И поэтому нужно точно определиться в режиме работы. ЛИНЕЙНЫЙ или ПЕРИОДИЧЕСКИЙ.
Параметры энкодера (разрешающая способность) двигателя и передаточное число редуктора, в разных
системах отличаются. Их нужно учитывать при программировании задач.
В этом примере подключен 17-ти разрядный энкодер. Поэтому мы вводим значение 131072
Коэффициент соотношения между настройками и выходными данными
В визуализации можно задать расстояние, скорость и другие параметры, необходимые двигателю для перемещения.
ПОРЯДОК ВКЛЮЧЕНИЯ:
Нажмите bDriveStart—bRegulatorOn—Enable по очереди в функциональном блоке MC_Power для включения двигателя в обычном режиме.
И последней кнопку EXECUTE в функциональном блоке MC_MoveRelative, чтобы начать относительное перемещение.
Начальные действия такие же, как и в прошлом примере.
В проекте будем использовать функциональные блоки (FB) из библиотеки SM3_BASIC.
Создаем экземпляры нужных FB и помещаем в разделе переменных главной программы.
PROGRAM PLC_PRG
VAR
fbPower :MC_Power;
bRegulatorState :BOOL;
bDriveQSPState :BOOL;
fbReset :MC_Reset;
fbHome :MC_Home;
fbMoveAbsolute :MC_MoveAbsolute;
fbReadActualPos :MC_ReadActualPosition;
END_VAR
Создаем объект Список глобальных переменных и объявляем их.
VAR_GLOBAL
bPowerStatus :BOOL;
bAxisEnable :BOOL;
bReset :BOOL;
bExecuteHome :BOOL;
lrActualPosition :LREAL;
lrSetPosition :LREAL;
bExecuteABS :BOOL;
lrVelocity :LREAL := 50;
lrAccel :LREAL := 10;
lrDecel :LREAL := 10;
bIsActive :BOOL;
bError : BOOL;
stErrorID : SMC_ERROR;
END_VAR
// Вызываем созданные функциональные блоки и привязываем переменные.
// Вызов блока Power
fbPower(
Axis:= Axis_X ,
Enable:= gGlobal.bAxisEnable,
bRegulatorOn:= TRUE ,
bDriveStart:= TRUE,
Status=> gGlobal.bPowerStatus,
bRegulatorRealState=> bRegulatorState ,
bDriveStartRealState=> bDriveQSPState ,
Error=> gGlobal.bError ,
ErrorID=> gGlobal.stErrorID );
//Вызов Axis Reset
fbReset(
Axis:= Axis_X ,
Execute:= gGlobal.bReset ,
Error=> gGlobal.bError ,
ErrorID=> gGlobal.stErrorID );
// Вызов блока Homing
fbHome(
Axis:= Axis_X ,
Execute:= gGlobal.bExecuteHome ,
Position:= 0 ,
Error=> gGlobal.bError ,
ErrorID=> gGlobal.stErrorID );
//Вызов блока AbsoluteMove
fbMoveAbsolute (
Axis:= Axis_X,
Execute:= gGlobal.bExecuteABS ,
Position:= gGlobal.lrSetPosition ,
Velocity:= gGlobal.lrVelocity ,
Acceleration:= gGlobal.lrAccel,
Deceleration:= gGlobal.lrDecel ,
BufferMode:= SM3_Common.MC_BUFFER_MODE.Aborting ,
Active=> gGlobal.bIsActive ,
Error=> gGlobal.bError ,
ErrorID=> gGlobal.stErrorID );
// Вызов блока Read Actual Pos
fbReadActualPos(
Axis:= Axis_X,
Enable:= TRUE ,
Error=> gGlobal.bError ,
Position => gGlobal.lrActualPosition,
ErrorID=> gGlobal.stErrorID );
Создаём визуализацию

Привязка переменных:
1.gGlobal.bAxisEnable;
2.gGlobal.bPowerStatus;
3.gGlobal.bIsActive;
4.gGlobal.bReset;
5.gGlobal.bExecuteHome;
6.gGlobal.lrActualPosition;
7.gGlobal.lrSetPosition;
8.gGlobal.bExecuteHome;
9.gGlobal.bExecuteABS;
9.gGlobal.bError;
10.gGlobal.stErrorID;
Если всё сделано правильно, до выглядеть будет так:

POSITION PROFILE
Создаем проект. Среда программирования CodeSys 3.5 Sp16 patch 40, язык ST.
Функциональный блок MC_PositionProfile находится в составе библиотеки SoftMotion.
Данный FB предназначен для управления профилем движения с фиксацией по времени. Т.е. весь путь — это временные участки. На каждом временном участке своя позиция.

Начинаем с нулевой позиции, потом первая и т. д. По сути все описание движения помещается в таблицу.
Профили времени/скорости и времени/ускорения аналогичны профилю положения с точками выборки на линиях скорости или ускорения.

Для написания программы нам понадобится следующее:
MC_TP_REF (STRUCT)
YPE MC_TP_REF : STRUCT
//Эта СТРУКТУРА описывает траекторию, которая позже может быть выполнена с помощью MC_PositionProfile.

В этот массив заносятся данные, которые описывают движение на данном отрезке пути. За какой интервал времени будет пройден путь. Т.е. таким образом мы на каждом отрезке назначаем скорость.

Общие настройки нужно также производить здесь.

PLC_PRG
VAR_GLOBAL
bAxisEnable :BOOL;
bAxisPowerState :BOOL;
bReset :BOOL;
bAxisIsBusy :BOOL;
bExecuteProfile :BOOL;
astTimePos :ARRAY[0..10] OF TimePositionProFile;
lrActualPos :LREAL;
END_VAR
TYPE TimePositionProFile :
STRUCT
// Time in ms
iTime :INT;
//Position in unit
lrPosition :LREAL;
END_STRUCT
END_TYPE
PROGRAM PLC_PRG
VAR
fbPower :MC_Power;
fbReset :MC_Reset;
fbPosProfile :MC_PositionProfile;
stTPRef :MC_TP_REF;
fbReadPos :MC_ReadActualPosition;
i :INT;
END_VAR
//Включение Axis
fbPower (
Axis:= X_Axis ,
Enable:= TRUE,
bRegulatorOn:= gAxis.bAxisEnable,
bDriveStart:=gAxis.bAxisEnable ,
Status=> gAxis.bAxisPowerState
);
fbReset(
Axis:= X_Axis,
Execute:= gAxis.bReset ,
);
//GET TIME POSITION
FOR _i := 0 TO 10 DO
stTPRef.MC_TP_Array[_i+1].delta_time := INT_TO_TIME(gAxis.astTimePos[_i].iTime);
stTPRef.MC_TP_Array[_i+1].position := gAxis.astTimePos[_i].lrPosition;
END_FOR
stTPRef.IsAbsolute := TRUE;
fbPosProfile(
Axis:= X_Axis,
TimePosition:= stTPRef,
Execute:= gAxis.bExecuteProfile ,
ArraySize:= 10 ,
//PositionScale:= ,
//Offset:= ,
//Done=> ,
Busy=> gAxis.bAxisIsBusy
);
//Read POSITION
fbReadPos(
Axis:= X_Axis ,
Enable:= TRUE,
//Valid=> ,
Position=> gAxis.lrActualPos );
Визуализация

CodeSys SoftMotion. Взаимодействие нескольких осей — ГЛАВНАЯ И ПОДЧИНЕННЫЕ
(CodeSys 3.5 Sp16 patch 40, ST).
В этом примере будет использовано взаимодействие между N осями (для начала у нас будет две). Главная ось и подчиненная. Подчиненная будет отслеживать перемещение главной и повторять движение в зависимости от коэффициента отслеживания. Не обязательно, что это будет какое либо число. Это может быть и ФУНКЦИЯ. Все в руках ПРОГРАММИСТА.
Создаем необходимые папки и добавляем объекты (Список глобальных переменных GVLs>>>gAxis, VISUs>>>Визуализация).
VAR_GLOBAL
bAxis_x_Enable :BOOL;
bAxis_x_PowerStatus :BOOL;
bAxis_x_ExecuteVel :BOOL;
bAxis_x_ExecuteStop :BOOL;
lrAxis_x_ActualVel :LREAL;
lrAxis_x_SetVel :LREAL;
lrAxis_x_Accel :LREAL:=500;
lrAxis_x_Decel :LREAL:=500;
bAxis_x_IsActive :BOOL;
//______________________________________________
bAxis_y_Enable :BOOL;
bAxis_y_PowerStatus :BOOL;
bAxis_y_Execute :BOOL;
bAxis_y_IsActive :BOOL;
dintRatioNum :DINT:=1;
dintRatioDnum :UDINT:=1;
lrAxis_y_Accel :LREAL:=10;
lrAxis_y_Decel :LREAL:=10;
bAxis_y_InGear :BOOL;
bAxis_y_ExecuteStop :BOOL;
bAxis_y_StopDone :BOOL;
END_VAR
//В основной программе объявляем функциональные блоки.
PROGRAM PLC_PRG
VAR
fbPower_X :MC_Power;
fbMoveVel_X :MC_MoveVelocity;
fbStop_X :MC_Stop;
fbReadVel_X :MC_ReadActualVelocity;
//______________________________________________________
fbPower_Y :MC_Power;
fbGearAxisY :MC_GearIn;
fbStop_Y :MC_Stop;
END_VAR
//Вызываем объявленные блоки.
//Для правильной работы нам необходимо включить блоки.
fbPower_X (
Axis:= Axis_X ,
Enable:= TRUE ,
bRegulatorOn:= gAxis.bAxis_x_Enable ,
bDriveStart:= gAxis.bAxis_x_Enable ,
Status=> gAxis.bAxis_x_PowerStatus ,
bRegulatorRealState=> ,
);
fbPower_Y(
Axis:= Axis_Y ,
Enable:= TRUE ,
bRegulatorOn:= gAxis.bAxis_y_Enable ,
bDriveStart:= gAxis.bAxis_y_Enable ,
Status=> gAxis.bAxis_y_PowerStatus ,
bRegulatorRealState=> ,
);
Нужно запустить главную ось и потом в случае необходимости корректно остановить.
fbMoveVel_X(
Axis:= Axis_X ,
Execute:= gAxis.bAxis_x_ExecuteVel ,
Velocity:= gAxis.lrAxis_x_SetVel,
Acceleration:= gAxis.lrAxis_x_Accel ,
Deceleration:= gAxis.lrAxis_x_Decel,
Direction:= MC_Direction.current,
BufferMode:= MC_BUFFER_MODE.Aborting,
Active=> gAxis.bAxis_x_IsActive
);
fbStop_X(
Axis:= Axis_X ,
Execute:= gAxis.bAxis_x_ExecuteStop,
Deceleration:= gAxis.lrAxis_x_Decel ,
);
Контроль скорости главного блока.
fbReadVel_X (
Axis:= Axis_X,
Enable:= TRUE ,
Velocity=> gAxis.lrAxis_x_ActualVel
);
Для запуска подчиненного устройства мы применяем функциональный блок MC_GearIn. Подключенное к данному блоку подчиненное устройство повторяет параметры скорости в зависимости от коэффициента.
ПАРАМЕТРЫ
RatioNumerator — числитель
RatioDenominator — знаменатель
fbGearAxisY (
Master:= Axis_X ,
Slave:= Axis_Y,
Execute:= gAxis.bAxis_y_Execute ,
RatioNumerator:= gAxis.dintRatioNum ,
RatioDenominator:= gAxis.dintRatioDnum ,
Acceleration:= gAxis.lrAxis_y_Accel ,
Deceleration:= gAxis.lrAxis_y_Decel ,
BufferMode:= SM3_Common.MC_BUFFER_MODE.Aborting ,
InGear=> gAxis.bAxis_y_InGear ,
Active=> gAxis.bAxis_y_IsActive
);

Нужно также корректно остановить подчинённое устройство в случае необходимости
fbStop_Y(
Axis:= Axis_Y ,
Execute:= gAxis.bAxis_y_ExecuteStop ,
Deceleration:= gAxis.lrAxis_y_Decel,
Done=> gAxis.bAxis_y_StopDone ,
);

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

Создание группы осей. Последовательность действий.
Создайте новый стандартный проект с помощью CODESYS. Для POU PLC_PRG выберите язык реализации.
В дерево устройств добавьте необходимые компоненты.
В дереве устройств откройте контекстное меню объекта приложения и выберите
Проект → Добавить объект → Группа Axis. Вставьте объект с указанным именем AxisGroup
Выбираем кинематику.


Настраиваем параметры осей. Привязываем оси, согласно выбранной модели кинематики.

Состояния группы Axis
- Ошибки отдельных осей всегда приводят группу осей в состояние GroupErrorStop
- Если группа осей переключается на GroupMoving, то все оси переключаются на SynchronizedMotion.
- Если группа осей переключается с GroupMoving на GroupStandby, то все оси переключаются на standstill
- Если группа осей переключается с GroupMoving на GroupErrorStop, то все оси переключаются на GroupErrorStop
- Если группа осей находится в GroupStandby, то не обязательно все отдельные оси находятся в standstill, поскольку ими можно управлять
с помощью функциональных блоков одноосного перемещения, таких как MC_Jog - Если движение завершается с ошибкой, то все буферизованные последующие движения прерываются с помощью CommandAborted
- Пока группа осей следует динамической системе координат, она будет оставаться в GroupMoving
- Группа axis находится в GroupMoving тогда и только тогда, когда группа перемещается скоординированным образом
- Переключение с GroupMoving на GroupStandby выполняется через один цикл после последнего изменения положения
Комбинация положения и кинематики
С помощью конфигуратора axis Group можно комбинировать кинематику положения и инструмента. В результате большое количество
роботов может быть сконфигурировано с использованием небольшого количества кинематик.
Примерами позиционной кинематики служат Kin_Gantry3 и штативы (Kin_Tripod_Lin, Kin_Tripod_Rotary).
Эта кинематика может перемещаться в любую точку или положение, но не может выполнять любое количество ориентаций.
Передняя система координат позиционной кинематической системы называется системой координат фланца. Она определяет место, где фиксируется кинематика ориентации .
SoftMotion Drives
General


Вкладка Scaling/Mapping
На этой вкладке вы можете определить взаимосвязь между техническими единицами измерения (например, миллиметрами или градусами) и
единицами измерения привода (приращениями). В зависимости от описания устройства параметры настройки отображаются упрощенными
(параметр bHiresMode = TRUE), и / или также возможно масштабирование для линейных двигателей (параметр IsLinearMotor = TRUE).
При необходимости вы можете повлиять на сопоставление циклически передаваемых объектов привода с переменными IEC.
Motor Type


В нашем проекте будем использовать следующие функциональные блоки:
MC_GroupEnable. Этот функциональный блок передает управление связанными осями группе осей AxisGroup.
MC_MoveDirectAbsolute Этот функциональный блок управляет перемещением группы осей в указанное абсолютное положение в указанной системе координат.
PLC_PRG
VAR_GLOBAL
bGroupPowerEnable :BOOL;
bGroupEnableDone :BOOL;
bGroupABS_Execute :BOOL;
bMoveStart :BOOL;
lrX_Position :LREAL:=100;
lrY_Position :LREAL:=200;
lrZ_Position :LREAL:=300;
bMoveDirABS_Done :BOOL;
bMoveDirABS_Activ :BOOL;
bError :BOOL;
ErrorID :SMC_ERROR;
fbGroupPower :SMC_GroupPower;
fbGroupEnable :MC_GroupEnable;
fbMoveDirectionABS :MoveDirABS;
END_VAR
FUNCTION_BLOCK MoveDirABS
VAR_INPUT
bExecute :BOOL;
lrX :LREAL;
lrY :LREAL;
lrZ :LREAL;
END_VAR
VAR_OUTPUT
bDone: BOOL;
bActive: BOOL;
END_VAR
VAR
fbMovDirABS :MC_MoveDirectAbsolute;
END_VAR

fbGroupPower(
AxisGroup:= Robot ,
Enable:= TRUE ,
bRegulatorOn:= gRobot.bGroupPowerEnable ,
bDriveStart:=gRobot.bGroupPowerEnable ,
Status=>,
Error => gRobot.bError,
ErrorID => gRobot.ErrorID,
);
fbGroupEnable(
AxisGroup:= Robot,
Execute:= fbGroupPower.Status ,
CompatibilityOptions:= ,
Done=> gRobot.bGroupEnableDone,
Busy=>,
Error=> gRobot.bError,
ErrorID=> gRobot.ErrorID,
);
IF gRobot.bMoveStart THEN
gRobot.bGroupABS_Execute := TRUE;
END_IF
fbMoveDirectionABS(
bExecute:= (fbGroupEnable.Done AND gRobot.bGroupABS_Execute),
lrX:= gRobot.lrX_Position ,
lrY:= gRobot.lrY_Position,
lrZ:=gRobot.lrZ_Position ,
bDone=> gRobot.bMoveDirABS_Done ,
bActive=> gRobot.bMoveDirABS_Activ );

Это упрощенный пример, задачей которого было продемонстрировать работу блоков и показать методику работы с этими блоками из библиотеки SoftMotion в среде CodeSys 3.5.
Данный пример реализован на конкретном оборудовании. Если вы все выполнили правильно, ОСИ ВРАЩАЮТСЯ и ВЗАИМОДЕЙСТВУЮТ.