6.6 应用案例
6.6.1 应用案例1
（1）工作内容
如图6-1所示，机器人在检测到发生碰撞等异常的冲击力时立即紧急停止。
图6-1 机器人在检测到异常冲击力时紧急停止
（2）样例程序
'——【设置控制模式组号=0时的各技术参量】
P_FsStf0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——刚度系数=0（N/mm）。
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——阻尼系数=0。
P_FsMod0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——力觉控制模式（全轴做位置控制）。
M_FsCod0=0 '——采用力觉工具坐标系。
'——【设置控制特性组号=0时的各技术参量】
P_FsGn0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——增益=0μm/N。
P_FsFLm0=（+50.00，+50.00，+50.00，+0.50，+0.50，+0.50）（0，0） '——设置各轴“作用力检测设定值”。
P_FsFCd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——作用力指令值（未设置）。
P_FsSpd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设定速度（未设置）。
P_FsSwF0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设定模式切换判定值。
'***<主程序>***
Def Act 1，M_FsLmtS=1 GoTo *XERR，F '——如果状态变量M_FsLmtS的值=1（超过“作用力检测设定值”），则跳转执行中断程序“XERR”。
Spd 10 '——设置速度。
Fsc On，0，0，1 '——力觉控制ON。
Act 1=1 '——设置“中断区间有效起点”。
Mvs P1 '——移动到P1点。
Fsc Off '——“力觉控制OFF”。
End
*XERR '——子程序号。
Act 1=0 '——退出Act 1中断区间。
Error 9100 '——报警发生。
End '——结束。
（3）说明
如果各轴的实际作用力或力矩超过状态变量P_FsFLm0设定的“作用力检测设定值”，状态变量M_FsLmtS的值会由0→1变化。当状态变量M_FsLmtS=1时，就执行“中断程序*XERR”。
6.6.2 应用案例2
（1）工作内容
如图6-2所示，同时检测机器人推入力和位置、判定装配工作是否完成。
①在Z坐标5mm以下，并且检测到在FZt方向作用力超过+18N时，判断为装配作业成功。
②安装开始后5s内不满足上述条件时，判断为安装失败并输出报警。
图6-2 位置及作用力检测
（2）样例程序
'——【设置控制模式组号=0时的各技术参量】
P_FsStf0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置刚度系数（N/mm）。
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置阻尼系数。
P_FsMod0=（+2.00，+2.00，+1.00，+0.00，+0.00，+0.00）（0，0） '——设置力觉控制模式（X，Y：刚度控制，Z：作用力控制）。
M_FsCod0=0 '——采用力觉工具坐标系。
'——【设置控制特性组号=0时的各技术参量】
P_FsGn0=（+2.00，+2.00，+2.00，+0.00，+0.00，+0.00）（0，0） '——设置增益（μm/N）。
P_FsFLm0=（+0.00，+0.00，+0.00，+0.0，+0.0，+0.0）（0，0） '——设置作用力检测设定值（N）。
P_FsFCd0=（+0.00，+0.00，+20.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力指令值（Z轴=20N）。
P_FsSpd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设定速度。
P_FsSwF0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值。
'***<主程序>***
Def MoTrg 1，（（P_Fbc.Z<=5）AND（P_FsCurD.Z>18）） '——定义#1Mo组合条件（Z轴位置<5mm以及Z向作用力>18N）。
Def Act 1，M_MoTrg（1）=1 GoTo *XOK，F '——设定#1Mo组合条件=1就跳转执行“XOK，F”中断程序。
Mvs PStart '——移动到PStart位置。
SetMoTrg 1 '——设置#1Mo组合条件有效。
Fsc On，0，0，1 '——“力觉控制ON”。
Act 1=1 '——进入中断有效区间。
M_Timer（1）=0 '——定时器清零。
*LBL1：If M_Timer（1）<5000 Then GoTo *LBL1 '——判断：如果工作时间大于5s，则执行下一行。
Fsc Off '——力觉控制OFF。
Error 9100 '——报警。
End '——结束。
*XOK '——“XOK”中断程序（如果装配完成就回程）。
Act 1=0 '——中断区间无效。
SetMoTrg 0 '——设置Mo组合条件无效。
Fsc Off '——力觉控制OFF。
H Open 1——张开抓手。
P2=P_Fbc '——获取反馈位置，设置P2点。
P2.Z=P2.Z+100 '——在当前的位置以Z方向+100mm的位置作为目标位置。
Mvs P2 '——移动到P2点。
End '——结束。
（3）说明
①使用Def MoTrg指令定义#1Mo组合条件。
②设置#1Mo组合条件#1后，启动力觉控制、执行对中断程序的条件判断。
③作业开始5s内未进入中断程序，即输出9100报警，程序结束。
6.6.3 应用案例3
（1）工作内容
如图6-3所示为锁存数据工程案例。
①锁存推入动作中的位置数据和作用力数据，计算弹簧部件的弹簧常数。
②要求在FZt超出10N的时间点，保存各轴的位置数据和作用力数据（锁存功能）。
③能使机器人动作到最终的推入位置，根据锁存的当前的位置数据和作用力数据计算弹簧常数。
图6-3 工程案例—锁存数据
（2）样例程序
'——【设置控制模式=0时的各技术参量】
P_FsStf0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——刚度系数（N/mm）。
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——阻尼系数。
P_FsMod0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置力觉控制模式（全轴位置控制）。
M_FsCod0=0 '——选择力觉工具坐标系。
'——【设置控制特性组号=0时的各技术参量】
P_FsGn0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置增益（μm/N）。
P_FsFLm0=（+200.00，+200.00，+10.00，+7.0，+7.0，+7.0）（0，0） '——设置各轴“作用力检测设定值”。
P_FsFCd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力指令值。
P_FsSpd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设定速度。
P_FsSwF0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值。
'***<计算弹簧常数程序>***
Mvs PStart '——移动到PStart位置上方。
Spd 10——设置速度。
Fsc On，0，0，1 '——力觉控制ON。
Fine 100 '——等待定位动作结束。
Mvs PEnd '——移动到PEnd结束位置。
MFD=P_FsCurD.Z-P_FsLmtD.Z '——计算Z轴上作用力之差。
MPD=Abs（P_Fbc.Z-P_FsLmtP.Z） '——计算位移量（mm）绝对值。
MSR=MFD/MPD '——计算弹簧常数（N/mm）。
Fsc Off '——力觉控制OFF。
End '——结束。
（3）说明
①FZt=10N时，锁存“位置数据”和“作用力数据”。
②以10mm/s的速度运动到位置Pend，缓慢移动压迫弹簧。
③在弹簧被压迫的过程中，弹簧的反作用力超过10N的时间点的“位置数据”“作用力数据”分别被保存在状态变量P_FsLmtP、P_FsLmtD中。
④到达目标位置后读取当前的位置数据、作用力数据（P_Fbc、P_FsCurD），根据以上数据计算出弹簧常数，保存为变量MSR（本例中在计算时假设目标位置的反作用力与10N相比足够大）。