4.7　样例程序

4.7.1　样例程序1

（1）工作要求

机器人往Y轴方向动作，探寻对象物。发现对象物后，在对象物Y轴方向上保持施加一定的力，同时向X轴方向运动。如图4-6所示。

（2）样例程序

'——设置控制模式=0的各技术参量。

P_FsStf0=（+0.00，+0.01，+0.00，+0.00，+0.00，+0.00）（0，0）　'——刚度系数（设置Y轴=0.01（N/mm））。

P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——阻尼系数（不设定）。

P_FsMod0=（+0.00，+1.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——力觉控制模式（设置Y轴=作用力控制，因为需要保持Y向压力）。

M_FsCod0=1　'——设置采用力觉直交坐标系。

'——设置控制特性=0的各技术参量。

P_FsGn0=（+0.00，+1.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——增益（设置Y轴=1.0（10^-3mm/N））

P_FsFLm0=（+20.00，+0.50，+20.00，+5.00，+5.00，+5.00）（0，0）　'——设置作用力检测设定值（设置Y=0.5（N））

P_FsFCd0=（+0.00，+2.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——设置作用力指令（设置Y=2.0（N））

'——主程序。

Def Act 1，M_FsLmtS=1 GoTo *XMOV，S　'——定义Act 1：如果“作用力当前值”超过“检测设定值”的时候，跳转执行*XMOV，S中断程序。

FsLog On　'——开始采集力觉日志数据。

Fsc On，0，0，1　'——力觉控制ON，控制模式组号=0，控制特性组号=0，零点清零。

P1=P_Curr　'——设置P1=位置当前值。

P1.Y=P1.Y+200　'——定义P1.Y。

Spd 5　'——设置速度。

Act 1=1　'——Act 1中断区间起点。

Mvs P1　'——移动到P1点。在动作中如果检测到与对象物接触，则执行跳转处理。

Fsc Off　'——力觉控制OFF。

FsLog Off，7　'——结束力觉日志数据采集，创建日志文件No.7。

Act 1=0　'——Act 1中断区间终点。

End　'——主程序结束。

'——中断程序。

*XMOV

P2=P_FsCurP　'——设置P2为“力觉指令位置”。

P2.X=P2.X+100　'——设置P2.X的位置。

FsGChg 5，100，2　'——（FsGChg——切换控制特性指令）切换位置=5%，切换时间100ms，切换后控制特性组号=2。

Mvs P2　'——移动到P2点。在Y轴方向上保持施加2.0N的力，往X轴方向移动。

Fsc Off　'——力觉控制OFF。

FsLog Off，7　'——结束力觉日志数据的采集，创建日志文件No.7。

FsOutLog 7　'——通过FTP传送日志文件No.7到电脑上。

End　'——结束。

4.7.2　样例程序2

（1）工作要求

机器人在X、Y方向处于弹性工作状态，在Z轴方向上执行插入动作。插入时，如果作用力超过设置的“检测值”时，立即报警。如图4-7所示。

（2）样例程序

'——设置控制模式=0的各技术参量。

P_FsStf0=（+0.01，+0.01，+0.00，+0.00，+0.00，+0.00）（0，0）　'——刚度系数［设置X、Y=0.01（N/mm）］。

P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——阻尼系数（不设置）。

P_FsMod0=（+2.00，+2.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——力觉控制模式（X、Y轴=2：刚度控制）。

M_FsCod0=0　'——使用力觉工具坐标系。

'——设置控制特性=0的各技术参量。

P_FsGn0=（+80.00，+80.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——增益［设置X、Y轴=80（10^-3mm/N）］。

P_FsFLm0=（+20.00，+20.00，+10.00，+5.00，+5.00，+5.00）（0，0）　'——作用力检测设定值［设置Z轴=10（N）］。

P_FsFCd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——作用力指令（未设置）。

'——主程序。

Def Act 1，P_FsLmtX.Z=1 GoTo *ESCP，S　'——定义Act 1：如果Z轴方向“实际作用力”超过“检测设定值”时，跳转到中断程序*ESCP，S。

Mov P1　'——移动到P1点。

FsLog On　'——开始采集力觉日志数据。

Fsc On，0，0，1　'——力觉控制ON，控制模式组号=0，控制特性组号=0，零点清零。

Spd 5　'——设置速度。

Act 1=1　'——Act 1中断区间起点。

