【正运动技术】二维螺距补偿（四）

假设工况3：X轴和Y轴的丝杆螺距均有异常需要补偿，且X轴和Y轴的安装夹角是135度。



假设X轴的激光干涉仪数据如下：

'运动到1mm处，激光干涉仪打出来实际是0.9mm处，所以需要补偿的脉冲 = 0.1*units
'运动到2mm处，激光干涉仪打出来实际是1.8mm处，所以需要补偿的脉冲 = 0.2*units
'运动到3mm处，激光干涉仪打出来实际是2.7mm处，所以需要补偿的脉冲 = 0.3*units
'Y轴丝杆工况和X轴丝杆工况类似



步骤一：规划起始补偿点，补偿间距，补偿的行数和列数。

'初始化二维螺距补偿的相关参数
TableId = 1000 '补偿数据的Table起始点
StartMposX = 0 'X轴开始补偿的位置
StartMposY = 0 'Y轴开始补偿的位置
DisX = 0.5 'X轴方向补偿的间距
DisY = 0.5 'Y轴方向补偿的间距
ColNumX = 50 'X轴方向补偿的点数
RowlNumY = 50 'Y轴方向补偿的点数


步骤二：根据该工况的测试环境，计算各个补偿点的X方向和Y方向的改补偿的脉冲数，并更新到TABLE。

'假设X轴的激光干涉仪数据如下：
'运动到1mm处，激光干涉仪实际是0.9mm处，所以需要补偿的脉冲 = 0.1*units
'运动到2mm处，激光干涉仪实际是1.8mm处，所以需要补偿的脉冲 = 0.2*units
'运动到3mm处，激光干涉仪实际是2.7mm处，所以需要补偿的脉冲 = 0.3*units
'Y轴工况和X轴工况类似
'更新该工况下二维补偿表的TABLE数据
FOR i=0  to  RowlNumY-1
   '先按行填写该行的每一列的数据
   FOR  j=0  to  ColNumX-1
       '【工况三：XY轴螺距异常，XY的夹角是135度】
       'X轴螺距有问题，实际是10000个脉冲跑0.5mm
       'Y轴螺距也有问题，实际是10000个脉冲跑0.9mm
       IF i=0 and j=0 THEN
           Angle=135'XY轴的安装夹角
       ENDIF
       LOCAL ActPosX,ActPosY '实际位置
       'X轴补偿数据
       ActPosX = 0.5*(j+1)+ (i+1)*0.9*(COS(Angle*PI/180))
       Table(TableId+2*(i*ColNumX+j)) = DisX*((1+j)-ActPosX)*UNITS(AxisX)
       'Y轴补偿数据
       ActPosY = 0.9*(SIN(Angle*PI/180))
       Table(TableId+2*(i*ColNumX+j)+1) = DisY*(1+i)*(1- ActPosY)*UNITS(AxisY)
   NEXT
NEXT


步骤三：调用二维螺距补偿指令启用二维螺距补偿功能。

'开始2D螺距补偿
WAIT IDLE
PITCH2SET(1,StartMposX ,StartMposY,DisX,DisY,ColNumX,RowlNumY,TableId)


步骤四：发送运动指令，根据实际的编码器反馈来验证控制器实际上发的脉冲数是否正确。

'开始运动
MoveTest(1,1,RunMode)
MoveTest(2,1,RunMode)
MoveTest(2,2,RunMode)


步骤五：验证效果。（PS：x轴和Y轴的脉冲当量均设置的是100000）

根据以上数据启用二维螺距补偿功能后，运动到点【1,1】时，根据实际发的脉冲数推算出实际运动的位置是【0.99999,0.99999】和【1,1】的脉冲误差也是在一个脉冲以内，所以二维螺距补偿功能正常。



MoveTest函数的实现：

GLOBAL SUB MoveTest(Pos1,Pos2,RunMode)
   MOVEABS(Pos1,Pos2)
   DELAY(1500)
   ?"运动前编码器数据",TempVarX,TempVarY
   ?"运动后坐标：【"+TOSTR(Pos1,4,1)+","+TOSTR(Pos2,4,1)+"】，当前XY轴的ENCODER:",ENCODER(AxisX),ENCODER(AxisY)
   ?"X轴实际补偿值",ENCODER(AxisX)-TempVarX-DPOS(AxisX)*UNITS(AxisX)
   ?"Y轴实际补偿值",ENCODER(AxisY)-TempVarY-DPOS(AxisY)*UNITS(AxisY)
   IF RunMode=0 THEN  '工况1
       ?"X轴理论补偿值",-POS2*(COS(Angle*pi/180)/SIN(Angle*pi/180))*UNITS(AxisX),'"和实际有差值，可能是脉冲当量的问题"
       ?"Y轴理论补偿值",POS2*(1/(SIN(Angle*pi/180)) -1)*UNITS(AxisY),'"和实际有差值，可能是脉冲当量的问题"
   ELSEIF RunMode=1 THEN  '工况2
       ?"X轴理论补偿值",POS1*0.1/0.9*UNITS(AxisX),'"和实际有差值，可能是脉冲当量的问题"
       ?"Y轴理论补偿值",POS2*0.1/0.9*UNITS(AxisY),'"和实际有差值，可能是脉冲当量的问题"
   ELSEIF RunMode=2 THEN  '工况3
       ?"X轴理论停止位：",((ENCODER(AxisX)-TempVarX)*0.5 +(ENCODER(AxisY)-TempVarY)*0.9*COS(Angle*pi/180))/UNITS(AxisX)
       ?"Y轴理论停止位：",((ENCODER(AxisY)-TempVarY)*0.9*SIN(Angle*pi/180))/UNITS(AxisX)
   ENDIF
ENDSUB