Skip to content

孙克 / RC1250-AssemblyLine

转到一个项目
切换导航栏
切换导航栏固定状态
Commit 938f54d5 由 LN
选项

io改回代码版本

1 个父辈 a01e00d9
内嵌 并排
正在显示 5 个修改的文件 包含 1381 行增加 和 6 行删除
abb/51程序.txt 0 → 100644
MODULE aaa
PERS tooldata MyTool:=[TRUE,[[31.792631019,0,229.638935148],[0.945518576,0,0.325568154,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];
CONST robtarget p1:=[[-117.61,192.28,69.30],[0.00397249,-0.0887776,-0.996016,0.00735526],[1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p2:=[[-61.06,191.49,69.36],[0.00388815,-0.0889101,-0.996005,0.00736597],[1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p3:=[[-9.24,191.25,69.40],[0.00381725,-0.0890201,-0.995996,0.0073171],[1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p4:=[[26.04,190.48,69.46],[0.00378112,-0.0890267,-0.995995,0.00730662],[1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p5:=[[-229.91,346.95,459.70],[4.83347E-05,-0.362215,0.932095,-0.000395109],[-1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p6:=[[-227.57,512.43,459.54],[1.03816E-05,0.362197,-0.932102,0.000179463],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p7:=[[1.82,-327.19,440.61],[0.463316,-0.882346,0.071927,-0.0403786],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p8:=[[8.10,-375.52,278.06],[0.352443,-0.931844,0.0590098,-0.0629934],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p9:=[[173.02,-380.60,301.74],[0.34939,-0.892928,0.250084,-0.134405],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p10:=[[-107.17,-393.26,299.48],[0.370443,-0.925911,-0.0735457,-0.00716374],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Home_Wobj1:=[[-351.42,194.01,81.92],[0.00398731,-0.0887517,-0.996018,0.00740822],[1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Home_Wobj2:=[[-203,23,6],[1,0,0,0],[-1,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Temp_Wobj2:=[[-335.14,1355.62,433.30],[0.0823269,-0.0110405,-0.00476913,-0.996533],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!*****12 matrix put position
CONST robtarget cp1:=[[243.83,232.32,471.11],[0.0978048,-0.00200365,-0.016052,-0.995074],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp2:=[[245.95,229.77,368.70],[0.0982053,0.0029793,-0.02617,-0.994818],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp3:=[[246.34,231.50,262.61],[0.0968083,0.00456822,-0.0206029,-0.995079],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp4:=[[246.34,232.05,157.17],[0.0968185,0.00456378,-0.0206124,-0.995078],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp5:=[[246.34,232.60,53.38],[0.0968328,0.00456078,-0.0206228,-0.995077],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp6:=[[245.76,231.61,-49.22],[0.0910044,0.00449019,-0.00208608,-0.995838],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp7:=[[243.98,761.47,473.09],[0.0966403,0.00376911,-0.00699505,-0.995288],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp8:=[[243.98,762.04,368.08],[0.0966636,0.00379419,-0.00699527,-0.995285],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp9:=[[243.79,760.36,260.01],[0.0981247,0.00244686,-0.0157174,-0.995047],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp10:=[[243.79,760.90,156.27],[0.0981354,0.00244851,-0.0157495,-0.995045],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp11:=[[243.79,761.45,52.50],[0.0981681,0.00244481,-0.0157816,-0.995042],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp12:=[[-389.76,1177.48,301.39],[0.0982071,0.00224386,-0.0157585,-0.995039],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp1:=[[228.64,127.89,603.12],[0.100897,0.00134554,-0.0101768,-0.994844],[-1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp2:=[[233.63,128.06,573.19],[0.100917,0.00137245,-0.0102123,-0.994842],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp3:=[[244.55,127.33,541.32],[0.100364,-0.000124375,-0.0115552,-0.994884],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp4:=[[244.54,127.50,510.96],[0.100367,-0.000113464,-0.0115611,-0.994883],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp5:=[[244.54,127.67,480.93],[0.100379,-9.76982E-05,-0.0115664,-0.994882],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp6:=[[244.54,127.82,451.17],[0.100374,-0.000101122,-0.011564,-0.994883],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp7:=[[244.53,127.99,419.77],[0.100381,-9.24279E-05,-0.0115766,-0.994882],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp8:=[[244.53,128.18,388.87],[0.100405,-8.6503E-05,-0.0116162,-0.994879],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp9:=[[245.34,128.34,358.30],[0.100414,-7.30067E-05,-0.0116172,-0.994878],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp10:=[[245.34,128.50,327.96],[0.10042,-6.91383E-05,-0.0116145,-0.994877],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp11:=[[244.37,127.83,297.44],[0.100875,-0.00304727,-0.014364,-0.994791],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp12:=[[244.21,128.76,267.00],[0.0998938,0.00265924,-0.011706,-0.994926],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp13:=[[244.21,128.92,236.16],[0.099898,0.00266751,-0.0117104,-0.994925],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp14:=[[244.21,129.08,205.83],[0.0999044,0.00266697,-0.0117213,-0.994924],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp15:=[[244.21,129.24,175.65],[0.0999041,0.00267135,-0.0117123,-0.994925],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp16:=[[244.21,129.41,144.94],[0.0999177,0.00267427,-0.0117116,-0.994923],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp17:=[[244.21,129.57,115.12],[0.099927,0.0026798,-0.0117129,-0.994922],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp18:=[[244.83,129.32,84.90],[0.0995257,0.00437063,-0.0123949,-0.994948],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp19:=[[244.83,129.49,53.49],[0.0995316,0.00438206,-0.0123983,-0.994947],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp20:=[[245.72,129.63,22.74],[0.0994972,0.00439587,-0.012377,-0.994951],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp21:=[[244.80,129.81,-8.52],[0.0995086,0.00441171,-0.0123951,-0.99495],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp22:=[[244.10,130.00,-38.14],[0.0995532,0.00445641,-0.012411,-0.994945],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp23:=[[245.95,129.72,-69.58],[0.099531,0.00567172,-0.0137238,-0.994924],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp24:=[[228.46,373.92,602.87],[0.100293,-0.000169706,-0.0115708,-0.994891],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp25:=[[237.24,374.07,572.42],[0.100278,-0.000177651,-0.0115732,-0.994892],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp26:=[[243.62,374.24,541.48],[0.100286,-0.000169116,-0.0115861,-0.994891],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp27:=[[244.13,374.27,510.63],[0.0973097,-0.0038501,-0.0143908,-0.995143],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp28:=[[244.10,372.99,480.59],[0.0980315,-0.00384397,-0.0143794,-0.995072],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp29:=[[244.09,372.07,450.82],[0.0980918,-0.00385324,-0.0143884,-0.995066],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp30:=[[244.09,373.96,419.45],[0.0974934,-0.00384255,-0.0143737,-0.995125],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp31:=[[244.08,373.04,388.56],[0.0975524,-0.00382894,-0.0143835,-0.995119],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp32:=[[244.12,373.73,358.01],[0.0936683,-0.00386922,-0.0143604,-0.995492],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp33:=[[244.12,373.16,327.67],[0.0946713,-0.00385772,-0.0143676,-0.995397],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp34:=[[244.08,373.40,297.11],[0.0978763,-0.00384044,-0.0143731,-0.995087],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp35:=[[244.09,373.94,266.70],[0.0968949,-0.00384472,-0.0143741,-0.995183],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp36:=[[244.11,374.05,235.86],[0.096525,-0.00384463,-0.0143814,-0.995219],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp37:=[[244.11,373.96,205.51],[0.0959894,-0.00386422,-0.0143749,-0.995271],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp38:=[[244.11,373.06,175.36],[0.0943097,-0.00386514,-0.0143687,-0.995432],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp39:=[[244.16,373.52,144.80],[0.0954382,-0.00684853,-0.0100885,-0.995361],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp40:=[[244.17,374.50,114.99],[0.0940068,-0.00686307,-0.010104,-0.995497],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp41:=[[244.19,374.10,84.77],[0.0903862,-0.00692783,-0.0100932,-0.995832],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp42:=[[244.19,373.29,53.36],[0.0920962,-0.00687231,-0.0101345,-0.995675],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp43:=[[244.18,373.82,22.61],[0.0943867,-0.00684261,-0.0101541,-0.99546],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp44:=[[244.17,373.77,-8.65],[0.0943331,-0.00685256,-0.0101639,-0.995465],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp45:=[[244.13,372.53,-38.42],[0.0920902,-0.00389248,-0.0143336,-0.99564],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp46:=[[244.12,372.56,-69.87],[0.0931951,-0.00387488,-0.0143613,-0.995537],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp47:=[[234.28,615.92,602.16],[0.0967417,-0.0038032,-0.0144122,-0.995198],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp48:=[[236.95,616.09,572.50],[0.096745,-0.00377847,-0.0143959,-0.995198],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp49:=[[242.38,617.37,541.88],[0.100176,-0.000249714,-0.0115919,-0.994902],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp50:=[[242.83,618.02,510.65],[0.100005,-0.0037585,-0.0144067,-0.994876],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp51:=[[242.84,617.90,480.62],[0.099168,-0.00376978,-0.0144167,-0.994959],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp52:=[[242.84,617.12,450.87],[0.098883,-0.00375966,-0.0144029,-0.994988],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp53:=[[242.78,618.22,419.45],[0.102629,-0.00373038,-0.0144171,-0.994608],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp54:=[[242.84,618.63,388.56],[0.0990308,-0.00377509,-0.0144045,-0.994973],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp55:=[[242.86,618.08,358.01],[0.0982308,-0.00376896,-0.0144031,-0.995052],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp56:=[[242.81,617.01,327.63],[0.101499,-0.00375162,-0.0144129,-0.994724],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp57:=[[242.88,618.77,297.16],[0.0965129,-0.00378246,-0.0143962,-0.99522],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp58:=[[242.86,618.16,266.70],[0.097638,-0.00378325,-0.0143966,-0.995111],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp59:=[[244.33,617.55,238.33],[0.0954024,0.000150206,-0.0157453,-0.995314],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp60:=[[242.87,618.38,205.54],[0.097574,-0.00377515,-0.014403,-0.995117],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp61:=[[242.85,618.66,175.35],[0.0983761,-0.00377477,-0.0144013,-0.995038],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp62:=[[242.86,618.32,144.66],[0.0969442,-0.00377942,-0.0144037,-0.995178],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp63:=[[242.84,619.07,114.82],[0.0977735,-0.00378974,-0.0143955,-0.995097],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp64:=[[242.85,618.38,84.62],[0.0983296,-0.00375276,-0.0144006,-0.995043],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp65:=[[242.84,618.61,53.19],[0.0985325,-0.00377218,-0.0144009,-0.995022],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp66:=[[242.87,619.20,22.46],[0.0968679,-0.00378401,-0.0144003,-0.995186],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp67:=[[242.84,619.25,-8.83],[0.0992603,-0.00376782,-0.0144045,-0.99495],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp68:=[[242.86,619.27,-38.43],[0.0963487,-0.00379532,-0.0143902,-0.995236],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp69:=[[242.87,619.32,-69.86],[0.0962846,-0.00378396,-0.0143943,-0.995243],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp70:=[[232.60,861.92,606.35],[0.100737,0.00365766,-0.0076209,-0.994877],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp71:=[[231.77,861.98,575.36],[0.100785,-0.000146837,-0.00751174,-0.99488],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp72:=[[237.34,862.15,544.94],[0.100802,-0.000144358,-0.00751235,-0.994878],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp73:=[[237.81,860.35,513.59],[0.0974597,-0.00379074,-0.0144472,-0.995127],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp74:=[[237.84,860.88,483.10],[0.093552,-0.0038125,-0.0144015,-0.995503],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp75:=[[238.90,863.73,453.38],[0.0978929,1.23566E-05,-0.00697826,-0.995173],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp76:=[[239.19,861.28,421.81],[0.0985798,0.000171224,-0.0145428,-0.995023],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp77:=[[239.27,862.00,391.09],[0.0952667,0.00134578,-0.00309106,-0.995446],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp78:=[[239.10,863.78,360.27],[0.098032,0.000558435,-0.00782763,-0.995152],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp79:=[[238.01,860.79,330.38],[0.098224,-0.00282592,-0.0113257,-0.995096],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp80:=[[243.08,860.84,300.07],[0.095055,0.00212525,-0.00532998,-0.995456],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp81:=[[243.08,863.48,270.01],[0.0950264,0.0021295,-0.00531318,-0.995458],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp82:=[[239.95,863.62,238.76],[0.0950615,0.000942865,-0.00532532,-0.995457],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp83:=[[239.17,863.79,209.07],[0.0950721,0.000949276,-0.00530521,-0.995456],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp84:=[[238.02,863.98,178.09],[0.0951248,0.000934793,-0.00529248,-0.995451],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp85:=[[239.47,864.15,148.01],[0.0951296,0.000910218,-0.00526557,-0.995451],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp86:=[[242.06,863.69,116.89],[0.0915525,-0.000912099,-0.00512892,-0.995787],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp87:=[[242.06,863.87,86.97],[0.0915797,-0.000910226,-0.00512763,-0.995784],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp88:=[[242.86,864.86,55.85],[0.09636,0.000926631,-0.0111487,-0.995284],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp89:=[[242.58,864.24,24.75],[0.0915699,0.000562291,-0.00514328,-0.995785],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp90:=[[243.26,864.44,-5.04],[0.0915341,0.00250889,-0.00517017,-0.995785],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp91:=[[243.52,866.41,-35.96],[0.0976029,0.00294521,-0.010501,-0.995166],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp92:=[[240.13,866.10,-65.84],[0.0984136,0.00382688,-0.0133621,-0.995049],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!*****12 matrix put position end
CONST robtarget p23:=[[325,777,-30],[1,0,0,0],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p24:=[[951.67,291.58,305.65],[0.283752,0.381565,-0.878309,0.0496596],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p25:=[[957.63,293.21,337.45],[0.265933,0.381999,-0.883532,0.0522222],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p26:=[[961.87,294.37,365.18],[0.250414,0.382266,-0.887808,0.0544283],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p27:=[[966.30,295.57,403.79],[0.228818,0.38247,-0.893341,0.057459],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p28:=[[968.46,296.16,431.77],[0.213148,0.382496,-0.897056,0.0596298],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p29:=[[969.95,296.57,468.46],[0.192573,0.382378,-0.901556,0.0624448],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p30:=[[969.94,296.56,505.18],[0.17191,0.382088,-0.905649,0.0652315],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p31:=[[943.39,251.39,232.51],[0.325692,0.363748,-0.871897,0.0375142],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p32:=[[933.30,286.58,232.51],[0.324931,0.380032,-0.864924,0.043614],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p33:=[[943.52,289.36,269.92],[0.303826,0.380911,-0.872016,0.0467343],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p34:=[[951.67,291.58,305.65],[0.283752,0.381565,-0.878309,0.0496596],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p35:=[[957.63,293.21,337.45],[0.265933,0.381999,-0.883532,0.0522222],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p36:=[[961.87,294.37,365.18],[0.250414,0.382266,-0.887808,0.0544283],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p37:=[[966.30,295.57,403.79],[0.228818,0.38247,-0.893341,0.057459],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p38:=[[968.46,296.16,431.77],[0.213148,0.382496,-0.897056,0.0596298],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p39:=[[969.95,296.57,468.46],[0.192573,0.382378,-0.901556,0.0624448],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p40:=[[969.94,296.56,505.18],[0.17191,0.382088,-0.905649,0.0652315],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p41:=[[943.39,251.39,232.51],[0.325692,0.363748,-0.871897,0.0375142],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p42:=[[933.30,286.58,232.51],[0.324931,0.380032,-0.864924,0.043614],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p43:=[[943.52,289.36,269.92],[0.303826,0.380911,-0.872016,0.0467343],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p44:=[[951.67,291.58,305.65],[0.283752,0.381565,-0.878309,0.0496596],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p45:=[[957.63,293.21,337.45],[0.265933,0.381999,-0.883532,0.0522222],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p46:=[[961.87,294.37,365.18],[0.250414,0.382266,-0.887808,0.0544283],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p47:=[[966.30,295.57,403.79],[0.228818,0.38247,-0.893341,0.057459],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p48:=[[968.46,296.16,431.77],[0.213148,0.382496,-0.897056,0.0596298],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p49:=[[969.95,296.57,468.46],[0.192573,0.382378,-0.901556,0.0624448],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p50:=[[969.94,296.56,505.18],[0.17191,0.382088,-0.905649,0.0652315],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
Var pos now_pos:=[0,0,0];
VAR robtarget PNowPos;
PERS speeddata speed1:=[1000,50,100,100];
PERS speeddata speed2:=[350,350,350,350];
PERS speeddata speed3:=[300,300,300,300];
VAR socketdev socket_server;
VAR socketdev socket_client;
PERS string str_address:="192.168.103.12";
!PERS string str_address:="192.168.201.88";
PERS num n_port:=10021;
VAR string strReceive:="p1,L,100";
VAR string resultStr_FINISHED:=",FINISHED;";
VAR string resultStr_OK:=",OK;";
VAR string resultStr_NG:=",NG;";
VAR socketstatus status;
VAR num starBit1:=0;
VAR num endBit1:=0;
VAR num LengBit1:=0;
VAR num starBit2:=0;
VAR num endBit2:=0;
VAR num LengBit2:=0;
VAR num starBit3:=0;
VAR num endBit3:=0;
VAR num LengBit3:=0;
VAR num endBit4:=0;
VAR num LengBit4:=0;
VAR num starBit4:=0;
PERS string shelfType:="c";
PERS string revice_Str1_Cmd:="moveput";
PERS string revice_Str2_pName:="cp9";
PERS string revice_Str3_moveType:="L";
PERS string revice_Str4_speed:="1000";
VAR bool flag1:=false;
!PERS num value_1:=700;
!PERS num value_2:=50;
VAR robtarget pTarget:=[[700,50,700],[0.0258621,-0.000364211,-0.999665,0.000003063],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget pTemp:=[[700,50,700],[0.0258621,-0.000364211,-0.999665,3.06266E-6],[0,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS bool booLineMove:=TRUE;
PERS bool dataOk:=TRUE;
VAR num len:=0;
PERS zonedata zone1:=[FALSE,5,8,8,0.8,8,0.8];
PERS loaddata load1:=[4,[0,0,0],[1,0,0,0],0,0,0];
PERS robtarget pCheckTest:=[[-1.73995,-351.15,465.56],[0.300709,-0.919332,0.248148,0.0531501],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS bool pCheckResult:=TRUE;
PERS tooldata tooltest:=[TRUE,[[-208.798,-217.638,93.8838],[1,0,0,0]],[1.3,[-31.7,-112.8,55.6],[1,0,0,0],0,0.045,0.029]];
PERS wobjdata wobjtest:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[1033.18,-257.207,479.665],[2.98351E-05,0.707094,0.707119,6.49066E-05]]];
PERS tooldata tool1:=[TRUE,[[-206.667,-217.202,115.341],[0.00825331,-0.950325,-0.311059,0.00745712]],[1.6,[-45.6,-95.4,67.7],[1,0,0,0],0.075,0.031,0.049]];
PERS wobjdata wobj1:=[FALSE,TRUE,"",[[-724.364,1011.14,829.487],[0.00329323,-0.707116,-0.707083,0.00323154]],[[0,0,0],[1,0,0,0]]];
PERS wobjdata wobj2:=[FALSE,TRUE,"",[[818.911,-803.257,564.373],[0.999997,-0.00262178,-4.74769E-06,-1.52733E-05]],[[0,0,0],[1,0,0,0]]];
CONST robtarget p17:=[[232.66,0.00,29.24],[1,-1.36004E-05,1.18275E-06,-4.95038E-06],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Temp_Wobj12:=[[-327.22,959.94,437.65],[0.499444,0.00340242,-0.0277838,0.865894],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p60:=[[300.96,782.57,1058.28],[0.56503,-0.00604911,-0.0120051,-0.824961],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pbackhomepos:=[[300.96,782.57,1058.28],[0.56503,-0.00604782,-0.012004,-0.824961],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pbackhomepos10:=[[300.96,782.57,1058.28],[0.565031,-0.00604999,-0.0120028,-0.82496],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget jbackhomepos:=[[300.96,782.57,1058.28],[0.56503,-0.0060503,-0.012003,-0.824961],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget PNowPos10:=[[582.05,356.05,1058.28],[0.0622783,0.000912002,-0.0132789,-0.99797],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Home_Wobj11:=[[-343.11,913.99,-87.78],[0.0645782,-0.488913,0.868423,-0.0513351],[1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Home_Wobj21:=[[-336.22,1333.92,433.10],[0.0461911,-0.0115964,-0.00495549,-0.998853],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PROC modifyPoint()
!!!!"Px" getposition;"CPX" BigSizePlate Position;"DPx"Smallsizeplate position;
MoveL p1,speed1,z50,tool1\WObj:=wobj1;
MoveL p2,speed1,z50,tool1\WObj:=wobj1;
MoveL p3,speed1,z50,tool1\WObj:=wobj1;
MoveL p4,speed1,z50,tool1\WObj:=wobj1;
MoveL p5,speed1,z50,tool1\WObj:=wobj1;
MoveL p6,speed1,z50,tool1\WObj:=wobj1;
MoveL p7,speed1,z50,tool1\WObj:=wobj1;
MoveL p8,speed1,z50,tool1\WObj:=wobj1;
MoveL p9,speed1,z50,tool1\WObj:=wobj1;
MoveL p10,speed1,z50,tool1\WObj:=wobj1;
MoveL cp1,speed1,z50,tool1\WObj:=wobj2;
MoveL cp2,speed1,z50,tool1\WObj:=wobj2;
MoveL cp3,speed1,z50,tool1\WObj:=wobj2;
MoveL cp4,speed1,z50,tool1\WObj:=wobj2;
MoveL cp5,speed1,z50,tool1\WObj:=wobj2;
MoveL cp6,speed1,z50,tool1\WObj:=wobj2;
MoveL cp7,speed1,z50,tool1\WObj:=wobj2;
MoveL cp8,speed1,z50,tool1\WObj:=wobj2;
MoveL cp9,speed1,z50,tool1\WObj:=wobj2;
MoveL cp10,speed1,z50,tool1\WObj:=wobj2;
MoveL cp11,speed1,z50,tool1\WObj:=wobj2;
MoveL cp12,speed1,z50,tool1\WObj:=wobj2;
MoveL dp1,speed1,z50,tool1\WObj:=wobj2;
MoveL dp2,speed1,z50,tool1\WObj:=wobj2;
MoveL dp3,speed1,z50,tool1\WObj:=wobj2;
MoveL dp4,speed1,z50,tool1\WObj:=wobj2;
MoveL dp5,speed1,z50,tool1\WObj:=wobj2;
MoveL dp6,speed1,z50,tool1\WObj:=wobj2;
MoveL dp7,speed1,z50,tool1\WObj:=wobj2;
MoveL dp8,speed1,z50,tool1\WObj:=wobj2;
MoveL dp9,speed1,z50,tool1\WObj:=wobj2;
MoveL dp10,speed1,z50,tool1\WObj:=wobj2;
MoveL dp11,speed1,z50,tool1\WObj:=wobj2;
MoveL dp12,speed1,z50,tool1\WObj:=wobj2;
MoveL dp13,speed1,z50,tool1\WObj:=wobj2;
MoveL dp14,speed1,z50,tool1\WObj:=wobj2;
MoveL dp15,speed1,z50,tool1\WObj:=wobj2;
MoveL dp16,speed1,z50,tool1\WObj:=wobj2;
MoveL dp17,speed1,z50,tool1\WObj:=wobj2;
MoveL dp18,speed1,z50,tool1\WObj:=wobj2;
MoveL dp19,speed1,z50,tool1\WObj:=wobj2;
MoveL dp20,speed1,z50,tool1\WObj:=wobj2;
MoveL dp21,speed1,z50,tool1\WObj:=wobj2;
MoveL dp22,speed1,z50,tool1\WObj:=wobj2;
MoveL dp23,speed1,z50,tool1\WObj:=wobj2;
MoveL dp24,speed1,z50,tool1\WObj:=wobj2;
MoveL dp25,speed1,z50,tool1\WObj:=wobj2;
MoveL dp26,speed1,z50,tool1\WObj:=wobj2;
MoveL dp27,speed1,z50,tool1\WObj:=wobj2;
MoveL dp28,speed1,z50,tool1\WObj:=wobj2;
MoveL dp29,speed1,z50,tool1\WObj:=wobj2;
MoveL dp30,speed1,z50,tool1\WObj:=wobj2;
MoveL dp31,speed1,z50,tool1\WObj:=wobj2;
MoveL dp32,speed1,z50,tool1\WObj:=wobj2;
MoveL dp33,speed1,z50,tool1\WObj:=wobj2;
MoveL dp34,speed1,z50,tool1\WObj:=wobj2;
MoveL dp35,speed1,z50,tool1\WObj:=wobj2;
MoveL dp36,speed1,z50,tool1\WObj:=wobj2;
MoveL dp37,speed1,z50,tool1\WObj:=wobj2;
MoveL dp38,speed1,z50,tool1\WObj:=wobj2;
MoveL dp39,speed1,z50,tool1\WObj:=wobj2;
MoveL dp40,speed1,z50,tool1\WObj:=wobj2;
MoveL dp41,speed1,z50,tool1\WObj:=wobj2;
MoveL dp42,speed1,z50,tool1\WObj:=wobj2;
MoveL dp43,speed1,z50,tool1\WObj:=wobj2;
MoveL dp44,speed1,z50,tool1\WObj:=wobj2;
MoveL dp45, speed1, z50, tool1\WObj:=wobj2;
MoveL dp46,speed1,z50,tool1\WObj:=wobj2;
MoveL dp47,speed1,z50,tool1\WObj:=wobj2;
MoveL dp48,speed1,z50,tool1\WObj:=wobj2;
MoveL dp49,speed1,z50,tool1\WObj:=wobj2;
MoveL dp50,speed1,z50,tool1\WObj:=wobj2;
MoveL dp51,speed1,z50,tool1\WObj:=wobj2;
MoveL dp52,speed1,z50,tool1\WObj:=wobj2;
MoveL dp53,speed1,z50,tool1\WObj:=wobj2;
MoveL dp54,speed1,z50,tool1\WObj:=wobj2;
MoveL dp55,speed1,z50,tool1\WObj:=wobj2;
MoveL dp56,speed1,z50,tool1\WObj:=wobj2;
MoveL dp57,speed1,z50,tool1\WObj:=wobj2;
MoveL dp58,speed1,z50,tool1\WObj:=wobj2;
MoveL dp59,speed1,z50,tool1\WObj:=wobj2;
MoveL dp60,speed1,z50,tool1\WObj:=wobj2;
MoveL dp61,speed1,z50,tool1\WObj:=wobj2;
MoveL dp62,speed1,z50,tool1\WObj:=wobj2;
MoveL dp63,speed1,z50,tool1\WObj:=wobj2;
MoveL dp64,speed1,z50,tool1\WObj:=wobj2;
MoveL dp65,speed1,z50,tool1\WObj:=wobj2;
MoveL dp66,speed1,z50,tool1\WObj:=wobj2;
MoveL dp67,speed1,z50,tool1\WObj:=wobj2;
MoveL dp68,speed1,z50,tool1\WObj:=wobj2;
MoveL dp69,speed1,z50,tool1\WObj:=wobj2;
MoveL dp70,speed1,z50,tool1\WObj:=wobj2;
MoveL dp71,speed1,z50,tool1\WObj:=wobj2;
MoveL dp72,speed1,z50,tool1\WObj:=wobj2;
MoveL dp73,speed1,z50,tool1\WObj:=wobj2;
MoveL dp74,speed1,z50,tool1\WObj:=wobj2;
MoveL dp75,speed1,z50,tool1\WObj:=wobj2;
MoveL dp76,speed1,z50,tool1\WObj:=wobj2;
MoveL dp77,speed1,z50,tool1\WObj:=wobj2;
MoveL dp78,speed1,z50,tool1\WObj:=wobj2;
MoveL dp79,speed1,z50,tool1\WObj:=wobj2;
MoveL dp80,speed1,z50,tool1\WObj:=wobj2;
MoveL dp81,speed1,z50,tool1\WObj:=wobj2;
MoveL dp82,speed1,z50,tool1\WObj:=wobj2;
MoveL dp83,speed1,z50,tool1\WObj:=wobj2;
MoveL dp84,speed1,z50,tool1\WObj:=wobj2;
MoveL dp85,speed1,z50,tool1\WObj:=wobj2;
MoveL dp86,speed1,z50,tool1\WObj:=wobj2;
MoveL dp87,speed1,z50,tool1\WObj:=wobj2;
MoveL dp88,speed1,z50,tool1\WObj:=wobj2;
MoveL dp89,speed1,z50,tool1\WObj:=wobj2;
MoveL dp90,speed1,z50,tool1\WObj:=wobj2;
MoveL dp91,speed1,z50,tool1\WObj:=wobj2;
MoveL dp92,speed1,z50,tool1\WObj:=wobj2;
MoveL p23,speed1,z50,tool1\WObj:=wobj2;
MoveL p24,speed1,z50,tool1\WObj:=wobj2;
MoveL p25,speed1,z50,tool1\WObj:=wobj2;
MoveL p26,speed1,z50,tool1\WObj:=wobj2;
MoveL p27,speed1,z50,tool1\WObj:=wobj2;
MoveL p28,speed1,z50,tool1\WObj:=wobj2;
MoveL p29,speed1,z50,tool1\WObj:=wobj2;
MoveL p30,speed1,z50,tool1\WObj:=wobj2;
MoveL p31,speed1,z50,tool1\WObj:=wobj2;
MoveL p32,speed1,z50,tool1\WObj:=wobj2;
MoveL p33,speed1,z50,tool1\WObj:=wobj2;
MoveL p34,speed1,z50,tool1\WObj:=wobj2;
MoveL p35,speed1,z50,tool1\WObj:=wobj2;
MoveL p36,speed1,z50,tool1\WObj:=wobj2;
MoveL p37,speed1,z50,tool1\WObj:=wobj2;
MoveL p38,speed1,z50,tool1\WObj:=wobj2;
MoveL p39,speed1,z50,tool1\WObj:=wobj2;
MoveL p40,speed1,z50,tool1\WObj:=wobj2;
MoveL p41,speed1,z50,tool1\WObj:=wobj2;
MoveL p42,speed1,z50,tool1\WObj:=wobj2;
MoveL p43,speed1,z50,tool1\WObj:=wobj2;
MoveL p44,speed1,z50,tool1\WObj:=wobj2;
MoveL p45,speed1,z50,tool1\WObj:=wobj2;
MoveL p46,speed1,z50,tool1\WObj:=wobj2;
MoveL p47,speed1,z50,tool1\WObj:=wobj2;
MoveL p48,speed1,z50,tool1\WObj:=wobj2;
MoveL p49,speed1,z50,tool1\WObj:=wobj2;
MoveL p50,speed1,z50,tool1\WObj:=wobj2;
ENDPROC
PROC main()
zone1:=z5;
SocketPro;
error
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ELSE
SocketPro;
RETURN ;
ENDIF
ENDPROC
PROC loadexample()
VAR robtarget ptemp:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
VAR robtarget pPlace:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
MoveL Offs(pTemp,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL pTemp,speed1,fine,tool1\WObj:=wobj1;
GripLoad load1;
MoveL Offs(pTemp,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL Offs(pPlace,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL pPlace,speed1,fine,tool1\WObj:=wobj1;
GripLoad load0;
MoveL Offs(pPlace,0,0,10),speed1,z50,tool1\WObj:=wobj1;
ENDPROC
PROC socketRecover()
SocketClose socket_client;
SocketCreate socket_client;
SocketConnect socket_client,str_address,n_port\Time:=5;
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ResetRetryCount;
RETRY;
ENDIF
ENDPROC
PROC SocketPro()
TPErase;
SocketClose socket_client;
!create client
SocketCreate socket_client;
!connect server
SocketConnect socket_client,str_address,n_port\Time:=WAIT_MAX;
WHILE TRUE DO
pTarget:=CRobT(\Tool:=tool1);
ptemp:=pTarget;
!receive instruction
label1:
SocketReceive socket_client\Str:=strReceive;
dataOk:=FALSE;
strParse strReceive;
IF dataOk THEN
! GOTO label1;
IF revice_Str1_Cmd="movep" THEN
IF booLineMove THEN
MoveL pTarget, speed1, fine, tool1\WObj:=wobj1;
ELSE
MoveJ pTarget, speed1, fine, tool1\WObj:=wobj1;
ENDIF
SocketSend socket_client\Str:=strReceive+resultStr_OK;
!SocketSend socket_client\Str:=revice_Str1_Cmd+","+revice_Str2_pName+","+resultStr_OK+";";
!!!!!!!!!!!!
ELSEIF revice_Str1_Cmd="movehome" THEN
rBackHome;
SocketSend socket_client\Str:=strReceive+resultStr_OK;
!!!!!!!!!!!!!!
ELSEIF revice_Str1_Cmd="moveget" THEN
MoveL Home_Wobj1, speed1, z50, tool1\WObj:=wobj1;
IF booLineMove THEN
! MoveL Offs(pTarget,0,0,10), speed1, zone1, tool1\WObj:=wobj1;
!MoveL Home_Wobj1,speed1,fine,tool1\WObj:=wobj1;
! GripLoad load0;
MoveL Offs(pTarget,0,0,30), speed1, zone1, tool1\WObj:=wobj1;
MoveL Offs(pTarget,0,0,-80), speed3, zone1, tool1\WObj:=wobj1;
MoveL Temp_Wobj2, speed3, z100, tool1\WObj:=wobj2;
!MoveL RelTool(pTarget,0,0,0\Rz:=10),speed1,zone1,tool1\WObj:=wobj1;
!MoveL RelTool(pTarget,-500,-100,0\Rz:=135),speed1,zone1,tool1\WObj:=wobj1;
ELSE
!MoveJ pTarget,speed1,fine,tool1\WObj:=wobj1;
ENDIF
SocketSend socket_client\Str:=strReceive+resultStr_OK;
! SocketSend socket_client\Str:=revice_Str1_Cmd+","+revice_Str2_pName+","+resultStr_OK+";";
ELSEIF revice_Str1_Cmd="moveput" THEN
!MoveJ Home_Wobj2,speed1,fine,tool1\WObj:=wobj1;
IF booLineMove AND shelfType="c" THEN
MoveL Temp_Wobj2, speed3, z100, tool1\WObj:=wobj2;
! GripLoad load1;
MoveL Offs(pTarget,-420,0,20), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,20), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,-20), speed2, zone1, tool1\WObj:=wobj2;
SocketSend socket_client\Str:=strReceive+resultStr_FINISHED;
MoveL Offs(pTarget,-420,0,-20), speed1, zone1, tool1\WObj:=wobj2;
ELSEIF booLineMove AND shelfType="d" THEN
MoveL Temp_Wobj2, speed3, z100, tool1\WObj:=wobj2;
! GripLoad load1;
MoveL Offs(pTarget,-350,0,8), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,8), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,-8), speed2, zone1, tool1\WObj:=wobj2;
SocketSend socket_client\Str:=strReceive+resultStr_FINISHED;
MoveL Offs(pTarget,-350,0,-8), speed1, zone1, tool1\WObj:=wobj2;
!MoveJ pTarget,speed1,fine,tool1\WObj:=wobj2;
ENDIF
! SocketSend socket_client\Str:=strReceive+resultStr_OK;
MoveL Temp_Wobj2, speed1, fine, tool1\WObj:=wobj2;
MoveL Home_Wobj1, speed1, fine, tool1\WObj:=wobj1;
SocketSend socket_client\Str:=strReceive+resultStr_OK;
ELSEIF revice_Str1_Cmd="validateP" THEN
!WaitRob\ZeroSpeed;
!wait robot stop;
pCheckTest:=CRobT(\Tool:=tool1\WObj:=wobj1);
!get current position
pCheckResult:=false;
pCheckResult:=validateP(pTarget,pCheckTest,0.1);
IF pCheckResult THEN
SocketSend socket_client\Str:=strReceive+resultStr_OK;
ELSE
SocketSend socket_client\Str:=strReceive+resultStr_NG;
ENDIF
ELSE
SocketSend socket_client\Str:=strReceive+" DATA ERROR ;";
ENDIF
ENDIF
ENDWHILE
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ResetRetryCount;
RETRY;
ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
socketRecover;
ResetRetryCount;
RETRY;
ENDIF
ENDPROC
FUNC bool validateP(robtarget ptest,robtarget pCompare,num threshold)
VAR jointtarget joinTest:=[[-12.1057,1.3138,-1.32846,-3.7545E-7,90.0147,-12.1057],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
VAR jointtarget joinCompare:=[[-12.143,1.32099,-1.33096,-0.000317427,90.0147,-12.1215],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
VAR num counter;
joinTest:=CalcJointT(pTest,tool1\WObj:=wobj1);
joinCompare:=CalcJointT(pCompare,tool1\WObj:=wobj1);
IF abs(joinTest.robax.rax_1-joinCompare.robax.rax_1)<threshold incr counter;
IF abs(joinTest.robax.rax_2-joinCompare.robax.rax_2)<threshold incr counter;
IF abs(joinTest.robax.rax_3-joinCompare.robax.rax_3)<threshold incr counter;
IF abs(joinTest.robax.rax_4-joinCompare.robax.rax_4)<threshold incr counter;
IF abs(joinTest.robax.rax_5-joinCompare.robax.rax_5)<threshold incr counter;
IF abs(joinTest.robax.rax_6-joinCompare.robax.rax_6)<threshold incr counter;
IF counter=6 THEN
RETURN TRUE;
ELSE
RETURN FALSE;
ENDIF
ENDFUNC
PROC strParse(string reviceStr)
VAR num revice_speed:=10;
lable2:
len:=StrLen(reviceStr);
starBit1:=1;
endBit1:=StrFind(reviceStr,starBit1,",");
LengBit1:=endBit1-starBit1;
starBit2:=endBit1+1;
IF starBit2>len THEN
RETURN ;
ENDIF
endBit2:=StrFind(reviceStr,starBit2,",");
LengBit2:=endBit2-starBit2;
starBit3:=endBit2+1;
IF starBit3>len THEN
RETURN ;
ENDIF
endBit3:=StrFind(reviceStr,starBit3,",");
LengBit3:=endBit3-starBit3;
starBit4:=endBit3+1;
IF starBit4>len THEN
RETURN ;
ENDIF
endBit4:=StrFind(reviceStr,starBit4,",");
LengBit4:=endBit4-starBit4;
revice_Str1_Cmd:=StrPart(reviceStr,starBit1,LengBit1);
revice_Str2_pName:=StrPart(reviceStr,starBit2,LengBit2);
revice_Str3_moveType:=StrPart(reviceStr,starBit3,LengBit3);
revice_Str4_speed:=StrPart(reviceStr,starBit4,LengBit4);
shelfType:= StrPart(revice_Str2_pName,1,1);
!capture target
GetDataVal revice_Str2_pName,ptemp;
!Stop;
!Stop;
pTarget:=ptemp;
!determine L or J
IF revice_Str3_moveType="L" THEN
booLineMove:=TRUE;
ELSEIF revice_Str3_moveType="J" THEN
booLineMove:=FALSE;
ENDIF
! get speed
flag1:=StrToVal(revice_Str4_speed,revice_speed);
IF revice_speed>1000 THEN
revice_speed:=1000;
ENDIF
speed1.v_tcp:=revice_speed;
dataOk:=TRUE;
ERROR
IF errno=ERR_SOCK_CLOSED THEN
ELSE
dataOk:=FALSE;
RETURN ;
ENDIF
ENDPROC
!PROC Path_10()
! MoveL Target_10,v1000,z100,tool1\WObj:=wobj1;
! MoveL Target_20,v1000,z100,tool1\WObj:=wobj1;
!ENDPROC
PROC Routine1()
MoveL [[-1.74,-351.15,465.56],[0.300707,-0.919334,0.248145,0.0531493],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z50,tool1\WObj:=wobj1;
MoveL [[-4.66,-300.07,453.63],[0.282192,-0.925597,0.248225,0.0449703],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z10,tool1\WObj:=wobj1;
MoveL [[79.15,-300.86,453.64],[0.282202,-0.925593,0.248224,0.0449831],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z10,tool1\WObj:=wobj1;
MoveL [[78.71,-347.14,453.64],[0.282192,-0.92559,0.248244,0.0450091],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z10,tool1\WObj:=wobj1;
ENDPROC
PROC Routine2()
MoveAbsJ [[0,0,0,0,90,-45],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs,v1000,fine,tool0;
ENDPROC
PROC Routine3()
MoveAbsJ [[0,0,0,0.0024417,-84.4403,-0.0039846],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs, v1000, z50, tool1\WObj:=wobj2;
ENDPROC
PROC rBackHome()
PNowPos:=CRobT(\Tool:=tool1\Wobj:=wobj0);
IF PNowPos.trans.y>=700 and PNowPos.trans.z>820 THEN
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
endif
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSEIF PNowPos.trans.y>700 and PNowPos.trans.z<=820 AND PNowPos.trans.X<=-250 THEN
PNowPos.trans.y:=700;
MOVEL PNowPos,V100,Z10,TOOL1;
PNowPos.trans.z:=900;
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
!TPWrite "chang to manual module";
!stop;
ELSeIF PNowPos.trans.y<=700 AND PNowPos.trans.X<=715 then
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
ENDIF
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSEIF PNowPos.trans.y<=700 AND PNowPos.trans.X>710 then
PNowPos.trans.X:=715;
MOVEL PNowPos,V100,Z10,TOOL1;
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
ENDIF
MOVEL PNowPos,V100,Z10,TOOL1;
MoveL PNowPos10, v100, z10, tool1;
MOVEj jbackhomepos,v100,fine, tool1;
ENDIF
ENDPROC
PROC Routine4()
MoveJ jbackhomepos, v1000, z50, tool1;
MoveL pbackhomepos, v1000, z50, tool1;
ENDPROC
ENDMODULE
\ No newline at end of file
abb/52程序.txt 0 → 100644
MODULE aaa
PERS tooldata MyTool:=[TRUE,[[31.792631019,0,229.638935148],[0.945518576,0,0.325568154,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];
CONST robtarget P1:=[[16.17,-7.50,-145.81],[0.995108,-0.00529605,-0.00620484,0.0984543],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p2:=[[73.77,1.66,-146.98],[0.995104,-0.00528212,-0.00619488,0.0985003],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p3:=[[124.21,10.00,-145.62],[0.995108,-0.00527065,-0.00618692,0.0984586],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p4:=[[161.35,15.98,-145.85],[0.995111,-0.00526213,-0.00612356,0.0984314],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p5:=[[-229.91,346.95,459.70],[4.83347E-05,-0.362215,0.932095,-0.000395109],[-1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p6:=[[-227.57,512.43,459.54],[1.03816E-05,0.362197,-0.932102,0.000179463],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p7:=[[1.82,-327.19,440.61],[0.463316,-0.882346,0.071927,-0.0403786],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p8:=[[8.10,-375.52,278.06],[0.352443,-0.931844,0.0590098,-0.0629934],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p9:=[[173.02,-380.60,301.74],[0.34939,-0.892928,0.250084,-0.134405],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p10:=[[-107.17,-393.26,299.48],[0.370443,-0.925911,-0.0735457,-0.00716374],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Home_Wobj1:=[[-172.69,-38.62,-160.40],[0.995107,-0.00528568,-0.00629137,0.0984558],[-1,-3,-3,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Home_Wobj2:=[[-203,23,6],[1,0,0,0],[-1,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Temp_Wobj2:=[[-301.05,-391.76,438.17],[0.998911,-0.00076512,-0.00303646,-0.046547],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!*****12 matrix put position
CONST robtarget cp1:=[[345.43,238.53,499.39],[0.999961,-0.00767894,-0.000719497,0.00437209],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp2:=[[349.88,235.10,397.10],[0.999749,-0.0214884,-0.00250497,0.00580575],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp3:=[[350.72,235.69,290.74],[0.999749,-0.0214991,-0.00250919,0.00579997],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp4:=[[351.55,236.29,185.26],[0.999748,-0.0215145,-0.0025124,0.00581933],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp5:=[[352.37,236.88,81.67],[0.999748,-0.021528,-0.00250979,0.00583732],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp6:=[[353.17,237.48,-20.55],[0.999747,-0.0215573,-0.00250887,0.00589586],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp7:=[[345.92,763.75,505.22],[0.999942,-0.00939056,-0.00510156,-0.00166134],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp8:=[[346.13,766.75,398.91],[0.999985,-0.00433806,-0.00329039,-0.000584401],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp9:=[[357.63,767.27,292.29],[0.999985,-0.00432659,-0.00328373,-0.000531956],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp10:=[[358.48,769.82,188.67],[0.999985,-0.00431653,-0.00326409,-0.000525566],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp11:=[[359.30,770.40,84.78],[0.999985,-0.00431388,-0.003262,-0.000523857],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp12:=[[355.12,769.98,-21.95],[0.999748,-0.0215194,-0.00249739,0.00590012],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp1:=[[337.69,134.02,632.11],[0.999877,-0.0121344,-0.00469915,0.0087367],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp2:=[[340.63,134.20,600.92],[0.999877,-0.0121303,-0.00472418,0.00876584],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp3:=[[351.21,134.23,570.70],[0.99988,-0.0121422,-0.00401846,0.00875981],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp4:=[[350.81,135.46,539.88],[0.999909,-0.00969493,-0.00224496,0.00912136],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp5:=[[351.05,135.65,508.92],[0.999909,-0.00969575,-0.00225711,0.00914035],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp6:=[[351.28,135.83,479.10],[0.999908,-0.00969568,-0.00226564,0.00916041],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp7:=[[351.59,136.43,447.25],[0.999833,-0.0155925,-0.00229068,0.00929],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp8:=[[351.83,136.60,417.50],[0.999833,-0.0155932,-0.00228097,0.00929406],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp9:=[[352.08,136.79,386.38],[0.999833,-0.0155771,-0.00227957,0.00931142],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp10:=[[352.32,136.97,356.47],[0.999832,-0.0155882,-0.00227877,0.00933308],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp11:=[[352.56,137.14,326.76],[0.999832,-0.015614,-0.00227344,0.00934752],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp12:=[[353.44,137.35,295.65],[0.999826,-0.0155961,-0.00405226,0.00935448],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp13:=[[353.68,137.53,265.23],[0.999826,-0.0155994,-0.00405334,0.00937555],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp14:=[[353.93,137.72,234.23],[0.999826,-0.0155997,-0.00405075,0.00939318],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp15:=[[354.16,137.90,203.81],[0.999826,-0.0156103,-0.0040453,0.00941586],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp16:=[[354.40,138.09,173.24],[0.999825,-0.015628,-0.00403922,0.00945458],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp17:=[[354.63,138.27,143.31],[0.999825,-0.0156403,-0.00403433,0.00948092],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp18:=[[354.86,138.46,113.22],[0.999824,-0.0156569,-0.00402251,0.00951363],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp19:=[[355.10,138.64,82.50],[0.999824,-0.0156514,-0.00402062,0.00953432],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp20:=[[353.17,138.90,50.63],[0.999818,-0.0156433,-0.00533198,0.00956749],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp21:=[[353.40,139.08,20.17],[0.999817,-0.015663,-0.00532834,0.00960587],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp22:=[[353.63,139.27,-9.71],[0.999816,-0.0156909,-0.00532695,0.00965102],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp23:=[[353.87,139.47,-40.78],[0.999815,-0.0157141,-0.0053249,0.00970075],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp24:=[[331.47,381.41,633.39],[0.999811,-0.0147978,-0.0054917,0.0113349],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp25:=[[337.93,380.75,602.91],[0.999915,-0.0107735,-0.00427597,0.00601987],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp26:=[[354.75,380.77,572.96],[0.999915,-0.0107634,-0.00427666,0.00606778],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp27:=[[353.19,380.41,541.74],[0.999906,-0.0122162,0.000789537,0.00613096],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp28:=[[353.96,380.76,511.16],[0.999911,-0.011798,-0.000693003,0.00613811],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp29:=[[354.22,381.58,480.86],[0.999911,-0.0117871,-0.000705914,0.00613887],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp30:=[[354.09,381.64,449.94],[0.999908,-0.0120991,0.000366658,0.00615926],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp31:=[[354.33,381.82,418.58],[0.999908,-0.0120977,0.00036414,0.00615959],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp32:=[[354.57,382.57,388.85],[0.999908,-0.0120948,0.000374984,0.0061522],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp33:=[[354.43,382.37,358.11],[0.999786,-0.0193647,0.00157683,0.00713599],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp34:=[[354.66,382.53,328.90],[0.999785,-0.0193752,0.00157953,0.00714703],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp35:=[[355.63,384.01,296.42],[0.999885,-0.0139007,-0.000440376,0.00602468],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp36:=[[355.86,384.19,266.52],[0.999885,-0.0138995,-0.000438155,0.00604482],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp37:=[[356.11,384.36,236.68],[0.999885,-0.013909,-0.000428451,0.00605974],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp38:=[[356.75,384.67,205.23],[0.999888,-0.0135918,-0.00153155,0.00606301],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp39:=[[356.99,384.82,176.04],[0.999888,-0.01362,-0.00152927,0.00606686],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp40:=[[357.23,385.00,144.80],[0.999887,-0.013632,-0.00151192,0.00608646],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp41:=[[356.28,385.76,113.96],[0.999887,-0.0122202,-0.00632451,0.00608557],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp42:=[[356.09,385.81,83.10],[0.999889,-0.012618,-0.00506072,0.00613563],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp43:=[[355.86,385.85,52.24],[0.999889,-0.0130289,-0.00371186,0.00616507],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp44:=[[356.10,386.03,21.94],[0.999889,-0.0130492,-0.00371514,0.0062008],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp45:=[[356.33,386.22,-8.88],[0.999888,-0.0130702,-0.00371638,0.00623696],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp46:=[[356.34,386.32,-39.17],[0.999887,-0.0132707,-0.003094,0.00627156],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp47:=[[326.74,623.25,635.32],[0.999914,-0.0107986,-0.00426971,0.0061048],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp48:=[[333.58,623.38,603.69],[0.999914,-0.0107812,-0.00427024,0.00612283],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp49:=[[357.46,625.47,574.49],[0.999952,-0.00305262,-0.00516121,0.00776183],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp50:=[[356.93,625.22,543.81],[0.999956,-0.00426976,-0.002983,0.00780496],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp51:=[[356.58,625.09,513.44],[0.9999,-0.0122717,-0.00117482,0.00686932],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp52:=[[356.14,624.55,482.99],[0.999933,-0.00900366,0.000623937,0.00731607],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp53:=[[357.13,625.16,451.50],[0.999942,-0.00782208,-0.00149072,0.00730679],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp54:=[[357.38,626.77,421.91],[0.999942,-0.00781879,-0.00148705,0.0073069],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp55:=[[357.64,626.96,390.75],[0.999941,-0.00782284,-0.00148208,0.00732235],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp56:=[[358.28,627.46,360.18],[0.999906,-0.0110666,-0.00254013,0.0076485],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp57:=[[358.00,627.62,329.58],[0.999937,-0.00846024,-0.00107243,0.00723399],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp58:=[[358.24,627.80,298.84],[0.999937,-0.00847453,-0.00105525,0.00724562],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp59:=[[359.05,627.49,269.18],[0.999901,-0.0112571,-0.00261928,0.00804665],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp60:=[[358.59,627.27,239.25],[0.999891,-0.0123736,-0.000660279,0.00808572],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp61:=[[358.84,627.46,207.23],[0.99989,-0.0123907,-0.000652971,0.00810959],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp62:=[[359.01,627.59,177.60],[0.999888,-0.0125182,-0.000456755,0.00813404],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp63:=[[359.72,629.59,146.82],[0.999896,-0.0118021,-0.00175152,0.0081358],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp64:=[[359.96,629.77,115.89],[0.999896,-0.0118023,-0.00174636,0.00816421],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp65:=[[360.20,629.95,85.60],[0.999895,-0.0118218,-0.00175258,0.00818474],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp66:=[[360.44,630.12,55.55],[0.999895,-0.0118344,-0.00175363,0.00819404],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp67:=[[359.69,629.72,24.76],[0.999876,-0.0134108,0.0010085,0.00822693],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp68:=[[359.93,629.90,-6.08],[0.999875,-0.0134332,0.00101617,0.00825616],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp69:=[[360.49,630.27,-37.18],[0.999882,-0.0129373,0.000115525,0.00827929],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp70:=[[117.47,871.62,635.17],[0.999952,-0.00307383,-0.00516145,0.00772701],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp71:=[[224.70,870.71,605.54],[0.999952,-0.00306043,-0.005148,0.00775771],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp72:=[[287.65,870.22,576.13],[0.999952,-0.00309618,-0.00515484,0.00776251],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp73:=[[354.95,869.73,546.40],[0.999952,-0.00308384,-0.00515957,0.00778448],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp74:=[[355.19,869.90,515.92],[0.999952,-0.00309032,-0.00516533,0.00778619],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp75:=[[360.55,868.83,484.41],[0.999948,-0.00647655,-0.00111027,0.00783676],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp76:=[[361.11,869.86,453.38],[0.999941,-0.00749554,-0.001949,0.00757678],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp77:=[[361.34,870.03,423.27],[0.999941,-0.00749537,-0.00194667,0.0075812],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp78:=[[361.79,869.92,392.32],[0.999935,-0.00818481,-0.00252626,0.00751235],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp79:=[[362.02,870.10,362.37],[0.999935,-0.00818043,-0.00252846,0.00753182],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp80:=[[362.28,870.95,331.56],[0.999935,-0.00819508,-0.00251152,0.00754881],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp81:=[[362.52,871.13,301.41],[0.999935,-0.00820144,-0.0025054,0.00755881],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp82:=[[361.80,871.31,270.44],[0.999935,-0.00820105,-0.00251058,0.00756183],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp83:=[[361.14,872.81,240.26],[0.999951,-0.00682655,5.30802E-05,0.00712324],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp84:=[[361.39,872.99,209.38],[0.999951,-0.00684082,4.53594E-05,0.00713508],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp85:=[[360.10,872.95,180.45],[0.999935,-0.00836053,-0.00122894,0.00763245],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp86:=[[360.35,874.09,148.83],[0.999935,-0.00837051,-0.00121695,0.00764644],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp87:=[[361.05,874.16,118.36],[0.999913,-0.00989276,-0.00248368,0.0083394],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp88:=[[361.29,874.33,87.72],[0.999913,-0.00990059,-0.00248569,0.00835244],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp89:=[[360.72,873.81,56.73],[0.999895,-0.011825,-0.000197653,0.00836898],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp90:=[[360.96,875.20,26.53],[0.999895,-0.0118284,-0.000191439,0.00838305],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp91:=[[361.21,875.39,-4.54],[0.999895,-0.0118345,-0.000187378,0.00840726],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp92:=[[361.44,875.55,-34.39],[0.999894,-0.01184,-0.000180527,0.00841147],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!*****12 matrix put position end
CONST robtarget p23:=[[325,777,-30],[1,0,0,0],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p24:=[[951.67,291.58,305.65],[0.283752,0.381565,-0.878309,0.0496596],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p25:=[[957.63,293.21,337.45],[0.265933,0.381999,-0.883532,0.0522222],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p26:=[[961.87,294.37,365.18],[0.250414,0.382266,-0.887808,0.0544283],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p27:=[[966.30,295.57,403.79],[0.228818,0.38247,-0.893341,0.057459],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p28:=[[968.46,296.16,431.77],[0.213148,0.382496,-0.897056,0.0596298],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p29:=[[969.95,296.57,468.46],[0.192573,0.382378,-0.901556,0.0624448],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p30:=[[969.94,296.56,505.18],[0.17191,0.382088,-0.905649,0.0652315],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p31:=[[943.39,251.39,232.51],[0.325692,0.363748,-0.871897,0.0375142],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p32:=[[933.30,286.58,232.51],[0.324931,0.380032,-0.864924,0.043614],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p33:=[[943.52,289.36,269.92],[0.303826,0.380911,-0.872016,0.0467343],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p34:=[[951.67,291.58,305.65],[0.283752,0.381565,-0.878309,0.0496596],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p35:=[[957.63,293.21,337.45],[0.265933,0.381999,-0.883532,0.0522222],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p36:=[[961.87,294.37,365.18],[0.250414,0.382266,-0.887808,0.0544283],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p37:=[[966.30,295.57,403.79],[0.228818,0.38247,-0.893341,0.057459],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p38:=[[968.46,296.16,431.77],[0.213148,0.382496,-0.897056,0.0596298],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p39:=[[969.95,296.57,468.46],[0.192573,0.382378,-0.901556,0.0624448],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p40:=[[969.94,296.56,505.18],[0.17191,0.382088,-0.905649,0.0652315],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p41:=[[943.39,251.39,232.51],[0.325692,0.363748,-0.871897,0.0375142],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p42:=[[933.30,286.58,232.51],[0.324931,0.380032,-0.864924,0.043614],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p43:=[[943.52,289.36,269.92],[0.303826,0.380911,-0.872016,0.0467343],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p44:=[[951.67,291.58,305.65],[0.283752,0.381565,-0.878309,0.0496596],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p45:=[[957.63,293.21,337.45],[0.265933,0.381999,-0.883532,0.0522222],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p46:=[[961.87,294.37,365.18],[0.250414,0.382266,-0.887808,0.0544283],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p47:=[[966.30,295.57,403.79],[0.228818,0.38247,-0.893341,0.057459],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p48:=[[968.46,296.16,431.77],[0.213148,0.382496,-0.897056,0.0596298],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p49:=[[969.95,296.57,468.46],[0.192573,0.382378,-0.901556,0.0624448],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p50:=[[969.94,296.56,505.18],[0.17191,0.382088,-0.905649,0.0652315],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pbackhomepos:=[[409.48,-496.58,1005.14],[0.999432,-0.0284694,0.00852407,0.0158778],[-1,-1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pbackhomepos10:=[[409.48,-496.58,1005.13],[0.999432,-0.0284685,0.00852428,0.0158778],[-1,-1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget jbackhomepos:=[[409.48,-496.58,1005.14],[0.999432,-0.0284698,0.00852399,0.0158778],[-1,-1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget PNowPos10:=[[582.05,356.05,1058.28],[0.0622783,0.000912002,-0.0132789,-0.99797],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS speeddata speed1:=[1000,600,600,600];
PERS speeddata speed2:=[350,350,350,300];
PERS speeddata speed3:=[200,200,200,200];
VAR socketdev socket_server;
VAR socketdev socket_client;
PERS string str_address:="192.168.103.12";
!PERS string str_address:="192.168.201.88";
PERS num n_port:=10021;
Var pos now_pos:=[0,0,0];
VAR robtarget PNowPos;
VAR string strReceive:="p1,L,100";
VAR string resultStr_FINISHED:=",FINISHED;";
VAR string resultStr_OK:=",OK;";
VAR string resultStr_NG:=",NG;";
VAR socketstatus status;
VAR num starBit1:=0;
VAR num endBit1:=0;
VAR num LengBit1:=0;
VAR num starBit2:=0;
VAR num endBit2:=0;
VAR num LengBit2:=0;
VAR num starBit3:=0;
VAR num endBit3:=0;
VAR num LengBit3:=0;
VAR num endBit4:=0;
VAR num LengBit4:=0;
VAR num starBit4:=0;
PERS string shelfType:="p";
PERS string revice_Str1_Cmd:="moveget";
PERS string revice_Str2_pName:="p1";
PERS string revice_Str3_moveType:="L";
PERS string revice_Str4_speed:="1000";
VAR bool flag1:=false;
!PERS num value_1:=700;
!PERS num value_2:=50;
VAR robtarget pTarget:=[[16.70,-8.84,-146.71],[0.995232,-0.0069405,-0.00673188,0.0970544],[-1,-3,-3,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget pTemp:=[[700,50,700],[0.0258621,-0.000364211,-0.999665,3.06266E-6],[0,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS bool booLineMove:=TRUE;
PERS bool dataOk:=TRUE;
VAR num len:=0;
PERS zonedata zone1:=[FALSE,5,8,8,0.8,8,0.8];
PERS loaddata load1:=[4,[0,0,0],[1,0,0,0],0,0,0];
PERS robtarget pCheckTest:=[[-1.73995,-351.15,465.56],[0.300709,-0.919332,0.248148,0.0531501],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS bool pCheckResult:=TRUE;
PERS tooldata tooltest:=[TRUE,[[-208.798,-217.638,93.8838],[1,0,0,0]],[1.3,[-31.7,-112.8,55.6],[1,0,0,0],0,0.045,0.029]];
PERS wobjdata wobjtest:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[1033.18,-257.207,479.665],[2.98351E-05,0.707094,0.707119,6.49066E-05]]];
PERS tooldata tool1:=[TRUE,[[209.468,-213.3,114.724],[0.0139655,0.916976,-0.398699,-0.000349054]],[1.7,[123.1,-86.1,47.4],[1,0,0,0],0.028,0,0.035]];
PERS wobjdata wobj1:=[FALSE,TRUE,"",[[137,-964,808.5],[0.00791395,0.00160744,0.0045071,0.999957]],[[0,0,0],[1,0,0,0]]];
PERS wobjdata wobj2:=[FALSE,TRUE,"",[[823,-121,536],[0.99997,-0.00283,0.00395,0.00521]],[[0,0,0],[1,0,0,0]]];
CONST robtarget p17:=[[232.66,0.00,29.24],[1,-1.36004E-05,1.18275E-06,-4.95038E-06],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget PNowPos20:=[[647.80,-254.06,1005.12],[0.999435,-0.0284313,0.00857599,0.0157199],[-1,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
TASK PERS tooldata tool3:=[TRUE,[[0,0,0],[1,0,0,0]],[-1,[0,0,0],[1,0,0,0],0,0,0]];
PROC modifyPoint()
MoveL p1,speed1,z50,tool1\WObj:=wobj1;
MoveL p2,speed1,z50,tool1\WObj:=wobj1;
MoveL p3,speed1,z50,tool1\WObj:=wobj1;
MoveL p4,speed1,z50,tool1\WObj:=wobj1;
MoveL p5,speed1,z50,tool1\WObj:=wobj1;
MoveL p6,speed1,z50,tool1\WObj:=wobj1;
MoveL p7,speed1,z50,tool1\WObj:=wobj1;
MoveL p8,speed1,z50,tool1\WObj:=wobj1;
MoveL p9,speed1,z50,tool1\WObj:=wobj1;
MoveL p10,speed1,z50,tool1\WObj:=wobj1;
MoveL cp1,speed1,z50,tool1\WObj:=wobj2;
MoveL cp2,speed1,z50,tool1\WObj:=wobj2;
MoveL cp3,speed1,z50,tool1\WObj:=wobj2;
MoveL cp4,speed1,z50,tool1\WObj:=wobj2;
MoveL cp5,speed1,z50,tool1\WObj:=wobj2;
MoveL cp6,speed1,z50,tool1\WObj:=wobj2;
MoveL cp7,speed1,z50,tool1\WObj:=wobj2;
MoveL cp8,speed1,z50,tool1\WObj:=wobj2;
MoveL cp9,speed1,z50,tool1\WObj:=wobj2;
MoveL cp10,speed1,z50,tool1\WObj:=wobj2;
MoveL cp11,speed1,z50,tool1\WObj:=wobj2;
MoveL cp12,speed1,z50,tool1\WObj:=wobj2;
MoveL dp1,speed1,z50,tool1\WObj:=wobj2;
MoveL dp2,speed1,z50,tool1\WObj:=wobj2;
MoveL dp3,speed1,z50,tool1\WObj:=wobj2;
MoveL dp4,speed1,z50,tool1\WObj:=wobj2;
MoveL dp5,speed1,z50,tool1\WObj:=wobj2;
MoveL dp6,speed1,z50,tool1\WObj:=wobj2;
MoveL dp7,speed1,z50,tool1\WObj:=wobj2;
MoveL dp8,speed1,z50,tool1\WObj:=wobj2;
MoveL dp9,speed1,z50,tool1\WObj:=wobj2;
MoveL dp10,speed1,z50,tool1\WObj:=wobj2;
MoveL dp11,speed1,z50,tool1\WObj:=wobj2;
MoveL dp12,speed1,z50,tool1\WObj:=wobj2;
MoveL dp13,speed1,z50,tool1\WObj:=wobj2;
MoveL dp14,speed1,z50,tool1\WObj:=wobj2;
MoveL dp15,speed1,z50,tool1\WObj:=wobj2;
MoveL dp16,speed1,z50,tool1\WObj:=wobj2;
MoveL dp17,speed1,z50,tool1\WObj:=wobj2;
MoveL dp18,speed1,z50,tool1\WObj:=wobj2;
MoveL dp19,speed1,z50,tool1\WObj:=wobj2;
MoveL dp20,speed1,z50,tool1\WObj:=wobj2;
MoveL dp21,speed1,z50,tool1\WObj:=wobj2;
MoveL dp22,speed1,z50,tool1\WObj:=wobj2;
MoveL dp23,speed1,z50,tool1\WObj:=wobj2;
MoveL dp24,speed1,z50,tool1\WObj:=wobj2;
MoveL dp25,speed1,z50,tool1\WObj:=wobj2;
MoveL dp26,speed1,z50,tool1\WObj:=wobj2;
MoveL dp27,speed1,z50,tool1\WObj:=wobj2;
MoveL dp28,speed1,z50,tool1\WObj:=wobj2;
MoveL dp29,speed1,z50,tool1\WObj:=wobj2;
MoveL dp30,speed1,z50,tool1\WObj:=wobj2;
MoveL dp31,speed1,z50,tool1\WObj:=wobj2;
MoveL dp32,speed1,z50,tool1\WObj:=wobj2;
MoveL dp33,speed1,z50,tool1\WObj:=wobj2;
MoveL dp34,speed1,z50,tool1\WObj:=wobj2;
MoveL dp35,speed1,z50,tool1\WObj:=wobj2;
MoveL dp36,speed1,z50,tool1\WObj:=wobj2;
MoveL dp37,speed1,z50,tool1\WObj:=wobj2;
MoveL dp38,speed1,z50,tool1\WObj:=wobj2;
MoveL dp39,speed1,z50,tool1\WObj:=wobj2;
MoveL dp40,speed1,z50,tool1\WObj:=wobj2;
MoveL dp41,speed1,z50,tool1\WObj:=wobj2;
MoveL dp42,speed1,z50,tool1\WObj:=wobj2;
MoveL dp43,speed1,z50,tool1\WObj:=wobj2;
MoveL dp44,speed1,z50,tool1\WObj:=wobj2;
MoveL dp45,speed1,z50,tool1\WObj:=wobj2;
MoveL dp46,speed1,z50,tool1\WObj:=wobj2;
MoveL dp47,speed1,z50,tool1\WObj:=wobj2;
MoveL dp48,speed1,z50,tool1\WObj:=wobj2;
MoveL dp49,speed1,z50,tool1\WObj:=wobj2;
MoveL dp50,speed1,z50,tool1\WObj:=wobj2;
MoveL dp51,speed1,z50,tool1\WObj:=wobj2;
MoveL dp52,speed1,z50,tool1\WObj:=wobj2;
MoveL dp53,speed1,z50,tool1\WObj:=wobj2;
MoveL dp54,speed1,z50,tool1\WObj:=wobj2;
MoveL dp55,speed1,z50,tool1\WObj:=wobj2;
MoveL dp56,speed1,z50,tool1\WObj:=wobj2;
MoveL dp57,speed1,z50,tool1\WObj:=wobj2;
MoveL dp58,speed1,z50,tool1\WObj:=wobj2;
MoveL dp59,speed1,z50,tool1\WObj:=wobj2;
MoveL dp60,speed1,z50,tool1\WObj:=wobj2;
MoveL dp61,speed1,z50,tool1\WObj:=wobj2;
MoveL dp62,speed1,z50,tool1\WObj:=wobj2;
MoveL dp63,speed1,z50,tool1\WObj:=wobj2;
MoveL dp64,speed1,z50,tool1\WObj:=wobj2;
MoveL dp65,speed1,z50,tool1\WObj:=wobj2;
MoveL dp66,speed1,z50,tool1\WObj:=wobj2;
MoveL dp67,speed1,z50,tool1\WObj:=wobj2;
MoveL dp68,speed1,z50,tool1\WObj:=wobj2;
MoveL dp69,speed1,z50,tool1\WObj:=wobj2;
MoveL dp70,speed1,z50,tool1\WObj:=wobj2;
MoveL dp71,speed1,z50,tool1\WObj:=wobj2;
MoveL dp72,speed1,z50,tool1\WObj:=wobj2;
MoveL dp73,speed1,z50,tool1\WObj:=wobj2;
MoveL dp74,speed1,z50,tool1\WObj:=wobj2;
MoveL dp75,speed1,z50,tool1\WObj:=wobj2;
MoveL dp76,speed1,z50,tool1\WObj:=wobj2;
MoveL dp77,speed1,z50,tool1\WObj:=wobj2;
MoveL dp78,speed1,z50,tool1\WObj:=wobj2;
MoveL dp79,speed1,z50,tool1\WObj:=wobj2;
MoveL dp80,speed1,z50,tool1\WObj:=wobj2;
MoveL dp81,speed1,z50,tool1\WObj:=wobj2;
MoveL dp82,speed1,z50,tool1\WObj:=wobj2;
MoveL dp83,speed1,z50,tool1\WObj:=wobj2;
MoveL dp84,speed1,z50,tool1\WObj:=wobj2;
MoveL dp85,speed1,z50,tool1\WObj:=wobj2;
MoveL dp86,speed1,z50,tool1\WObj:=wobj2;
MoveL dp87,speed1,z50,tool1\WObj:=wobj2;
MoveL dp88,speed1,z50,tool1\WObj:=wobj2;
MoveL dp89,speed1,z50,tool1\WObj:=wobj2;
MoveL dp90,speed1,z50,tool1\WObj:=wobj2;
MoveL dp91,speed1,z50,tool1\WObj:=wobj2;
MoveL dp92,speed1,z50,tool1\WObj:=wobj2;
MoveL p23,speed1,z50,tool1\WObj:=wobj2;
MoveL p24,speed1,z50,tool1\WObj:=wobj2;
MoveL p25,speed1,z50,tool1\WObj:=wobj2;
MoveL p26,speed1,z50,tool1\WObj:=wobj2;
MoveL p27,speed1,z50,tool1\WObj:=wobj2;
MoveL p28,speed1,z50,tool1\WObj:=wobj2;
MoveL p29,speed1,z50,tool1\WObj:=wobj2;
MoveL p30,speed1,z50,tool1\WObj:=wobj2;
MoveL p31,speed1,z50,tool1\WObj:=wobj2;
MoveL p32,speed1,z50,tool1\WObj:=wobj2;
MoveL p33,speed1,z50,tool1\WObj:=wobj2;
MoveL p34,speed1,z50,tool1\WObj:=wobj2;
MoveL p35,speed1,z50,tool1\WObj:=wobj2;
MoveL p36,speed1,z50,tool1\WObj:=wobj2;
MoveL p37,speed1,z50,tool1\WObj:=wobj2;
MoveL p38,speed1,z50,tool1\WObj:=wobj2;
MoveL p39,speed1,z50,tool1\WObj:=wobj2;
MoveL p40,speed1,z50,tool1\WObj:=wobj2;
MoveL p41,speed1,z50,tool1\WObj:=wobj2;
MoveL p42,speed1,z50,tool1\WObj:=wobj2;
MoveL p43,speed1,z50,tool1\WObj:=wobj2;
MoveL p44,speed1,z50,tool1\WObj:=wobj2;
MoveL p45,speed1,z50,tool1\WObj:=wobj2;
MoveL p46,speed1,z50,tool1\WObj:=wobj2;
MoveL p47,speed1,z50,tool1\WObj:=wobj2;
MoveL p48,speed1,z50,tool1\WObj:=wobj2;
MoveL p49,speed1,z50,tool1\WObj:=wobj2;
MoveL p50,speed1,z50,tool1\WObj:=wobj2;
ENDPROC
PROC main()
zone1:=z5;
SocketPro;
error
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ELSE
SocketPro;
RETURN ;
ENDIF
ENDPROC
PROC loadexample()
VAR robtarget ptemp:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
VAR robtarget pPlace:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
MoveL Offs(pTemp,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL pTemp,speed1,fine,tool1\WObj:=wobj1;
GripLoad load1;
MoveL Offs(pTemp,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL Offs(pPlace,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL pPlace,speed1,fine,tool1\WObj:=wobj1;
GripLoad load0;
MoveL Offs(pPlace,0,0,10),speed1,z50,tool1\WObj:=wobj1;
ENDPROC
PROC socketRecover()
SocketClose socket_client;
SocketCreate socket_client;
SocketConnect socket_client,str_address,n_port\Time:=5;
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ResetRetryCount;
RETRY;
ENDIF
ENDPROC
PROC SocketPro()
TPErase;
SocketClose socket_client;
!create client
SocketCreate socket_client;
!connect server
SocketConnect socket_client,str_address,n_port\Time:=WAIT_MAX;
WHILE TRUE DO
pTarget:=CRobT(\Tool:=tool1);
ptemp:=pTarget;
!receive instruction
label1:
SocketReceive socket_client\Str:=strReceive;
dataOk:=FALSE;
strParse strReceive;
IF dataOk THEN
! GOTO label1;
IF revice_Str1_Cmd="movep" THEN
IF booLineMove THEN
MoveL pTarget, speed1, fine, tool1\WObj:=wobj1;
ELSE
MoveJ pTarget, speed1, fine, tool1\WObj:=wobj1;
ENDIF
SocketSend socket_client\Str:=strReceive+resultStr_OK;
!SocketSend socket_client\Str:=revice_Str1_Cmd+","+revice_Str2_pName+","+resultStr_OK+";";
!!!!!!!!!!!!!
ELSEIF revice_Str1_Cmd="movehome" THEN
rBackHome;
SocketSend socket_client\Str:=strReceive+resultStr_OK;
!!!!!!!!!!!!!!!
ELSEIF revice_Str1_Cmd="moveget" THEN
MoveL Home_Wobj1, speed1, z50, tool1\WObj:=wobj1;
IF booLineMove THEN
! MoveL Offs(pTarget,0,0,10), speed1, zone1, tool1\WObj:=wobj1;
!MoveL Home_Wobj1,speed1,fine,tool1\WObj:=wobj1;
! GripLoad load0;
MoveL Offs(pTarget,0,0,-30), speed1, z80, tool1\WObj:=wobj1;
MoveL Offs(pTarget,0,0,80), speed3, z80, tool1\WObj:=wobj1;
!Set do_02;
!waittime 1;
MoveL RelTool(pTarget,-400,0,100\Rz:=90), speed2, z80, tool1\WObj:=wobj1;
MoveL RelTool(pTarget,-500,-100,100\Rz:=135), speed2, z80, tool1\WObj:=wobj1;
ELSE
!MoveJ pTarget,speed1,fine,tool1\WObj:=wobj1;
ENDIF
SocketSend socket_client\Str:=strReceive+resultStr_OK;
! SocketSend socket_client\Str:=revice_Str1_Cmd+","+revice_Str2_pName+","+resultStr_OK+";";
ELSEIF revice_Str1_Cmd="moveput" THEN
!MoveJ Home_Wobj2,speed1,fine,tool1\WObj:=wobj1;
IF booLineMove AND shelfType="c" THEN
MoveL Temp_Wobj2, speed2, z100, tool1\WObj:=wobj2;
! GripLoad load1;
MoveL Offs(pTarget,-450,0,20), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,20), speed2, zone1, tool1\WObj:=wobj2;
!reset do_02;
MoveL Offs(pTarget,0,0,-20), speed2, zone1, tool1\WObj:=wobj2;
SocketSend socket_client\Str:=strReceive+resultStr_FINISHED;
MoveL Offs(pTarget,-450,0,-20), speed2, zone1, tool1\WObj:=wobj2;
ELSEIF booLineMove AND shelfType="d" THEN
MoveL Temp_Wobj2, speed2, z100, tool1\WObj:=wobj2;
! GripLoad load1;
MoveL Offs(pTarget,-350,0,8), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,8), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,-9), speed2, zone1, tool1\WObj:=wobj2;
SocketSend socket_client\Str:=strReceive+resultStr_FINISHED;
MoveL Offs(pTarget,-350,0,-9), speed2, zone1, tool1\WObj:=wobj2;
!MoveJ pTarget,speed1,fine,tool1\WObj:=wobj2;
ENDIF
! SocketSend socket_client\Str:=strReceive+resultStr_OK;
MoveL Home_Wobj1, speed1, z100, tool1\WObj:=wobj1;
SocketSend socket_client\Str:=strReceive+resultStr_OK;
ELSEIF revice_Str1_Cmd="validateP" THEN
!WaitRob\ZeroSpeed;
!wait robot stop;
pCheckTest:=CRobT(\Tool:=tool1\WObj:=wobj1);
!get current position
pCheckResult:=false;
pCheckResult:=validateP(pTarget,pCheckTest,0.1);
IF pCheckResult THEN
SocketSend socket_client\Str:=strReceive+resultStr_OK;
ELSE
SocketSend socket_client\Str:=strReceive+resultStr_NG;
ENDIF
ELSE
SocketSend socket_client\Str:=strReceive+" DATA ERROR ;";
ENDIF
ENDIF
ENDWHILE
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ResetRetryCount;
RETRY;
ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
socketRecover;
ResetRetryCount;
RETRY;
ENDIF
ENDPROC
FUNC bool validateP(robtarget ptest,robtarget pCompare,num threshold)
VAR jointtarget joinTest:=[[-12.1057,1.3138,-1.32846,-3.7545E-7,90.0147,-12.1057],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
VAR jointtarget joinCompare:=[[-12.143,1.32099,-1.33096,-0.000317427,90.0147,-12.1215],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
VAR num counter;
joinTest:=CalcJointT(pTest,tool1\WObj:=wobj1);
joinCompare:=CalcJointT(pCompare,tool1\WObj:=wobj1);
IF abs(joinTest.robax.rax_1-joinCompare.robax.rax_1)<threshold incr counter;
IF abs(joinTest.robax.rax_2-joinCompare.robax.rax_2)<threshold incr counter;
IF abs(joinTest.robax.rax_3-joinCompare.robax.rax_3)<threshold incr counter;
IF abs(joinTest.robax.rax_4-joinCompare.robax.rax_4)<threshold incr counter;
IF abs(joinTest.robax.rax_5-joinCompare.robax.rax_5)<threshold incr counter;
IF abs(joinTest.robax.rax_6-joinCompare.robax.rax_6)<threshold incr counter;
IF counter=6 THEN
RETURN TRUE;
ELSE
RETURN FALSE;
ENDIF
ENDFUNC
PROC strParse(string reviceStr)
VAR num revice_speed:=10;
lable2:
len:=StrLen(reviceStr);
starBit1:=1;
endBit1:=StrFind(reviceStr,starBit1,",");
LengBit1:=endBit1-starBit1;
starBit2:=endBit1+1;
IF starBit2>len THEN
RETURN ;
ENDIF
endBit2:=StrFind(reviceStr,starBit2,",");
LengBit2:=endBit2-starBit2;
starBit3:=endBit2+1;
IF starBit3>len THEN
RETURN ;
ENDIF
endBit3:=StrFind(reviceStr,starBit3,",");
LengBit3:=endBit3-starBit3;
starBit4:=endBit3+1;
IF starBit4>len THEN
RETURN ;
ENDIF
endBit4:=StrFind(reviceStr,starBit4,",");
LengBit4:=endBit4-starBit4;
revice_Str1_Cmd:=StrPart(reviceStr,starBit1,LengBit1);
revice_Str2_pName:=StrPart(reviceStr,starBit2,LengBit2);
revice_Str3_moveType:=StrPart(reviceStr,starBit3,LengBit3);
revice_Str4_speed:=StrPart(reviceStr,starBit4,LengBit4);
shelfType:= StrPart(revice_Str2_pName,1,1);
!capture target
GetDataVal revice_Str2_pName,ptemp;
!Stop;
!Stop;
pTarget:=ptemp;
!determine L or J
IF revice_Str3_moveType="L" THEN
booLineMove:=TRUE;
ELSEIF revice_Str3_moveType="J" THEN
booLineMove:=FALSE;
ENDIF
! get speed
flag1:=StrToVal(revice_Str4_speed,revice_speed);
IF revice_speed>1000 THEN
revice_speed:=1000;
ENDIF
speed1.v_tcp:=revice_speed;
dataOk:=TRUE;
ERROR
IF errno=ERR_SOCK_CLOSED THEN
ELSE
dataOk:=FALSE;
RETURN ;
ENDIF
ENDPROC
!PROC Path_10()
! MoveL Target_10,v1000,z100,tool1\WObj:=wobj1;
! MoveL Target_20,v1000,z100,tool1\WObj:=wobj1;
!ENDPROC
PROC rBackHome()
PNowPos:=CRobT(\Tool:=tool1\Wobj:=wobj0);
IF PNowPos.trans.y<=-620 and PNowPos.trans.z>820 THEN
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
endif
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSEIF PNowPos.trans.y<=-620 and PNowPos.trans.z<=820 AND PNowPos.trans.X<=300 THEN
PNowPos.trans.x:=300;
MOVEL PNowPos,V100,Z10,TOOL1;
PNowPos.trans.z:=900;
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSEIF PNowPos.trans.y<=-620 and PNowPos.trans.z<=820 AND PNowPos.trans.X>300 THEN
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
endif
MOVEL PNowPos, v100, Z10, TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
!TPWrite "chang to manual module";
!stop;
ELSeIF (-620<PNowPos.trans.y and PNowPos.trans.y<=550) AND PNowPos.trans.X<=715 then
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
ENDIF
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSeIF (-620<PNowPos.trans.y and PNowPos.trans.y<=550) AND PNowPos.trans.X>715 then
PNowPos.trans.X:=715;
MOVEL PNowPos,V100,Z10,TOOL1;
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
ENDIF
MOVEL PNowPos,V100,Z10,TOOL1;
MoveL PNowPos20, v100, z10, tool1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSeIF PNowPos.trans.y> 550 then
TPWrite"robot is to far";
TPWrite"move the robot to safty posion by manule module";
stop;
ENDIF
ENDPROC
PROC Routine1()
MoveL [[-1.74,-351.15,465.56],[0.300707,-0.919334,0.248145,0.0531493],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z50, tool1\WObj:=wobj1;
MoveL [[-4.66,-300.07,453.63],[0.282192,-0.925597,0.248225,0.0449703],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z10, tool1\WObj:=wobj1;
MoveL [[79.15,-300.86,453.64],[0.282202,-0.925593,0.248224,0.0449831],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z10, tool1\WObj:=wobj1;
MoveL [[78.71,-347.14,453.64],[0.282192,-0.92559,0.248244,0.0450091],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z10, tool1\WObj:=wobj1;
ENDPROC
PROC Routine2()
MoveAbsJ [[0,0,0,0,90,-45],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs,v1000,fine,tool0;
ENDPROC
ENDMODULE
\ No newline at end of file
source/AssemblyLineClient/App.config
......@@ -91,7 +91,17 @@
</layout>
</appender>
<appender name="TheRFID" type="log4net.Appender.RollingFileAppender">
<file value="logs/TheRFID-line.log" />
<file value="logs/rfid/TheRFID-line.log" />
<param name="Encoding" value="UTF-8" />
<appendToFile value="true" />
<rollingStyle value="Date" />
<datePattern value="yyyy-MM-dd" />
<layout type="log4net.Layout.PatternLayout">
<conversionPattern value="[%date][%t]%-5p %m%n" />
</layout>
</appender>
<appender name="AIOBOX" type="log4net.Appender.RollingFileAppender">
<file value="logs/aio/AIOBOX-line.log" />
<param name="Encoding" value="UTF-8" />
<appendToFile value="true" />
<rollingStyle value="Date" />
......@@ -108,6 +118,10 @@
<level value="Info" />
<appender-ref ref="TheRFID" />
</logger>
<logger name="AIOBOX">
<level value="Info" />
<appender-ref ref="AIOBOX" />
</logger>
<!--<root>
<level value="Info" />
<appender-ref ref="RollingLogFileAppender" />
......
source/DeviceLibrary/DeviceLibrary.csproj
......@@ -36,9 +36,6 @@
<Prefer32Bit>false</Prefer32Bit>
</PropertyGroup>
<ItemGroup>
<Reference Include="Asa.IOModule.AIOBOX">
<HintPath>..\..\dll\Asa.IOModule.AIOBOX.dll</HintPath>
</Reference>
<Reference Include="Asa.RFID">
<HintPath>..\..\dll\RFID\Asa.RFID.dll</HintPath>
</Reference>
......@@ -87,6 +84,7 @@
<Compile Include="baan\AxisBean.cs" />
<Compile Include="baan\WaitUtil.cs" />
<Compile Include="deviceLibrary\halcon\CodeManager.cs" />
<Compile Include="deviceLibrary\IO\AIOBOX\AIOBOX.cs" />
<Compile Include="deviceLibrary\IO\AIOBOX\AIOBOXManager.cs" />
<Compile Include="deviceLibrary\IO\IOManager.cs" />
<Compile Include="assemblyLine\LineBean.cs" />
......
source/DeviceLibrary/deviceLibrary/IO/AIOBOX/AIOBOXManager.cs
......@@ -94,8 +94,8 @@ namespace OnlineStore.DeviceLibrary
//aioBox.AutoReadInput(true);
//aioBox.AutoReadOutput(false, DOMS);
aioBox = new AIOBOX();
aioBox.SetType(Asa.IOModule.Box_Type.DI, DILength, Asa.IOModule.Box_Type.DO, DOLength);
aioBox = new AIOBOX(Asa.IOModule.Box_Type.DI, DILength, Asa.IOModule.Box_Type.DO, DOLength);
// aioBox.SetType(Asa.IOModule.Box_Type.DI, DILength, Asa.IOModule.Box_Type.DO, DOLength);
aioBox.IP = ioIp;
aioBox.Upload = true;
......
支持 Markdown 格式
你添加了 0 人 到此讨论。请谨慎行事。
Finish editing this message first!