Mvs P2　'——移动到P2点。在Z轴方向上执行插入动作，Z轴上作用力如果超过“检测设定值”，执行中断插入处理。

Fsc Off　'——力觉控制OFF。

FsLog Off，1　'——结束力觉日志数据的采集，创建日志文件No.1。

Act 1=0　'——Act 1中断区间终点。

End　'——主程序结束。

'——中断插入程序。

*ESCP

Fsc Off　'——力觉控制OFF。

Spd 50　'——设置速度。

Mvs P1　'——退回到插入开始位置P1。

FsLog Off，1　'——结束力觉日志数据采集，创建日志文件No.1。

FsOutLog 1　'——通过FTP传送日志文件No.1到电脑上。

Error 9100　'——报警L9100发生。

End——程序结束。

4.7.3　样例程序3

（1）工作要求

机器人探索XY平面上的孔。发现孔的时候，算出该中心位置的X、Y坐标值。动作过程如图4-8所示。

（2）样例程序

'——设置控制模式=0的各技术参量。P_FsStf0=（+0.00，+0.00，+1.00，+0.00，+0.00，+0.00）（0，0）　'——刚度系数［设置Z轴=1（N/mm）］。

P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——阻尼系数（未设置）。

P_FsMod0=（+0.00，+0.00，+2.00，+0.00，+0.00，+0.00）（0，0）　'——力觉控制模式（设置Z轴为刚度控制）。

M_FsCod0=1　'——设置采用力觉直交坐标系。

'——设置控制特性=0的各技术参量。P_FsGn0=（+0.00，+0.00，+40.00，+0.00，+0.00，+0.00）（0，0）　'——增益［（Z轴=40（10^-3mm/N）］

P_FsFLm0=（+20.00，+20.00，+5.00，+5.00，+5.00，+5.00）（0，0）　'——设置“作用力检测设定值”［设置Z轴=5（N）］。

P_FsFCd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0）　'——作用力指令（未指定）。

'——主程序。

Def Act 1，P_FsLmtR.Z=1 GoTo *PCEN，S　'——定义Act 1：如果Z轴作用力大于“作用力检测设定值”时，跳转执行中断程序*PCEN，S。

P2=P1　'——设置P2点。

P2.X=P2.X+100　'——设置P2点X值（X向探索行程100mm）。

Fsc On，0，0，1　'——力觉控制ON。控制模式组号=0，控制特性组号=0，零点清零。

Mvs P1　'——移动到探索开始位置P1（探索平面在Z向-10mm的位置，往Z方向推入）。

Spd 5　'——设置速度。

Act 1=1　'——Act 1中断区间起点。

For M1=1 To 10　'——Y方向上每隔5mm执行探索孔的位置。

Mvs P1　'——移动到P1点。

Mvs P2　'——移动到P2点。

P1.Y=P1.Y+5　'——设置P1.Y的数值。

P2.Y=P1.Y　'——设置P2.Y的数值。

Mvs P2　'——移动到P2点。

Mvs P1　'——移动到P1点。

P1.Y=P1.Y+5　'——设置P1.Y的数值。

P2.Y=P1.Y　'——设置P2.Y的数值。

Next M1

Act 1=0　'——Act 1中断区间终点。

Fsc Off　'——力觉控制OFF。

End　'——主程序结束。

'——中断程序。

*PCEN

Dim PX（2），PY（2）　'——定义数组。

P0=P_Curr　'——设置P0位置。以探索到孔，停止的位置为基准。

PX（1）=P0　'——设置PX（1）位置。

PX（2）=P0　'——设置PX（2）位置。

PY（1）=P0　'——设置PY（1）位置。

PY（2）=P0　'——设置PY（2）位置。

PX（1）.X=P0.X+10　'——算出基准位置的X、Y方向上±10mm的位置。

PX（2）.X=P0.X-10　'——设置PX（2）位置。

PY（1）.Y=P0.Y+10　'——设置PY（1）位置。

PY（2）.Y=P0.Y-10　'——设置PY（2）位置。

Fsc Off　'——力觉控制OFF。

P_FsFLm0=（+2.00，+2.00，+5.00，+5.00，+5.00，+5.00）（0，0）　'——X、Y轴方向的“作用力检测设定值”变更为2（N）。

Fsc On，0，0，1　'——力觉控制ON。

MFLG=0　'——设置MFLG=0。

For M1=1 To 2

