&ACCESS RVP &REL 5 DEF RoboDKsync542() DECL INT io_id DECL INT j_id DECL INT program_id ;------------------------------------------------- ; Define Digital input linked to the probe INTERRUPT DECL 5 WHEN $IN[1] DO ProbeTriggered() ;------------------------------------------------- BAS(#INITMOV,0) PTP $AXIS_ACT $ORI_TYPE = #VAR $CIRC_TYPE = #BASE $VEL.ORI1 = 200 $VEL.ORI2 = 200 $ACC.ORI1 = 100 $ACC.ORI2 = 100 $VEL.CP=3 $ACC.CP=10 $BASE = {FRAME: X 0,Y 0,Z 0,A 0,B 0,C 0} $TOOL = {FRAME: X 0,Y 0,Z 0,A 0,B 0,C 0} $ADVANCE = 0 COM_ROUNDM = -1 COM_ACTION = 0 COM_ACTCNT = 0 $APO.CPTP = 0 $APO.CDIS = 0 WHILE COM_ACTION >= 0 SWITCH COM_ACTION CASE 1 ; do nothing COM_ACTCNT = COM_ACTCNT + 1 COM_ACTION = 0 CASE 2 IF COM_ROUNDM >= 0 THEN PTP COM_E6AXIS C_PTP ELSE PTP COM_E6AXIS ENDIF COM_ACTION = 0 CASE 3 IF COM_ROUNDM >= 0 THEN LIN COM_E6POS C_DIS ELSE LIN COM_E6POS ENDIF COM_ACTION = 0 CASE 4 IF COM_ROUNDM >= 0 THEN CIRC COM_POS,COM_E6POS C_DIS ELSE CIRC COM_POS,COM_E6POS ENDIF COM_ACTION = 0 CASE 5 COM_ACTCNT = COM_ACTCNT + 1 $TOOL = COM_FRAME COM_ACTION = 0 CASE 6 COM_ACTCNT = COM_ACTCNT + 1 $VEL.CP = COM_VALUE1 COM_ACTION = 0 CASE 7 COM_ACTCNT = COM_ACTCNT + 1 IF COM_VALUE1 > 0 THEN $VEL.CP = COM_VALUE1 ENDIF IF COM_VALUE2 > 0 THEN ;$VEL.ORI1 = COM_VALUE2 ;$VEL.ORI2 = COM_VALUE2 FOR j_id=1 TO 6 IF COM_VALUE2 > 100 THEN $VEL_AXIS[j_id] = 100 ELSE $VEL_AXIS[j_id] = COM_VALUE2 ENDIF ENDFOR ENDIF IF COM_VALUE3 > 0 THEN $ACC.CP = COM_VALUE3 ENDIF IF COM_VALUE4 > 0 THEN ;$ACC.ORI1 = COM_VALUE4 ;$ACC.ORI2 = COM_VALUE4 FOR j_id=1 TO 6 IF COM_VALUE4 > 100 THEN $ACC_AXIS[j_id] = 100 ELSE $ACC_AXIS[j_id] = COM_VALUE4 ENDIF ENDFOR ENDIF COM_ACTION = 0 CASE 8 COM_ACTCNT = COM_ACTCNT + 1 IF COM_ROUNDM >= 0 THEN IF COM_ROUNDM > 100 THEN $APO.CPTP = 100 ELSE $APO.CPTP = COM_ROUNDM ENDIF $APO.CDIS = COM_ROUNDM $ADVANCE = 1 ELSE $APO.CPTP = 0 $APO.CDIS = 0 $ADVANCE = 0 ENDIF COM_ACTION = 0 CASE 9 COM_ACTCNT = COM_ACTCNT + 1 WAIT SEC COM_VALUE1 COM_ACTION = 0 CASE 10 COM_ACTCNT = COM_ACTCNT + 1 io_id = COM_VALUE1 IF COM_VALUE2 > 0.5 THEN $OUT[io_id] = TRUE ELSE $OUT[io_id] = FALSE ENDIF COM_ACTION = 0 CASE 11 PTP COM_E6AXIS LIN COM_FRAME WAIT SEC 0 COM_ACTION = 0 CASE 12 COM_ACTCNT = COM_ACTCNT + 1 io_id = COM_VALUE1 IF COM_VALUE2 > 0.5 THEN WAIT FOR $IN[io_id]==TRUE ELSE WAIT FOR $IN[io_id]==FALSE ENDIF COM_ACTION = 0 CASE 13 ;----- Run program COM_VALUE1 --------- ; (to trigger from RoboDK: use robot.RunCodeCustom("program id", INSTRUCTION_CALL_PROGRAM) program_id = COM_VALUE1 SWITCH program_id CASE 1 ; -- run program 1 -- ; Drill() ; ------------------- CASE 2 ; -- run program 2 -- ; Cut() ; ------------------- ENDSWITCH ;-------------------------------------------- COM_VALUE1 = 0 COM_ACTION = 0 CASE 14 COM_VALUE1 = 0 INTERRUPT ON 5 ; arm interrupt LIN COM_FRAME INTERRUPT OFF 5 ; nothing detected, disarm the interrupt COM_ACTION = 0 ; DEFAULT ; COM_ACTION = 0 ENDSWITCH ENDWHILE END ;-------------------------------------------- DEF ProbeTriggered() ; Disarm interrupt to prevent double-triggers INTERRUPT OFF 5 ; Change the global variable so we can read if the probe was triggered COM_VALUE1 = 1 ;- BRAKE ; Halt motion ;- LIN_REL {Z 100} ; move straight up 100mm ;- RESUME ; cancel MovTowardsP2 subroutine, send pointer back into top-level program END