Наконец то вчера удалось запустить пример на живом роботе. Как и ожидалось, выявилось несколько
досадных багов:
1. Ошибки синтаксиса
CASE #RRET_UP100
вместо
CASE #RET_UP100
2. Думал одно, а написал другое. Обработчики прерывания должны заканчиваться
оператором RESUME, а не RETURN.
3. Перед входом в главный цикл обязательно сбросить счетчик прекомпилятора комманд в 0, иначе
статусы работаю не верно:
$ADVANCE = 0
4. Движения с PTP поменял на LIN для лучшей наглядности.
Вот исправленный пример:
Переменные которые надо вставить в $config.dat
ENUM PROC_STATUS MAIN_UP, MAIN_DOWN, FORWARD200, UP100, RET_FORWARD200, RET_UP100
DECL E6POS UpperPos, LowerPos ;точки главной траектории
DECL PROC_STATUS ACT_STATUS, PRV_STATUS ;задаем переменные для хранения текущего и предыдущего статусов
DECL E6POS RetFw200, RetUp100 ;точки возврата из прерывания на главную траекторию
Сам исправленный пример:
DEF TestIRQ ( )
E6AXIS HOME
DECL INT sts
;FOLD INI;%{PE}
;FOLD BASISTECH INI
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
INTERRUPT ON 3
BAS (#INITMOV,0 )
;ENDFOLD (BASISTECH INI)
;FOLD USER INI
;Make your modifications here
;ENDFOLD (USER INI)
;ENDFOLD (INI)
;задаем прерывания
INTERRUPT DECL 5 WHEN $MEAS_PULSE[3]==TRUE DO IR_Forward200()
INTERRUPT DECL 4 WHEN $MEAS_PULSE[1]==TRUE DO IR_Up100()
sts = 0
RetFw200 = $NULLFRAME
RetUp100 = $NULLFRAME
$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[1]
HOME = {A1 0, A2 -90, A3 90, A4 90, A5 0, A6 0}
PTP HOME ;выйти на главную траекторию
;конвертируем HOME axis pos. в координаты верхней точки
UpperPos = FORWARD(HOME, sts)
;задаем нижнюю точку
LowerPos = UpperPos
LowerPos.Z = LowerPos.Z - 2000
;начальный статус движения (предполагаем, что робот вверху)
ACT_STATUS = #MAIN_DOWN
PRV_STATUS = ACT_STATUS
INTERRUPT ON 5
$ADVANCE = 0
LOOP
;статус менеджер
SWITCH ACT_STATUS
CASE #MAIN_DOWN
MainMoveDown() ;едем вниз и ждем срабатывания 5 прерывания
CASE #MAIN_UP
MainMoveUp() ;едем вверх и ждем срабатывания 5 прерывания
CASE #FORWARD200
Forward200() ;5 прерывание сработало, едем вперед и ждем срабатывания 4 прерывания
CASE #RET_FORWARD200
RetForward200() ;выходим на главную траекторию после движения вперед
CASE #UP100
Up100() ;4 прерывание сработало, движениу вперед остановлено, едем вверх
CASE #RET_UP100
RetUp100() ;возвращаемся вниз к точке срабатывания 4 прерывания
ENDSWITCH
WAIT FOR TRUE
ENDLOOP
END
;вниз по главной траектории
DEF MainMoveDown()
LIN LowerPos
ACT_STATUS = #MAIN_UP
END
;вверх по главной траектории
DEF MainMoveUp()
LIN UpperPos
ACT_STATUS = #MAIN_DOWN
END
;обработчик 5 прерывания
DEF IR_Forward200()
BRAKE F
$OV_PRO=100
INTERRUPT OFF 5
INTERRUPT ON 4
RetFw200 = $POS_RET
PRV_STATUS = ACT_STATUS
ACT_STATUS = #FORWARD200
RESUME
END
;обработчик 4 прерывания
DEF IR_Up100()
BRAKE F
RetUp100 = $POS_RET
ACT_STATUS = #UP100
RESUME
END
;просто вперед на 200 мм
DEF Forward200()
LIN_REL {X 200}
ACT_STATUS = #RET_FORWARD200
END
;назад к точке возврата из 5 прерывания
DEF RetForward200()
LIN RetFw200
INTERRUPT OFF 4
INTERRUPT ON 5
$OV_PRO=80
ACT_STATUS = PRV_STATUS ;продолжить прерванное движение: или вверх или вниз
END
;просто вверх на 100 мм
DEF Up100()
LIN_REL {Z 100}
ACT_STATUS = #RET_UP100
END
;назад к точке возврата из 4 прерывания
DEF RetUp100()
LIN RetUp100
ACT_STATUS = #RET_FORWARD200
END
|
