3 분 소요

Hits

인터럽트

20250124_01

MODULE MainModule
	CONST robtarget pHome:=[[329.55,-26.08,334.47],[0.000108947,-0.556346,-0.830951,-8.14346E-05],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget pHome2:=[[491.93,-26.07,230.36],[0.000135218,-0.556332,-0.83096,-5.93678E-05],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget p10:=[[-32,-442,7],[1.84964E-05,-0.650831,-0.759223,-2.51296E-05],[-2,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget p20:=[[476.85,-432.45,413.36],[4.38213E-05,-0.556315,-0.830971,-8.55472E-05],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget p30:=[[615,-459.90,332.39],[2.28305E-06,-0.326002,-0.945369,-9.10512E-07],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget con_end:=[[616,-16,130],[7.02875E-06,0.103828,-0.994595,2.62461E-08],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget metal_box:=[[394.80,326.41,102.30],[2.7142E-05,0.103895,-0.994588,-2.8837E-05],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget b_p_box:=[[394.84,426.36,102.29],[7.57723E-06,0.103902,-0.994588,-4.6509E-05],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget w_p_box:=[[394.86,542.55,102.27],[5.74707E-06,-0.10393,0.994585,6.29787E-05],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget store1:=[[-37,604.81,257.58],[0.42092,-0.561505,0.571821,0.424923],[1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

	VAR num x_pos :=0;
	VAR num y_pos :=0;
	VAR num tw_pos :=0;
	VAR num ct_st :=0;
	
	VAR intnum interrupt_sign;
	
	PROC main()
		CONNECT interrupt_sign WITH E_STOP;
		ISignalDI di09_interrupt, high, interrupt_sign;
		IWatch interrupt_sign;
		
		AccSet 1, 1;
		MoveJ pHome, v200, fine, tool2_ej;

		WHILE TRUE DO

			IF di02_ma_none = 1 AND di07_running=0 THEN
				pick_plate;
				PulseDO\PLength:=0.2,do02_plcstart;
			ENDIF
			
			IF di06_arrive = 1 THEN
				con_end_proc;
				IF di03_metal = 1 THEN
					PulseDO\PLength:=0.2,do03_reset_jedg;
					metal_proc;
				ELSEIF di04_plastic_black = 1 THEN
					PulseDO\PLength:=0.2,do03_reset_jedg;
					b_p_proc;
				ELSEIF di05_plastic_white = 1 THEN
					PulseDO\PLength:=0.2,do03_reset_jedg;
					w_p_proc;
				ENDIF
				MoveJ pHome2, v200, fine, tool2_ej;
			ENDIF
			
		ENDWHILE
	ENDPROC
	
	PROC metal_proc()
		MoveJ Offs(metal_box,0,0,60), v200, z30, tool2_ej;
		MoveL metal_box, v30, fine, tool2_ej;
		grip_off;
		MoveL metal_box, v100, z30, tool2_ej;
	ENDPROC
	
	PROC b_p_proc()
		MoveJ Offs(b_p_box,0,0,60), v200, z30, tool2_ej;
		MoveL b_p_box, v30, fine, tool2_ej;
		grip_off;
		MoveL b_p_box, v100, z30, tool2_ej;
	ENDPROC
	
	PROC w_p_proc()

		MoveJ RelTool(store1,-20,0,-200), v200, z30, tool2_ej;
		
		pos7:

		IF di16_st07 = 0 THEN
			ct_st := 0;
		ELSEIF di17_st08 = 0 THEN
			ct_st := -50;
		ELSEIF di18_st09 = 0 THEN
			ct_st := -100;
		ENDIF

		MoveJ RelTool(store1,-20,ct_st,-200), v200, z30, tool2_ej;
		MoveL RelTool(store1,-20,ct_st,-50), v100, fine, tool2_ej;
		MoveL RelTool(store1,-20,ct_st,0), v20, fine, tool2_ej;
		MoveL RelTool(store1,0,ct_st,0), v20, fine, tool2_ej;
		grip_off;
		MoveL RelTool(store1,0,ct_st,-200), v200, fine, tool2_ej;

		IF di19_storeA = 1 THEN
			PulseDO\PLength:=0.2,do04_buzzer;
			WaitDI di08_restart,1;
			GOTO pos7;
		ENDIF
		

		!MoveJ Offs(w_p_box,0,0,60), v200, z30, tool2_ej;
		!MoveL w_p_box, v30, fine, tool2_ej;
		!grip_off;
		!MoveL w_p_box, v100, z30, tool2_ej;
	ENDPROC	
	
	PROC con_end_proc()
		MoveJ Offs(con_end,0,0,60), v200, z30, tool2_ej;
		MoveL con_end, v30, fine, tool2_ej;
		grip_on;
		MoveL Offs(con_end,0,0,60), v100, z30, tool2_ej;
	ENDPROC	
	
	PROC grip_on()
		PulseDO\PLength:=0.2,do00_gripon;			
		WaitDI di00_grip_on_sen,1;
	ENDPROC

	PROC grip_off()
		PulseDO\PLength:=0.2,do01_gripoff;
		WaitDI di01_grip_off_sen,1;
	ENDPROC
	
	PROC rout1()
		MoveJ P20, v200, z50, tool2_ej;
		MoveJ Offs(p30,0,0,50),v200,z20,tool2_ej;
		MoveL p30, v30, fine, tool2_ej;
		grip_off;
		MoveL Offs(p30,0,0,50), v100, z20, tool2_ej;
		MoveJ P20, v200, z50, tool2_ej;
	ENDPROC
	
	PROC pick_plate()
		x_pos := 0;
		y_pos := 0;
		tw_pos := 0;
		FOR ct2 FROM 1 TO 2 DO
			FOR ct1 FROM 1 TO 3 DO
				MoveJ Offs(p10,x_pos,y_pos+tw_pos,100),v200,z30,tool2_ej;
				MoveL Offs(p10,x_pos,y_pos+tw_pos,0), v30, fine, tool2_ej;
				grip_on;
				MoveL Offs(p10,x_pos,y_pos+tw_pos,100), v100, z30, tool2_ej;
				rout1;
				
				IF di02_ma_none = 1 THEN
					PulseDO\PLength:=0.2,do04_buzzer;
					WaitDI di08_restart,1;
					MoveL pHome, v200, fine, tool2_ej;
					RETURN;
				ENDIF
				
				x_pos := x_pos+110;
				tw_pos := tw_pos+2;
			ENDFOR
			x_pos := 0;
			tw_pos := 0;
			y_pos := y_pos-110;
		ENDFOR

	ENDPROC
	
	TRAP E_STOP
		VAR robtarget e_stop_pos;
		StopMove;
		StorePath;
		e_stop_pos := CRobT();
		
		MoveJ pHome, v200, z50, tool2_ej;
		
		WaitDi di09_interrupt,0;
		MoveJ e_stop_pos,v200, fine, tool2_ej;
		RestoPath;
		StartMove;
	ENDTRAP
	
	
ENDMODULE

소켓 통신

20250207_01

MODULE Mainmodule 
	CONST robtarget p10:=[[345.31,-150.09,359.14],[7.68532E-05,0.445296,-0.895384,5.27011E-06],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget p20:=[[345.27,16.62,359.12],[0.000103355,0.445286,-0.895388,-6.39308E-06],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
	CONST robtarget p30:=[[345.28,219.80,359.11],[6.67339E-05,0.445286,-0.895389,-1.18733E-05],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
   
	VAR socketdev server_socket;	!???? ?? ????
	VAR socketdev client_socket;	!????? ?? ????
	VAR string received_string;		!?? ?? ?? ????
	VAR bool keep_listening := TRUE;	!???? ??
	VAR socketstatus status_soc;		!?? ?? ?? ??
	VAR string client_ip;			!????? ip?? ??
	VAR num len;					!??? ?? ?? ??
   
    TASK PERS tooldata tool1_lee:=[TRUE,[[0,0,108],[1,0,0,0]],[0.2,[0,0,60],[1,0,0,0],0,0,0]];
     
    PROC main()
        AccSet 1,1;
		
		SocketCreate server_socket;		!?? ?? ??
		SocketBind server_socket, "10.10.32.141", 5000;	!?? ip ???? ???
		SocketListen server_socket;		!??? ?? ??
		SocketAccept server_socket, client_socket\ClientAddress:=client_ip;	!??????? ?? ?? ??

		WHILE keep_listening DO	!??? ???? ??
    		! Waiting for a connection request
	   		status_soc := SocketGetStatus(server_socket);	!?? ?? ?? ??
    		TPWrite "status_soc - " \num := status_soc;		
    		TPWrite client_ip;								!????? ip??
    		
    		IF(status_soc = SOCKET_CLOSED)THEN				!??? ?? ????, ????? ??
    			SocketAccept server_socket, client_socket;
    		ENDIF

    		! Communication
    		SocketReceive client_socket \Str:=received_string;	!?? ??? ??
    		TPWrite "Client wrote - " + received_string;
			
			len := StrLen(received_string);						!?? ??? ????
			
			received_string := StrPart(received_string,1,3);	!?? ??? 1~3??? ??
			
			IF received_string="aaa" THEN
				MoveJ p10, v200, fine, tool1_lee;
			ENDIF
			
			IF received_string="bbb" THEN
				MoveJ p20, v200, fine, tool1_lee;
			ENDIF
			
			IF received_string="ccc" THEN
				MoveJ p30, v200, fine, tool1_lee;
			ENDIF
			
			received_string := "";
			SocketSend client_socket \Str:="Message acknowledged"+client_ip;			
			
		ENDWHILE
		
		ERROR
		IF ERRNO=ERR_SOCK_TIMEOUT THEN
			TPWrite "SOCK Time out Retry";
			RETRY;
		ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
			RETURN;
		ELSE
			! No error recovery handling
		ENDIF
		
	 ENDPROC      
	 
ENDMODULE

태그:

카테고리:

업데이트:

Comments

Related Posts