IF M8002=1 THEN
    M0 := 0;
    M3 := 0;
END_IF;

IF M0 THEN
    D132 := 1;
END_IF;

IF M3 THEN
    D168 := 100;
END_IF;

(****************************************************************************
***************
Входы/Выходы
X0 - Сигнал датчику нуля оси Z
Y0 - STEP. Выход на драйвер ШД
Y4 - DIR. Выход на драйвер ШД

Регистры
M0            Bit                Запускает/останавливает движение по заданным координатам
M1            Bit                Ручное движение вверх
M2            Bit                Ручное движение вниз
M3            Bit                Режим скорости ручного перемещения (0 - медленно S1, 1 - быстро S)
M5            Bit                Поимк нулевой точки

D9            DWORD        Уставка Z в импульсах
D128        INT                Высота стола (0.1 мм)
D132        INT                Высота заготовки (0.1 мм)
D136        INT                Количество импульсов на 1 мм перемещения по оси Z
D140        INT                Скорость движения по оси Z (мм/сек). Автоматический режим
D142        INT                Скорость движения по оси Z (мм/сек). Ручное управление
D144        INT                Текущая координата Axis Z (0.1 мм)
D148        INT                Текущая высота от заготовки до линзы Work Z (0.1 мм)
D152        INT                Текущий номер линзы Lens (1..4)
D156        INT                Фокусное расстояние линзы F1 (0.1 мм)
D160        INT                Фокусное расстояние линзы F2 (0.1 мм)
D164        INT                Фокусное расстояние линзы F3 (0.1 мм)
D168        INT                Фокусное расстояние линзы F4 (0.1 мм)
D172        INT                MaxZ. Максимальное значение Z
D176        INT                Фокусное расстояние выбранной линзы

D8140    DWORD        Текущее значение координаты Z в импульсах

M8029    Bit                Сигнал завершения позиционирования (в одном цикле)
M8147    Bit                1 - происходит перемещение (Занято), 0 - ожидание команды
********************************************************************************
************)

IF M8002 = 1 THEN  (* Выполняется однократно при старте программы *)
    M0 := FALSE;
    M1 := FALSE;
    M2 := FALSE;
    M4 := FALSE;
END_IF;

(* Вычисляем чатсоту импульсов *)
dK := INT_TO_DINT(D136);     (* K *)
HiSpeed :=  INT_TO_DINT(D140);     (* Hi Speed *)
HiFreq := (dK * HiSpeed) / 10;
LoSpeed :=  INT_TO_DINT(D142);     (* Lo Speed *)
LoFreq := (dK * LoSpeed) / 10;

CASE D152 OF
    1: (* Линза 1 *)
        D176 := D156;
        FLeanse := INT_TO_DINT(D156);
    2: (* Линза 2 *)
        D176 := D160;
        FLeanse := INT_TO_DINT(D160);
    3: (* Линза 3 *)
        D176 := D164;
        FLeanse := INT_TO_DINT(D164);
    4: (* Линза 4 *)
        D176 := D168;
        FLeanse := INT_TO_DINT(D168);
    ELSE
        D176 := 0;
        FLeanse := 0;
END_CASE;

(* Вычисляем заданную координату Z в импульсах *)
TableH := INT_TO_DINT(D128); (* Высота стола *)
Mesure := INT_TO_DINT(D132); (* Высота заготовки *)
MaxZ := INT_TO_DINT(D172 ); (* MaxZ смещение*)
DestZ :=  MaxZ -  (TableH + Mesure + FLeanse); (* Значение Z в 0,1 мм с учетом смещения MaxZ*)
DestPulsZ := DestZ * (dK/10);  (* Вычисляем количество импульсов для смещения *)
DMOV( TRUE, DestPulsZ , D9);

(* Вычисляем текущую координату Z в 0,1 мм*)
DMOV( TRUE, D8140 , HardZ);
AxisZ :=  MaxZ - (HardZ * 10) / dK;
D144 := DINT_TO_INT(AxisZ);

(* Вычисляем текущее расстояние от заготовки до линзы Z1 в 0,1 мм*)
WorkZ := AxisZ - (TableH + Mesure);
D148 := DINT_TO_INT(WorkZ);

(* Soft Limit max *)
IF AxisZ > MaxZ THEN
    M0 :=FALSE;
    M1 := FALSE;
END_IF;
(* Soft Limit min *)
IF (AxisZ <= (TableH + Mesure)) THEN
    M0 := FALSE;
    M2 := FALSE;
END_IF;

(* По сигналам ручного управления отключить автоматическое M0 *)
IF M1 OR M2 THEN
    M0 := FALSE;
    M5 := FALSE;
END_IF;
DDRVA( M0 ,  DestPulsZ, HiFreq, Y0 , Y4);

(* Переключение скорости перемещения в ручном режиме *)
IF M3 THEN
    HandSpeed := HiFreq;
ELSE
    HandSpeed := LoFreq;
END_IF;

DDRVI( M1 , -80000 , HandSpeed , Y0 , Y4 );
DDRVI( M2 , 80000 , HandSpeed , Y0 , Y4 );

Y7 := M8147;

(* Поиск референтной точки/ мащинный 0 по датчику *)
DZRN( M5 , HiFreq, MaxZ * dK,  X0 , Y0 );

(* Сброс M0 по оканчании позиционирования *)
IF M8029 THEN
    M0 := FALSE;
    M5 := FALSE;
END_IF;