工业机器人搬运码垛

'V1%： 工具

'V11！：X、V12！：Y；

' 工具选择
IF I1=1 AND I2=0
V1% = 1
ELSEIF I2=1 AND I1=0
V1% = 2
ELSE
ENDIF
' 偏移计算
FOR L%[1] = 1 TO 10 STEP 1
IF I[168+L%[1]]=1 OR I[178+L%[1]]=1
V2% = L1%
BREAK
ENDIF
NEXT
V11! = (V3%/10)*((V2%-1) MOD V1%)
V12! = (V4%/10)*((V2%-1)/V1%)
INH
SHIFTA 2,V11!,V12!,80
MOVEX A=1, AC=0, SM=0, M1X, L, P11, R=50, H=V1%, MS
SHIFTA 2,V11!,V12!,0
MOVEX A=1, AC=0, SM=0, M1X, L, P11, R=10, H=V1%, MS
SHIFTA 2,0,0,0