Hi,
My task is to make a contact to workpiece with some pre defined force. So I am starting with HOME position and reading FTT values then commanding KUKA to move on -ve Z direction based on current force feedback. Below is the code-
Code
&ACCESS RVP
&REL 68
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF ForceLowering()
REAL kp, desiredFz, Fz, FError, ZKorr
FRAME relFrame
;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)
;ENDFOLD (INI)
$TOOL=TOOL_DATA[2]
$BASE=$NULLFRAME
; We need 5N contact force
desiredFz=-5.0
kp=0.5
; 5mm approximation
$APO.CDIS = 5
;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 5.6.13,%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)
$H_POS=XHOME
PTP XHOME
;ENDFOLD
REPEAT
relFrame = {X 0,Y 0,Z 0,A 0,B 0,C 0}
readFTT(ftt[,])
Fz = ftt[3,1]
FError = desiredFz - FZ
ZKorr = FError * kp
relFrame.Z = ZKorr
PTP_REL relFrame C_PTP
UNTIL (ZKorr >= 0)
;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 )
$H_POS=XHOME
PTP XHOME
;ENDFOLD
END
Display More
While running this code, it shows "Approximation not possible" (Please see the attached screenshot). I can clearly see flicking in motion (Since it stops and starts the motion). The motion is not smooth.
I want to follow the smooth motion.
-
Thanks