学习随笔#7 机器人末端直线插补算法(基于IEC 61131-3)

插补是机器人最基础的功能，它分为直线插补和圆弧插补。插补中的位置由前三轴控制，姿态由后三轴控制。下面为直线插补的算法：

IF GVL.yidong THEN
	GVL.L:=SQRT((GVL.XIn-GVL.X0)*(GVL.XIn-GVL.X0)+(GVL.YIn-GVL.Y0)*(GVL.YIn-GVL.Y0)+(GVL.ZIn-GVL.Z0)*(GVL.ZIn-GVL.Z0));//插补路径长度
	GVL.kx:=(GVL.XIn-GVL.X0)/GVL.L;//X方向系数
	GVL.ky:=(GVL.YIn-GVL.Y0)/GVL.L;//Y方向系数
	GVL.kz:=(GVL.ZIn-GVL.Z0)/GVL.L;//Z方向系数
	GVL.yidong:=FALSE;
END_IF

GVL.L1:=SQRT((GVL.XIn-GVL.X0)*(GVL.XIn-GVL.X0)+(GVL.YIn-GVL.Y0)*(GVL.YIn-GVL.Y0)+(GVL.ZIn-GVL.Z0)*(GVL.ZIn-GVL.Z0));//剩余路径长

IF GVL.L1>0.001 THEN
	GVL.X0:=PX0+0.001*GVL.kx;
	GVL.Y0:=PY0+0.001*GVL.ky;
	GVL.Z0:=PZ0+0.001*GVL.kz;
END_IF

其中GVL.为全局变量表，在TC2中使用时删去即可。