Advertisement
Guest User

Untitled

a guest
Oct 14th, 2019
113
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 10.32 KB | None | 0 0
  1. MODULE Module1
  2.  
  3. VAR num masa := 0;
  4. VAR num i:=0;
  5. VAR socketdev client_socket;
  6. VAR rawbytes merenja;
  7. VAR num header := 0;
  8. VAR num Fx := 0;
  9. VAR num Fy := 0;
  10. VAR num Fz := 0;
  11. VAR num Tx := 0;
  12. VAR num Ty := 0;
  13. VAR num Tz := 0;
  14. VAR num biasFX := 0;
  15. VAR num biasFY := 0;
  16. VAR num biasFZ := 0;
  17. VAR num biasTX := 0;
  18. VAR num biasTY := 0;
  19. VAR num biasTZ := 0;
  20. VAR num sile{6,24};
  21.  
  22.  
  23.  
  24.  
  25. ! targeti za prolazenje kroz 24 pozicije za kalibraciju
  26. VAR byte komanda{1} := [1];
  27. CONST jointtarget t1:=[[0,0,0,0,0,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  28. CONST jointtarget t2:=[[-90,0,0,90,-90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  29. CONST jointtarget t3:=[[0,0,0,0,-90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  30. CONST jointtarget t4:=[[0,0,0,0,90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  31. CONST jointtarget t5:=[[0,0,0,-90,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  32. CONST jointtarget t6:=[[90,0,0,0,0,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  33. CONST jointtarget t7:=[[0,0,0,0,-90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  34. CONST jointtarget t8:=[[0,0,0,0,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  35. CONST jointtarget t9:=[[0,0,0,0,0,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  36. CONST jointtarget t10:=[[-90,0,0,90,-90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  37. CONST jointtarget t11:=[[0,0,0,0,-90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  38. CONST jointtarget t12:=[[0,0,0,0,90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  39. CONST jointtarget t13:=[[0,0,0,90,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  40. CONST jointtarget t14:=[[90,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  41. CONST jointtarget t15:=[[0,0,0,0,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  42. CONST jointtarget t16:=[[0,0,0,0,90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  43. CONST jointtarget t17:=[[-90,0,0,90,90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  44. CONST jointtarget t18:=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  45. CONST jointtarget t19:=[[-90,0,0,90,90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  46. CONST jointtarget t20:=[[0,0,0,0,0,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  47. CONST jointtarget t21:=[[0,0,0,90,90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  48. CONST jointtarget t22:=[[0,0,0,-90,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  49. CONST jointtarget t23:=[[0,0,0,90,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  50. CONST jointtarget t24:=[[0,0,0,90,90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  51.  
  52.  
  53.  
  54.  
  55.  
  56. PROC main()
  57. SocketCreate client_socket;
  58. SocketConnect client_socket, "192.168.0.6", 50005;
  59. Path_1;
  60. Path_9;
  61. Path_18;
  62. Path_20;
  63. Path_3;
  64. Path_7;
  65. Path_11;
  66. Path_15;
  67. Path_4;
  68. Path_12;
  69. Path_16;
  70. Path_8;
  71. Path_13;
  72. Path_5;
  73. Path_17;
  74. Path_19;
  75. Path_21;
  76. Path_22;
  77. Path_23;
  78. Path_24;
  79. Path_6;
  80. Path_14;
  81. Path_10;
  82. Path_2;
  83. ! racunanje bias vrednosti sila kao aritmeticke sredine svih merenja
  84. biasFX := (sile{1,1}+sile{1,2}+sile{1,3}+sile{1,4}+sile{1,5}+sile{1,6}+sile{1,7}+sile{1,8} +sile{1,9} +sile{1,10} +sile{1,11} +sile{1,12} +sile{1,13} +sile{1,14} +sile{1,15} +sile{1,16} +sile{1,17} +sile{1,18} +sile{1,19} +sile{1,20} +sile{1,21} +sile{1,22} + sile{1,23} + sile{1,24})/24;
  85. biasFY := (sile{2,1}+sile{2,2}+sile{2,3}+sile{2,4}+sile{2,5}+sile{2,6}+sile{2,7}+sile{2,8} +sile{2,9} +sile{2,10} +sile{2,11} +sile{2,12} +sile{2,13} +sile{2,14} +sile{2,15} +sile{2,16} +sile{2,17} +sile{2,18} +sile{2,19} +sile{2,20} +sile{2,21} +sile{2,22} + sile{2,23} + sile{2,24})/24;
  86. biasFZ := (sile{3,1}+sile{3,2}+sile{3,3}+sile{3,4}+sile{3,5}+sile{3,6}+sile{3,7}+sile{3,8} +sile{3,9} +sile{3,10} +sile{3,11} +sile{3,12} +sile{3,13} +sile{3,14} +sile{3,15} +sile{3,16} +sile{3,17} +sile{3,18} +sile{3,19} +sile{3,20} +sile{3,21} +sile{3,22} + sile{3,23} + sile{3,24})/24;
  87. biasTX := (sile{4,1}+sile{4,2}+sile{4,3}+sile{4,4}+sile{4,5}+sile{4,6}+sile{4,7}+sile{4,8})/8;
  88. biasTY := (sile{5,9}+sile{5,10}+sile{5,11}+sile{5,12}+sile{5,13}+sile{5,14}+sile{5,15}+sile{5,16})/8;
  89. biasTZ := (sile{6,17}+sile{6,18}+sile{6,19}+sile{6,20}+sile{6,21}+sile{6,22}+sile{6,23}+sile{6,24})/8;
  90.  
  91. MoveAbsJ t18,v1000,z100,griper_senzor\WObj:=wobj0;
  92. WHILE (TRUE) DO
  93. povezivanje_sa_senzorom;
  94.  
  95. ENDWHILE
  96.  
  97. SocketClose client_socket;
  98. ENDPROC
  99.  
  100.  
  101.  
  102. !procedura za primanje mernih podataka sa senzora preko plc-a
  103. PROC povezivanje_sa_senzorom()
  104. WHILE (header <> 4660) DO
  105. SocketSend client_socket \Data:= komanda;
  106. SocketReceive client_socket \RawData:=merenja;
  107. UnpackRawBytes merenja , 1 , header \IntX:=DINT;
  108. ! salje zahteva podatke sa plc-a, sve dok header ne bude 0x1234 (to znaci da je bafer dobar)
  109. ENDWHILE
  110. UnpackRawBytes merenja , 5 , Fx \IntX:=DINT;
  111. UnpackRawBytes merenja , 9 , Fy \IntX:=DINT;
  112. UnpackRawBytes merenja , 13, Fz \IntX:=DINT;
  113. UnpackRawBytes merenja , 17, Tx \IntX:=DINT;
  114. UnpackRawBytes merenja , 21, Ty \IntX:=DINT;
  115. UnpackRawBytes merenja , 25 ,Tz \IntX:=DINT;
  116. Fx := Fx*1.0/1000000 - biasFX ;
  117. Fy := Fy*1.0/1000000 - biasFY;
  118. Fz := Fz*1.0/1000000 - biasFZ;
  119. Tx := Tx*1.0/1000000 - biasTX;
  120. Ty := Ty*1.0/1000000 - biasTY;
  121. Tz := Tz*1.0/1000000 - biasTZ;
  122. header := 0;
  123. ENDPROC
  124.  
  125. PROC ubaci_u_niz(num i)
  126. sile{1,i} := Fx;
  127. sile{2,i} := Fy;
  128. sile{3,i} := Fz;
  129. sile{4,i} := Tx;
  130. sile{5,i} := Ty;
  131. sile{6,i} := Tz;
  132. ENDPROC
  133.  
  134.  
  135. PROC Path_1()
  136. MoveAbsJ t1,v1000,z100,griper_senzor\WObj:=wobj0;
  137. WaitTime 8;
  138. povezivanje_sa_senzorom;
  139.  
  140. i := 1 ;
  141. ubaci_u_niz(i);
  142.  
  143. ENDPROC
  144.  
  145. PROC Path_2()
  146. MoveAbsJ t2,v1000,z100,griper_senzor\WObj:=wobj0;
  147. WaitTime 8;
  148. povezivanje_sa_senzorom;
  149.  
  150. i := 2;
  151. ubaci_u_niz(i);
  152. ENDPROC
  153. PROC Path_3()
  154. MoveAbsJ t3,v1000,z100,griper_senzor\WObj:=wobj0;
  155. WaitTime 8;
  156. povezivanje_sa_senzorom;
  157.  
  158. i := 3;
  159. ubaci_u_niz(i);
  160. ENDPROC
  161. PROC Path_4()
  162. MoveAbsJ t4,v1000,z100,griper_senzor\WObj:=wobj0;
  163. WaitTime 8;
  164. povezivanje_sa_senzorom;
  165.  
  166. i := 4;
  167. ubaci_u_niz(i);
  168. ENDPROC
  169. PROC Path_5()
  170. MoveAbsJ t5,v1000,z100,griper_senzor\WObj:=wobj0;
  171. WaitTime 8;
  172. povezivanje_sa_senzorom;
  173.  
  174. i := 5;
  175. ubaci_u_niz(i);
  176. ENDPROC
  177. PROC Path_6()
  178. MoveAbsJ t6,v1000,z100,griper_senzor\WObj:=wobj0;
  179. WaitTime 8;
  180. povezivanje_sa_senzorom;
  181.  
  182. i := 6;
  183. ubaci_u_niz(i);
  184. ENDPROC
  185. PROC Path_7()
  186. MoveAbsJ t7,v1000,z100,griper_senzor\WObj:=wobj0;
  187. WaitTime 8;
  188. povezivanje_sa_senzorom;
  189.  
  190. i := 7;
  191. ubaci_u_niz(i);
  192. ENDPROC
  193. PROC Path_8()
  194. MoveAbsJ t8,v1000,z100,griper_senzor\WObj:=wobj0;
  195. WaitTime 8;
  196. povezivanje_sa_senzorom;
  197.  
  198. i := 8;
  199. ubaci_u_niz(i);
  200. ENDPROC
  201. PROC Path_9()
  202. MoveAbsJ t9,v1000,z100,griper_senzor\WObj:=wobj0;
  203. WaitTime 8;
  204. povezivanje_sa_senzorom;
  205.  
  206. i := 9;
  207. ubaci_u_niz(i);
  208. ENDPROC
  209. PROC Path_10()
  210. MoveAbsJ t10,v1000,z100,griper_senzor\WObj:=wobj0;
  211. WaitTime 8;
  212. povezivanje_sa_senzorom;
  213.  
  214. i := 10;
  215. ubaci_u_niz(i);
  216. ENDPROC
  217. PROC Path_11()
  218. MoveAbsJ t11,v1000,z100,griper_senzor\WObj:=wobj0;
  219. WaitTime 8;
  220. povezivanje_sa_senzorom;
  221.  
  222. i := 11;
  223. ubaci_u_niz(i);
  224. ENDPROC
  225. PROC Path_12()
  226. MoveAbsJ t12,v1000,z100,griper_senzor\WObj:=wobj0;
  227. WaitTime 8;
  228. povezivanje_sa_senzorom;
  229.  
  230. i := 12;
  231. ubaci_u_niz(i);
  232. ENDPROC
  233. PROC Path_13()
  234. MoveAbsJ t13,v1000,z100,griper_senzor\WObj:=wobj0;
  235. WaitTime 8;
  236. povezivanje_sa_senzorom;
  237.  
  238. i := 13;
  239. ubaci_u_niz(i);
  240. ENDPROC
  241.  
  242. PROC Path_14()
  243. MoveAbsJ t14,v1000,z100,griper_senzor\WObj:=wobj0;
  244. WaitTime 8;
  245. povezivanje_sa_senzorom;
  246.  
  247. i := 14;
  248. ubaci_u_niz(i);
  249. ENDPROC
  250. PROC Path_15()
  251. MoveAbsJ t15,v1000,z100,griper_senzor\WObj:=wobj0;
  252. WaitTime 8;
  253. povezivanje_sa_senzorom;
  254.  
  255. i := 15;
  256. ubaci_u_niz(i);
  257. ENDPROC
  258. PROC Path_16()
  259. MoveAbsJ t16,v1000,z100,griper_senzor\WObj:=wobj0;
  260. WaitTime 8;
  261. povezivanje_sa_senzorom;
  262.  
  263. i := 16;
  264. ubaci_u_niz(i);
  265. ENDPROC
  266. PROC Path_17()
  267. MoveAbsJ t17,v1000,z100,griper_senzor\WObj:=wobj0;
  268. WaitTime 8;
  269. povezivanje_sa_senzorom;
  270.  
  271. i := 17;
  272. ubaci_u_niz(i);
  273. ENDPROC
  274. PROC Path_18()
  275. MoveAbsJ t18,v1000,z100,griper_senzor\WObj:=wobj0;
  276. WaitTime 8;
  277. povezivanje_sa_senzorom;
  278.  
  279. i := 18;
  280. ubaci_u_niz(i);
  281. ENDPROC
  282. PROC Path_19()
  283. MoveAbsJ t19,v1000,z100,griper_senzor\WObj:=wobj0;
  284. WaitTime 8;
  285. povezivanje_sa_senzorom;
  286.  
  287. i := 19;
  288. ubaci_u_niz(i);
  289. ENDPROC
  290. PROC Path_20()
  291. MoveAbsJ t20,v1000,z100,griper_senzor\WObj:=wobj0;
  292. WaitTime 8;
  293. povezivanje_sa_senzorom;
  294.  
  295. i := 20;
  296. ubaci_u_niz(i);
  297. ENDPROC
  298. PROC Path_21()
  299. MoveAbsJ t21,v1000,z100,griper_senzor\WObj:=wobj0;
  300. WaitTime 8;
  301. povezivanje_sa_senzorom;
  302.  
  303. i := 21;
  304. ubaci_u_niz(i);
  305. ENDPROC
  306. PROC Path_22()
  307. MoveAbsJ t22,v1000,z100,griper_senzor\WObj:=wobj0;
  308. WaitTime 8;
  309. povezivanje_sa_senzorom;
  310.  
  311. i := 22;
  312. ubaci_u_niz(i);
  313. ENDPROC
  314. PROC Path_23()
  315. MoveAbsJ t23,v1000,z100,griper_senzor\WObj:=wobj0;
  316. WaitTime 8;
  317. povezivanje_sa_senzorom;
  318.  
  319. i := 23;
  320. ubaci_u_niz(i);
  321. ENDPROC
  322. PROC Path_24()
  323. MoveAbsJ t24,v1000,z100,griper_senzor\WObj:=wobj0;
  324. WaitTime 8;
  325. povezivanje_sa_senzorom;
  326.  
  327. i := 24;
  328. ubaci_u_niz(i);
  329. ENDPROC
  330.  
  331. ENDMODULE
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement