KUKA机器人电伺服移动到力的控制MOVE_TO_FORCE程序分析

移动焊钳到使用力 

GLOBALDEF SGM_MOVE_TO_FORCE (FORCE :IN, MovementType:IN, GunClosingReason:IN, SGEQ_ParamSet:IN, SGRS_ParamSet:IN )

力值的给入图:

声明变量:

DECL SG_MoveType_TMovementType

DECL ForceMovementType_T GunClosingReason

DECL SGEQg_POINT_T SGEQ_ParamSet

DECLREAL FORCE, SPEED, factor, SavedExValue

DECLINT HANDLE

DECL SGRSg_POINT_T SGRS_ParamSet

声明结束

进入程序

SG_LogbookCurrent.TargetForce = FORCE

factor = 1.0

IF(SGEQ_ActivePointCond)THEN SGEQ_ActivePointCond=false

说明一下的IF语句里暂时没有使用  

SGEQ_ExpectedTime =SGEQ_TimeExpectedHelper(SGEQ_ParamSet)

SGEQ_SpeedProz = SGEQ_CalcSpeedAndTimes(SGEQ_ExpectedTime)

SPEED = SGEQ_SpeedProz

SGEQ_CurrentParamSet = SGEQ_ParamSet

IF NOT SGEQ_ContactMode THEN

SGEQ_INIT(SGEQ_ParamSet,SGEQ_ExpectedTime)

ENDIF

IF(ServoGun[SG_ActiveGunNumber].GunControl == #Position)THEN

TRIGGERWHENDISTANCE= 1 DELAY= 0 DO SGEQ_Start(SGEQ_CurrentParamSet,SGEQ_ExpectedTime)Prio=-1

ELSE

SavedExValue = SGL_GET_AXIS_VALUE(P_APPROX)

P_APPROX = SGEQ_StartPoint

SGL_SET_AXIS_VALUE(P_APPROX, SavedExValue)

ENDIF

SGL_MoveToPos(SGEQ_StartPoint,SGEQ_StartPoint,#CLS_OPEN,MovementType)

ENDIF

-----------------endif--------------

IF MovementType == #SG_PTP THEN  移动方式是PTP

BAS (#ACC_PTP,100 )  使用 ACC加速度100%

BAS (#VEL_PTP,100 )  使用 VEL速度100%

$APO.CPTP=1    使用接近距离

IF(SGEQ_ActivePointCond)THEN SGEQ_ActivePointCond=false

说明一下的IF语句里暂时没有使用  

$VEL_EXTAX[SG_ExtaxActive]= SPEED * factor

IF(ServoGun[SG_ActiveGunNumber].GunControl == #Force)THEN

TRIGGERWHENDISTANCE= 1 DELAY= 0 DO SGEQ_Start(SGEQ_CurrentParamSet,SGEQ_ExpectedTime)Prio=-1

ENDIF

ELSE

IF(GunClosingReason == #SingleWear)THEN

factor = SG_SingleWearSpeedF

$VEL_EXTAX[SG_ExtaxActive]= SG_ConstSpeed * factor

ELSE

$VEL_EXTAX[SG_ExtaxActive]= 100

ENDIF

ENDIF

-------------end-------------------------

---------------if-----------------

IF(GunClosingReason == #Init)THEN

焊接时GunClosingReason =#WELD 测量时GunClosingReason == #Init

PTP P_APPROX C_PTP

ELSE

PTP P_APPROX C_PTP

ENDIF

----------end------------------

$VEL_EXTAX[SG_ExtaxActive]=SG_ConstSpeed * factor

-----提前运行中外部轴的轴速度-----

-----SG_ConstSpeed 之前在更换电极时算得速度------------

IF(ServoGun[SG_ActiveGunNumber].GunControl == #Position)THEN

ServoGun[1]=#Position

ACTIVATE_FORCE_MODE((6+SG_ExtaxActive), FORCE, SGL_CalcPartPos(SG_CurrentCmd),0)

----使用力功能进行移动--------给入焊接控制器给的力值进行力的移动-------

ENDIF 

IF(ServoGun[SG_ActiveGunNumber].GunControl == #Force)THEN

由于ServoGun[1]= #Position所以不执行力模式

ACTIVATE_FORCE_MODE((6+SG_ExtaxActive), FORCE, SGL_CalcPartPos(SG_CurrentCmd),SGEQ_AFM_ConstSpeedUser)

ENDIF

PTP P_PART C_PTP   在有力的情况下执行点到点的位置P_PART

P_PART补偿之后的力的值

C_PTP带轨迹逼近-提前执行

TRIGGERWHENDISTANCE=0 DELAY=0 DO SGM_VELEXAXFULL()PRIO=-1

提前触发激活  外部轴速度$VEL_EXTAX[1]=100

$APO.CDIS激活逼近范围

PTP P_WELD与ACTIVATE_FORCE_MODE()函数和$VEL_EXTAX[1]一起使用实现力的控制

PTP P_WELD 执行焊接轨迹点 

ENDIF