https://pastein.ru/t/8IA
Загрузка данных
IF GVL.regim = 3 AND NOT GVL.NoHome THEN
// ============================================================ //
// БЛОК ПАРКОВКИ НАД ПРОМЫВКАМИ ПРИ ДЛИТЕЛЬНОМ ПРОСТОЕ
// ============================================================ //
// Таймер считает время, пока автооператор абсолютно свободен
parktimer(IN := (NOT gvl.aobusy AND NOT gvl.aoparkbusy AND NOT gvl.resetactive), PT := T#3S);
IF parktimer.Q AND NOT gvl.aobusy AND NOT gvl.aoparkbusy THEN
parkTarget := 0;
// Сценарий 1: Если робот застрял на позиции 0, отправляем его в буфер отстоя 102
IF gvl.aoposition = 0 THEN
parkTarget := 102;
// Сценарий 2: Если робот стоит над занятой ванной с длительным процессом
ELSIF (gvl.aoposition >= 1 AND gvl.aoposition <= 17) AND gvl.vanbusy[gvl.aoposition] THEN
// Переводим проверку времени в безопасный секундный DINT
IF (DT_TO_DINT(gvl.vanendtime[gvl.aoposition]) - DT_TO_DINT(gvl.systime)) >= MAX_PROCESS_TIME THEN
// Ищем ближайшую свободную ванну промывки для отстоя
parkTarget := FindNearestFreeRinseVat(gvl.aoposition);
END_IF;
END_IF;
// Если цель парковки определена, инициируем путевое задание отстоя
IF parkTarget > 0 AND (parkTarget <> gvl.aoposition) THEN
gvl.aojobsource := gvl.aoposition;
gvl.aoJobDest := parkTarget;
// ВАЖНО: Взводим ОБА флага, чтобы CyclicCall понимал, что робот занят парковкой
gvl.aobusy := TRUE;
gvl.aoparkbusy := TRUE;
// Парковка — это чистый переезд, поэтому стартуем сразу с Фазы 5 или 6!
// Пропускаем фазы открытия поддона и опускания за деталью у источника
gvlDriveInterface.xDriveDone := FALSE; // Очищаем старый флаг привода
horizCmdActive := TRUE;
aoPhase := 6; // ИСПРАВЛЕНО: прыгаем сразу в Фазу 6 (горизонтальный ход к приемнику)
END_IF;
END_IF;
// Блок контроля движения в режиме парковки
IF gvl.aoparkbusy THEN
// Вызываем Horizmove, которая запишет координаты parkTarget (aojobdestination) в dwTargetPos
Horizmove(getorput := FALSE);
// Пинаем привод на старт, так как для парковки мы зашли в обход стандартного цикла
IF NOT gvlDriveInterface.xDriveBusy AND NOT gvlDriveInterface.xDriveDone THEN
gvlDriveInterface.xCmdTrigger := TRUE;
END_IF;
// Ожидаем успешного приезда в зону отстоя строго по флагу xDriveDone от Leadshine
IF gvlDriveInterface.xDriveDone THEN
horizCmdActive := FALSE;
parkTarget := -1;
// Освобождаем робота: гасим оба флага занятости
gvl.aoparkbusy := FALSE;
gvl.aobusy := FALSE;
aoPhase := 0;
END_IF;
END_IF;
// ============================================================ //
CyclicCall();
// END_IF
IF gvl.candidate > 0 AND aoPhase = 0 THEN
// Данные извлекаются заново – гарантирует актуальность состояния подвески
a := gvl.candidate;
sourcePos := gvl.podveskas[a].currentPos;
route := gvl.podveskas[a].processType;
curStep := gvl.podveskas[a].currentStep;
liftFault := FALSE; // Сброс флага ошибки
// aoPhase остаётся 0, но в следующем условии будет переключён на 1
END_IF
// //планирование перемещений подвесок
IF lineActive OR NOT gvl.aoBusy THEN //Должна быть активна линия + Не должно быть выполняющейся операции, иначе - выход
GVL.Oscillaciya_Run := TRUE;
// Инициализация шагового автомата
IF gvl.aoBusy THEN
// Инициализация шагового автомата
IF aoPhase = 0 AND (NOT GVL.aoParkBusy) THEN
aoPhase := 1;
END_IF;
// ФАЗА 1: Поддон открыть
// -------------------------------------------------------------------------
IF aoPhase = 1 THEN
IF Trayopen() THEN
aoPhase := 2;
END_IF;
END_IF;
// ФАЗА 2: Опускание за деталью у источника
// -------------------------------------------------------------------------
IF aoPhase = 2 THEN
IF LiftDown() THEN
// Включаем программный затвор: мы готовы к новому маневру хода
xWaitDrive := TRUE;
aoPhase := 3;
horizCmdActive := TRUE; // Активируем функцию Horizmove (в ней сработает фронт старта)
moveStartTime := gvl.sysTime;
END_IF;
END_IF;
// ФАЗА 3: Горизонтальное перемещение к источнику (gvl.aoJobSource)
// -------------------------------------------------------------------------
IF aoPhase = 3 THEN
// Функция Horizmove один раз по фронту horizCmdActive запишет целевую координату
// источника в dwTargetPos и взведет gvlDriveInterface.xCmdTrigger := TRUE;
Horizmove(getorput := TRUE);
// СВЯЗАННОСТЬ: Как только нижний автомат Modbus принял команду от Horizmove и перешел в BUSY
IF gvlDriveInterface.xDriveBusy THEN
xWaitDrive := FALSE; // Мгновенно открываем затвор: старый True от Leadshine теперь проигнорирован
END_IF;
// Переходим к подъему СТРОГО если затвор открыт и Modbus-флаг подтверждает финиш НОВОГО маневра
IF (NOT xWaitDrive) AND gvlDriveInterface.xDriveDone THEN
aoPhase := 4;
horizCmdActive := FALSE; // Гасим разрешение хода для функции Horizmove
END_IF;
END_IF;
// ФАЗА 4: Подъём подвески из ванны-источника
// -------------------------------------------------------------------------
IF aoPhase = 4 THEN
IF LiftUp() THEN
// Включаем программный затвор: мы готовы к следующему маневру хода к приемнику
xWaitDrive := TRUE;
aoPhase := 5;
horizCmdActive := TRUE; // Активируем функцию Horizmove (в ней подготовится фронт старта для Фазы 6)
END_IF;
END_IF;
// ФАЗА 5: Закрывание каплеуловительного поддона
// -------------------------------------------------------------------------
IF aoPhase = 5 THEN
// Проверяем позицию источника (где сейчас физически стоим с поднятой деталью)
IF GVL.aoPosition = 0 OR GVL.aoPosition = 1 OR GVL.aoPosition = 16 OR
GVL.aoPosition = 13 OR GVL.aoPosition = 8 OR
(GVL.aoPosition >= 101 AND GVL.aoPosition <= 103) THEN
// На этих позициях поддон закрывать нельзя/не нужно, сразу прыгаем в ход
aoPhase := 6;
ELSE
// Для химических ванн закрываем поддон во избежание брака растворов
IF Trayclose() THEN
trayState := 6;
aoPhase := 6;
END_IF;
END_IF;
END_IF;
// ФАЗА 6: Горизонтальное перемещение к приёмнику (gvl.aoJobDest)
// -------------------------------------------------------------------------
IF aoPhase = 6 THEN
// Функция Horizmove один раз по фронту horizCmdActive (который мы взвели в Фазе 4)
// запишет координату приемника в dwTargetPos и взведет gvlDriveInterface.xCmdTrigger := TRUE;
Horizmove(getorput := FALSE);
// СВЯЗАННОСТЬ: Как только нижний автомат Modbus принял команду и перешел в BUSY
IF gvlDriveInterface.xDriveBusy THEN
xWaitDrive := FALSE; // Открываем затвор
END_IF;
// Переходим к Фазе 7 строго по финишу НОВОГО маневра переезда к приемнику
IF (NOT xWaitDrive) AND gvlDriveInterface.xDriveDone THEN
// Измеряем точное время чистого транзита хода (от конца Фазы 4 до финиша Фазы 6)
gvl.transitTimeMeasured := TIME_TO_REAL(gvl.sysTime - moveStartTime) / 1000.0;
aoPhase := 7;
horizCmdActive := FALSE; // Гасим разрешение хода
END_IF;
END_IF;
// ФАЗА 7: Поддон открыть над ванной назначения перед опусканием
// -------------------------------------------------------------------------
IF aoPhase = 7 THEN
IF Trayopen() THEN
aoPhase := 8;
END_IF;
END_IF;
// ФАЗА 8: Опускание подвески в приемник и завершение маневра
// -------------------------------------------------------------------------
IF aoPhase = 8 THEN
IF LiftDown() THEN
// Адаптируем предиктивную матрицу PV под износ механики хода
IF gvl.transitTimeMeasured < 60.0 AND gvl.transitTimeMeasured > 0.1 THEN
UpdateTransitTime(srcPos := gvl.aoJobSource, dstPos := gvl.aoJobDest, measured := gvl.transitTimeMeasured);
END_IF;
// Вызов вашей функции завершения маневра (инкремент шага, перезапись координат)
OnMoveComplete();
// Полностью освобождаем автооператора для планировщика CyclicCall
gvl.aoBusy := FALSE;
aoPhase := 0;
gvl.candidate := 0;
END_IF;
END_IF;
END_IF
//-------------------------------------------------------------------------------------------------------------------------------------------------
GVL.Majak_Avtooperatora := xDriveMoving;
IF gvl.aoBusy AND lineActive THEN
IF xLiftUpOverTravel OR xLiftDownOverTravel THEN // обработка аварийных концевиков
// Немедленная остановка привода
xLiftDriveStart := FALSE;
xLiftDriveStop := TRUE;
xLiftUpCmd := FALSE;
xLiftDownCmd := FALSE;
xTrayDriveOpen := FALSE;// поддон – убрать
gvl.aoBusy := FALSE; // автооператор свободен
aoPhase := 0; // фаза 0
horizCmdActive := FALSE; // TRUE = команда отправлена, ожидаем завершения
IF horizCmdActive THEN
horizCmdActive := FALSE;// Отмена активной горизонтальной команды
END_IF
gvl.aoBusy := FALSE;// Операция прервана
xError := TRUE;
aoPhase := 0;// Сброс фазы
END_IF
END_IF;
END_IF
END_IF