Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- MODULE Module1
- VAR num masa := 0;
- VAR num i:=0;
- VAR socketdev client_socket;
- VAR rawbytes merenja;
- VAR num header := 0;
- VAR num Fx := 0;
- VAR num Fy := 0;
- VAR num Fz := 0;
- VAR num Tx := 0;
- VAR num Ty := 0;
- VAR num Tz := 0;
- VAR num biasFX := 0;
- VAR num biasFY := 0;
- VAR num biasFZ := 0;
- VAR num biasTX := 0;
- VAR num biasTY := 0;
- VAR num biasTZ := 0;
- VAR num sile{6,24};
- ! targeti za prolazenje kroz 24 pozicije za kalibraciju
- VAR byte komanda{1} := [1];
- CONST jointtarget t1:=[[0,0,0,0,0,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t2:=[[-90,0,0,90,-90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t3:=[[0,0,0,0,-90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t4:=[[0,0,0,0,90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t5:=[[0,0,0,-90,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t6:=[[90,0,0,0,0,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t7:=[[0,0,0,0,-90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t8:=[[0,0,0,0,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t9:=[[0,0,0,0,0,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t10:=[[-90,0,0,90,-90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t11:=[[0,0,0,0,-90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t12:=[[0,0,0,0,90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t13:=[[0,0,0,90,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t14:=[[90,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t15:=[[0,0,0,0,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t16:=[[0,0,0,0,90,270],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t17:=[[-90,0,0,90,90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t18:=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t19:=[[-90,0,0,90,90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t20:=[[0,0,0,0,0,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t21:=[[0,0,0,90,90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t22:=[[0,0,0,-90,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t23:=[[0,0,0,90,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- CONST jointtarget t24:=[[0,0,0,90,90,180],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
- PROC main()
- SocketCreate client_socket;
- SocketConnect client_socket, "192.168.0.6", 50005;
- Path_1;
- Path_9;
- Path_18;
- Path_20;
- Path_3;
- Path_7;
- Path_11;
- Path_15;
- Path_4;
- Path_12;
- Path_16;
- Path_8;
- Path_13;
- Path_5;
- Path_17;
- Path_19;
- Path_21;
- Path_22;
- Path_23;
- Path_24;
- Path_6;
- Path_14;
- Path_10;
- Path_2;
- ! racunanje bias vrednosti sila kao aritmeticke sredine svih merenja
- 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;
- 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;
- 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;
- 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;
- 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;
- 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;
- MoveAbsJ t18,v1000,z100,griper_senzor\WObj:=wobj0;
- WHILE (TRUE) DO
- povezivanje_sa_senzorom;
- ENDWHILE
- SocketClose client_socket;
- ENDPROC
- !procedura za primanje mernih podataka sa senzora preko plc-a
- PROC povezivanje_sa_senzorom()
- WHILE (header <> 4660) DO
- SocketSend client_socket \Data:= komanda;
- SocketReceive client_socket \RawData:=merenja;
- UnpackRawBytes merenja , 1 , header \IntX:=DINT;
- ! salje zahteva podatke sa plc-a, sve dok header ne bude 0x1234 (to znaci da je bafer dobar)
- ENDWHILE
- UnpackRawBytes merenja , 5 , Fx \IntX:=DINT;
- UnpackRawBytes merenja , 9 , Fy \IntX:=DINT;
- UnpackRawBytes merenja , 13, Fz \IntX:=DINT;
- UnpackRawBytes merenja , 17, Tx \IntX:=DINT;
- UnpackRawBytes merenja , 21, Ty \IntX:=DINT;
- UnpackRawBytes merenja , 25 ,Tz \IntX:=DINT;
- Fx := Fx*1.0/1000000 - biasFX ;
- Fy := Fy*1.0/1000000 - biasFY;
- Fz := Fz*1.0/1000000 - biasFZ;
- Tx := Tx*1.0/1000000 - biasTX;
- Ty := Ty*1.0/1000000 - biasTY;
- Tz := Tz*1.0/1000000 - biasTZ;
- header := 0;
- ENDPROC
- PROC ubaci_u_niz(num i)
- sile{1,i} := Fx;
- sile{2,i} := Fy;
- sile{3,i} := Fz;
- sile{4,i} := Tx;
- sile{5,i} := Ty;
- sile{6,i} := Tz;
- ENDPROC
- PROC Path_1()
- MoveAbsJ t1,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 1 ;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_2()
- MoveAbsJ t2,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 2;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_3()
- MoveAbsJ t3,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 3;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_4()
- MoveAbsJ t4,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 4;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_5()
- MoveAbsJ t5,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 5;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_6()
- MoveAbsJ t6,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 6;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_7()
- MoveAbsJ t7,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 7;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_8()
- MoveAbsJ t8,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 8;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_9()
- MoveAbsJ t9,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 9;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_10()
- MoveAbsJ t10,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 10;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_11()
- MoveAbsJ t11,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 11;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_12()
- MoveAbsJ t12,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 12;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_13()
- MoveAbsJ t13,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 13;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_14()
- MoveAbsJ t14,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 14;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_15()
- MoveAbsJ t15,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 15;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_16()
- MoveAbsJ t16,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 16;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_17()
- MoveAbsJ t17,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 17;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_18()
- MoveAbsJ t18,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 18;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_19()
- MoveAbsJ t19,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 19;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_20()
- MoveAbsJ t20,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 20;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_21()
- MoveAbsJ t21,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 21;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_22()
- MoveAbsJ t22,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 22;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_23()
- MoveAbsJ t23,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 23;
- ubaci_u_niz(i);
- ENDPROC
- PROC Path_24()
- MoveAbsJ t24,v1000,z100,griper_senzor\WObj:=wobj0;
- WaitTime 8;
- povezivanje_sa_senzorom;
- i := 24;
- ubaci_u_niz(i);
- ENDPROC
- ENDMODULE
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement