Hi, I have a KR10 R900 robot running KSS 8.6.6
I've been trying to get Conveyor Tech to work and for most part it does. The system consists of a camera that sends the robot a coordinate to go to, while following the conveyor, and it does.
But my problem is how can I make the robot do nothing when a zero coordinate is given, i.e. camera sees nothing, and then carry on with its following. Any suggestions??
I've included the code below, up to the case statement where the movement is initiated.
Code
;FOLD DECLARATIONS
DECL INT ret
DECL INT err
;DECL CHAR JUMP
;ENDFOLD (DECLARATIONS)
;FOLD INI
;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)
XPick=XPickNominal
;ENDFOLD (INI)
;FOLD CONV INI
CONV_REINIT_MD()
;Alarm outputs initialisations
$OUT[Z_EMS_OUT_NBR[1]]=TRUE
$OUT[Z_DIST_ALARM_OUT_NBR[1]]=TRUE
$OUT[Z_EMS_OUT_NBR[2]]=TRUE
$OUT[Z_DIST_ALARM_OUT_NBR[2]]=TRUE
$OUT[Z_EMS_OUT_NBR[3]]=TRUE
$OUT[Z_DIST_ALARM_OUT_NBR[3]]=TRUE
$OUT[Z_EMS_OUT_NBR[4]]=TRUE
$OUT[Z_DIST_ALARM_OUT_NBR[4]]=TRUE
$OUT[Z_EMS_OUT_NBR[5]]=TRUE
$OUT[Z_DIST_ALARM_OUT_NBR[5]]=TRUE
;Interrupts Conveyor 1
INTERRUPT DECL Z_ALARM_DIST_INT_NBR[1] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[1]]>=R_ALARM_DIST_CONV[1] DO INT_CONV_ALARM(1)
INTERRUPT DECL Z_MAX_DIST_INT_NBR[1] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[1]]>=R_MAX_DIST_CONV[1] DO INT_CONV_MAX_DIST(1)
INTERRUPT DECL Z_EMS_INT_NBR[1] WHEN $STOPMESS==TRUE DO INT_CONV_EMS(1)
;Interrupts Conveyor 2
INTERRUPT DECL Z_ALARM_DIST_INT_NBR[2] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[2]]>=R_ALARM_DIST_CONV[2] DO INT_CONV_ALARM(2)
INTERRUPT DECL Z_MAX_DIST_INT_NBR[2] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[2]]>=R_MAX_DIST_CONV[2] DO INT_CONV_MAX_DIST(2)
INTERRUPT DECL Z_EMS_INT_NBR[2] WHEN $STOPMESS==TRUE DO INT_CONV_EMS(2)
;Interrupts Conveyor 3
INTERRUPT DECL Z_ALARM_DIST_INT_NBR[3] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[3]]>=R_ALARM_DIST_CONV[3] DO INT_CONV_ALARM(3)
INTERRUPT DECL Z_MAX_DIST_INT_NBR[3] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[3]]>=R_MAX_DIST_CONV[3] DO INT_CONV_MAX_DIST(3)
INTERRUPT DECL Z_EMS_INT_NBR[3] WHEN $STOPMESS==TRUE DO INT_CONV_EMS(3)
;Interrupts Conveyor 4
INTERRUPT DECL Z_ALARM_DIST_INT_NBR[4] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[4]]>=R_ALARM_DIST_CONV[4] DO INT_CONV_ALARM(4)
INTERRUPT DECL Z_MAX_DIST_INT_NBR[4] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[4]]>=R_MAX_DIST_CONV[4] DO INT_CONV_MAX_DIST(4)
INTERRUPT DECL Z_EMS_INT_NBR[4] WHEN $STOPMESS==TRUE DO INT_CONV_EMS(4)
;Interrupts Conveyor 5
INTERRUPT DECL Z_ALARM_DIST_INT_NBR[5] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[5]]>=R_ALARM_DIST_CONV[5] DO INT_CONV_ALARM(5)
INTERRUPT DECL Z_MAX_DIST_INT_NBR[5] WHEN $SEN_PREA_C[Z_SEN_PREA_NBR[5]]>=R_MAX_DIST_CONV[5] DO INT_CONV_MAX_DIST(5)
INTERRUPT DECL Z_EMS_INT_NBR[5] WHEN $STOPMESS==TRUE DO INT_CONV_EMS(5)
$ADVANCE=1
;ENDFOLD CONV INI
;JUMP:
; Clear Buffer
ret = CONV_DELETE_ALL_WPS(1,err)
; CP_VEL_MODE should be 5, so DLIN can be used
$CP_VEL_MODE=5
; Move to home position
;FOLD PTP HOME Vel= 100 % DEFAULT;%{PE}%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART = FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS (#PTP_PARAMS,100 )
SET_CD_PARAMS( 0)
$H_POS=XHOME
PTP XHOME
;ENDFOLD
WAIT FOR rCameraY>0
; Initialise Conveyor
;Fold Conveyor.INI_OFF LinConveyor1 ;%{PE}
;Fold ;%{h}
;Params IlfProvider=KukaRoboter.Conveyor.InlineForms.ConveyorIniOffInlineForm; ConveyorName=LinConveyor1; ConveyorId=1
;EndFold
CONV_INI_OFF(1)
;EndFold
; Switch ON Conveyor
;Fold Conveyor.ON LinConveyor1 ;%{PE}
;Fold ;%{h}
;Params IlfProvider=KukaRoboter.Conveyor.InlineForms.ConveyorOnInlineForm; ConveyorName=LinConveyor1; ConveyorId=1
;EndFold
CONV_ON(1)
;EndFold
; Start Conveyor Tracking
;Fold Conveyor.FOLLOW LinConveyor1, Movement 1, Cancel on: Max_time 60, Input 1, Input-Level TRUE, Flag 1, Flag-Level TRUE, WaitDist 1300, MaxDist 1500 ;%{PE}
;Fold ;%{h}
;Params IlfProvider=KukaRoboter.Conveyor.InlineForms.ConveyorFollowInlineForm; ConveyorName=LinConveyor1; ConveyorId=1; Movement=1; Cancel on: Max_time=60; Input=1; Input-Level=TRUE; Flag=1; Flag-Level=TRUE; WaitDist=1300; MaxDist=1500
;EndFold
CONV_FOLLOW (1, 1, 60, 1, TRUE, 1, TRUE, 1300, 1500)
CONV_MOV (1, 1)
;EndFold
; Stop Conveyor Tracking
;Fold Conveyor.Quit LinConveyor1, Stop after error FALSE, Reset SEN_PREA_C TRUE ;%{PE}
;Fold ;%{h}
;Params IlfProvider=KukaRoboter.Conveyor.InlineForms.ConveyorQuitInlineForm; ConveyorName=LinConveyor1; ConveyorId=1; Stop after error=FALSE; Reset SEN_PREA_C=TRUE
;EndFold
CONV_QUIT(1,FALSE)
CONV_RESET_SEN_PREA(1)
;EndFold
LOOP
;IF rCameraY==0 then
;GOTO JUMP
;ENDIF
;FOLD LIN Pre Vel=2 m/s CPDAT12 Tool[1]:MainTool Base[0] ;%{PE}
;FOLD Parameters ;%{h}
;Params IlfProvider=kukaroboter.basistech.inlineforms.movement.old; Kuka.IsGlobalPoint=False; Kuka.PointName=Pre; Kuka.BlendingEnabled=False; Kuka.MoveDataName=CPDAT12; Kuka.VelocityPath=2; Kuka.CurrentCDSetIndex=0; Kuka.MovementParameterFieldEnabled=True; IlfCommand=LIN
;ENDFOLD
$BWDSTART = FALSE
LDAT_ACT = LCPDAT12
FDAT_ACT = FPre
BAS(#CP_PARAMS, 2.0)
SET_CD_PARAMS (0)
PTP XPre
;ENDFOLD
;Fold Conveyor.Skip LinConveyor1, Skip to part index 1, Movement 1, Cancel on: Max_time 60, Input 1, Input-Level TRUE, Flag 1, Flag-Level TRUE, WaitDist 1300, MaxDist 1500 ;%{PE}
;Fold ;%{h}
;Params IlfProvider=KukaRoboter.Conveyor.InlineForms.ConveyorSkipInlineForm; ConveyorName=LinConveyor1; ConveyorId=1; Skip to part index=1; Movement=1; Cancel on: Max_time=60; Input=1; Input-Level=TRUE; Flag=1; Flag-Level=TRUE; WaitDist=1300; MaxDist=1500
;EndFold
CONV_SKIP(1, 1, 1, 60, 1, TRUE, 1, TRUE, 1300, 1500)
CONV_MOV(1, 1)
;EndFold
;Fold Conveyor.Quit LinConveyor1, Stop after error FALSE, Reset SEN_PREA_C TRUE ;%{PE}
;Fold ;%{h}
;Params IlfProvider=KukaRoboter.Conveyor.InlineForms.ConveyorQuitInlineForm; ConveyorName=LinConveyor1; ConveyorId=1; Stop after error=FALSE; Reset SEN_PREA_C=TRUE
;EndFold
CONV_QUIT(1,FALSE)
CONV_RESET_SEN_PREA(1)
;EndFold
ENDLOOP
; Move back to home position
;FOLD PTP HOME Vel=10 % DEFAULT ;%{PE}
;FOLD Parameters ;%{h}
;Params IlfProvider=kukaroboter.basistech.inlineforms.movement.old; Kuka.IsGlobalPoint=False; Kuka.PointName=HOME; Kuka.BlendingEnabled=False; Kuka.MoveDataPtpName=DEFAULT; Kuka.VelocityPtp=10; Kuka.CurrentCDSetIndex=0; Kuka.MovementParameterFieldEnabled=True; IlfCommand=PTP
;ENDFOLD
$BWDSTART = FALSE
PDAT_ACT = PDEFAULT
FDAT_ACT = FHOME
BAS(#PTP_PARAMS, 10.0)
SET_CD_PARAMS (0)
PTP XHOME
;ENDFOLD
END
DEF CONV_MOV(Z_CONV_NBR:IN,Z_MOV_NBR:IN)
;FOLD INI CONV_MOV
INT Z_CONV_NBR,Z_MOV_NBR
IF B_CALL_CONV_MOV THEN
CONTINUE
$OUT[Z_DIST_ALARM_OUT_NBR[1]]=TRUE
CONTINUE
$OUT[Z_DIST_ALARM_OUT_NBR[2]]=TRUE
CONTINUE
$OUT[Z_DIST_ALARM_OUT_NBR[3]]=TRUE
CONTINUE
$OUT[Z_DIST_ALARM_OUT_NBR[4]]=TRUE
CONTINUE
$OUT[Z_DIST_ALARM_OUT_NBR[5]]=TRUE
CONTINUE
$OUT[Z_EMS_OUT_NBR[1]]=TRUE
CONTINUE
$OUT[Z_EMS_OUT_NBR[2]]=TRUE
CONTINUE
$OUT[Z_EMS_OUT_NBR[3]]=TRUE
CONTINUE
$OUT[Z_EMS_OUT_NBR[4]]=TRUE
CONTINUE
$OUT[Z_EMS_OUT_NBR[5]]=TRUE
B_QUIT_BECAUSE_MAX_DIST=FALSE
B_QUIT_BECAUSE_EMS=FALSE
SWITCH Z_CONV_NBR
CASE 1 ;*** CONVEYOR 1 ***
CONTINUE
IF NOT $T1 THEN
INTERRUPT ON Z_ALARM_DIST_INT_NBR[1]
INTERRUPT ON Z_MAX_DIST_INT_NBR[1]
INTERRUPT ON Z_EMS_INT_NBR[1]
ENDIF
SWITCH Z_MOV_NBR
;ENDFOLD INI CONV_MOV
;FOLD CONVEYOR 1 MOVEMENT GROUP 1
CASE 1
CONTINUE
WAIT FOR NOT $T1
; Calculate positions
XProcess.Y = rCameraY
XProcess02=XProcess
XProcess02.Y = XProcess.Y + 10
; Move to positions using DLIN
;FOLD DLIN.MOVE Process Vel.CP=15m/s Vel.Ori1=2000deg/s Vel.Ori2=2000deg/s Acc.CP=50m/s^2 Acc.Ori1=5000deg/s^2 Acc.Ori2=5000deg/s^2 Tool[1]:MainTool Base[1]:FlangeBase(LinConveyor1) CONT
;FOLD ;%{h}
;Params IlfProvider=kukaroboter.conveyor.inlineforms.dlin.dlinmoveinlineform; Kuka.IsGlobalPoint=False; Kuka.PointName=Process; Kuka.Dlin.VelCp=15; Kuka.MoveDataName=XProcess; Kuka.VelocityPath=2; Kuka.CurrentCDSetIndex=0; Kuka.MovementParameterFieldEnabled=True; Kuka.Dlin.VelOri1=2000; Kuka.Dlin.VelOri2=2000; Kuka.Dlin.AccCp=50; Kuka.Dlin.AccOri1=5000; Kuka.Dlin.AccOri2=5000; Kuka.Dlin.Cont=CONT
;ENDFOLD
$BWDSTART = FALSE
LDAT_ACT = LXProcess
FDAT_ACT = FProcess
CP_DYNLIN(15,2000,2000,50,5000,5000)
SET_CD_PARAMS (0)
LIN XProcess C_DIS C_DIS
$OUT[234]=TRUE
;ENDFOLD
;FOLD DLIN.MOVE Process02 Vel.CP=15m/s Vel.Ori1=2000deg/s Vel.Ori2=2000deg/s Acc.CP=50m/s^2 Acc.Ori1=5000deg/s^2 Acc.Ori2=5000deg/s^2 Tool[1]:MainTool Base[1]:FlangeBase(LinConveyor1)
;FOLD ;%{h}
;Params IlfProvider=kukaroboter.conveyor.inlineforms.dlin.dlinmoveinlineform; Kuka.IsGlobalPoint=False; Kuka.PointName=Process02; Kuka.Dlin.VelCp=15; Kuka.MoveDataName=XProcess; Kuka.VelocityPath=2; Kuka.CurrentCDSetIndex=0; Kuka.MovementParameterFieldEnabled=True; Kuka.Dlin.VelOri1=2000; Kuka.Dlin.VelOri2=2000; Kuka.Dlin.AccCp=50; Kuka.Dlin.AccOri1=5000; Kuka.Dlin.AccOri2=5000; Kuka.Dlin.Cont=empty
;ENDFOLD
$BWDSTART = FALSE
LDAT_ACT = LXProcess
FDAT_ACT = FProcess02
CP_DYNLIN(15,2000,2000,50,5000,5000)
SET_CD_PARAMS (0)
LIN XProcess02
$OUT[234]=FALSE
;ENDFOLD
;ENDFOLD
Display More