Mvs PX（M1） WthIf P_FsLmtR.X=1，Skip　'——在向PX（M1）的移动过程中，如果X轴上的作用力超过“作用力检测设定值”，就执行跳转。

IfM_SkipCq=1 Then　'——如果执行了跳转，则。

PX（M1）=P_FsLmtP　'——设置PX（M1）保持为第一次“超过作用力检测设定值的位置”。

MFLG=MFLG+1　'——设置MFLG。

EndIf

Mvs P0　'——移动到P0点。

Fsc Off　'——为了复位P_FsLmtP，使力觉控制从无效→有效。

Fsc On，0，0，1　'——力觉控制ON。

Next M1

For M1=1 To 2

Mvs PY（M1） WthIf P_FsLmtR.Y=1，Skip　'——在向PY（M1）的移动过程中，如果Y轴上的作用力超过“作用力检测设定值”，就执行跳转。

If M_SkipCq=1 Then　'——如果执行了跳转，则。

PY（M1）=P_FsLmtP　'——跳转的时候，保持PY（M1）=超过作用力检测设定值的位置。

MFLG=MFLG+1　'——设置MFLG。

EndIf

Mvs P0　'——移动到P0。

Fsc Off　'——力觉控制OFF。

Fsc On，0，0，1　'——为了复位P_FsLmtP，使力觉控制从无效→有效。

Next M1

If MFLG=4 Then　'——找到4个点。

PTMP=（PX（1）+PX（2））/2　'——设置PTMP。

P0.X=PTMP.X　'——设定X轴方向的中心位置为P0。

PTMP=（PY（1）+PY（2））/2　'——设置PTMP。

P0.Y=PTMP.Y　'——设定Y轴方向的中心位置为P0。

Else　'——没有找到4个点。

Error 9100　'——报警L9100。

EndIf　'——结束。

End　'——结束。

4.7.4　样例程序4

（1）工作要求及过程

在Z轴方向上以指定的速度动作，接触工件对象物。接触工件后，如果机器人的Z向位置和传感器数据满足所规定的条件，保持Z向的推入，Y轴方向开始动作。如图4-9所示。

（2）样例程序

'——设置控制模式=0的各技术参量。

M_FsCod0=0　'——使用力觉工具坐标系。

P_FsMod0=（0，1，1，0，0，0）（0，0）　'——设置Y，Z轴方向为“作用力控制”。

'——设置控制特性=0的各技术参量。

P_FsFCd0=（0，0，5，0，0，0）（0，0）　'——设置Z轴方向作用力指令=5N。

P_FsSpd0=（0，0，10，0，0，0）（0，0）　'——设置Z轴方向的速度=10mm/s。

P_FsSwF0=（0，0，0.5，0，0，0）（0，0）　'——设置Z轴方向的模式切换判定值=0.5N。

P_FsGn0=（0，0，10，0，0，0）（0，0）　'——设定Z轴方向的力觉控制增益=10×10^-3mm/N。

：'——为了使Y轴方向不动作，指定为0。

'——设置控制特性=-1的各技术参量。

P_FsFCd1=（0，3，5，0，0，0，0，0）（0，0）　'——设置Y轴方向作用力指令=3N，Z轴方向=5N。

P_FsSpd1=（0，20，0，0，0，0，0，0）（0，0）　'——设置Y轴方向的速度=20mm/s。

，为了使Z轴方向继续执行作用力控制，设置0mm/s。

P_FsSwF1=（0，2，0，0，0，0，0，0）（0，0）　'——设定Y轴方向模式切换判定值=2N。

P_FsGn1=（0，10，10，0，0，0）（0，0）　'——设定Y轴方向的力觉控制增益=10×10^-3mm/N（往Y轴方向动作）。

'——主程序。

Def MoTrg 1，（P_Fbc.Z<100） And （P_FsCurD.Z>4.8）　'——定义1#Mo组合条件：如果Z向行程小于100，同时Z向作用力大于4.8N。

SetMoTrg 1　'——设置1#Mo组合条件有效。

Fsc On，0，0，1　'——力觉控制ON。控制模式组号=0，控制特性组号=0。

Wait M_MoTrg（1）=1　'——如果组合条件1#Mo=ON，表示在Z轴方向动作接触到对象物。

Dly 3　'——（暂停）接触后，等待调整。

FsCTrg 1，100，-1　'——1# Mo=ON后，切换控制特性，控制特性组号=-1。

：'——Z轴方向上以5N的力推入的同时，在Y轴方向上开始动作。