MODULE MainModule
CONST robtarget yuandian:=[[1271.38,-25.11,2023.30],[0.0381007,0.00383459,0.999265,-0.00185829],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
CONST robtarget qutuopanweizhiup:=[[2361.80,-595.80,2189.34],[0.0323203,0.00266198,0.999454,0.00625964],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget qutuopanweizhimiddle:=[[2361.88,-595.82,1593.39],[0.032351,0.00265054,0.999453,0.0062529],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget qutuopanweizhidow:=[[2362.01,-595.86,-163.83],[0.0324003,0.00264856,0.999452,0.0062541],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
CONST robtarget shuipingjizundian:=[[2335.47,-339.90,554.70],[0.0377045,0.00379575,0.99928,-0.00177707],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!-------------A
CONST robtarget Axian_quliao:=[[1113.02,1812.46,600.72],[0.0177786,-0.0179848,0.999605,0.0122313],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Axian_mafangweizhi:=[[-1047.73,1600.34,-15.02],[0.00829455,0.00552996,-0.999769,-0.0190338],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!-------------A
!-------------B
CONST robtarget Bxian_quliao:=[[1104.67,-1889.35,600.62],[0.0315602,0.019157,0.99918,-0.0166505],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Bxian_mafangweizhi:=[[-1018.34,-1574.04,-15.47],[3.96022E-05,0.00997105,0.999847,-0.0143956],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget safe_point;
!-------------B
!-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
!----------------------------------System Variables
!-------------A
VAR num A_x:=0;
VAR num A_y:=0;
VAR num A_z:=0;
VAR num A_reclaimer_offset_x:=0;
VAR num A_reclaimer_offset_y:=0;
VAR num A_reclaimer_pallet_dow:=0;
VAR bool A_reclaimer_pallet_dow_flage:=true;
!-------------A
!-------------B
VAR num B_x:=0;
VAR num B_y:=0;
VAR num B_z:=0;
VAR num B_reclaimer_offset_x:=0;
VAR num B_reclaimer_offset_y:=0;
VAR num B_reclaimer_pallet_dow:=0;
VAR bool B_reclaimer_pallet_dow_flage:=true;
!-------------B
VAR bool Aflage:=false;
VAR bool Bflage:=true;
VAR bool Amaterial:=false;
VAR bool Bmaterial:=false;
VAR num sucker_x:=550;
VAR num sucker_y:=400;
VAR num System_pallet_sizes_x:=1190;
VAR num System_pallet_sizes_y:=1190;
VAR bool go_home:=false;
VAR bool safe_point_flage:=false;
VAR num safe_point_dow:=0;
!----------------------------------System Variables
!-------------------------------------------------------------------------------------------------------------------------------------------------------------
!----------------------------------Socket Variables
!-------------A
VAR num Socket_A_box_num_x:=3;
VAR num Socket_A_box_num_y:=3;
VAR num Socket_A_box_num_z:=3;
!Socket_A_box_x<=>Width Socket_A_box_y<=>Long Socket_A_box_z<=>Hight
VAR num Socket_A_box_x:=400;
VAR num Socket_A_box_y:=400;
VAR num Socket_A_box_z:=400;
VAR num Socket_A_box_offset_x:=0;
VAR num Socket_A_box_offset_y:=0;
VAR num Socket_A_box_offset_z:=50;
VAR num A_box_wile:=0;
VAR num A_while:=2;
!-------------A
!-------------B
VAR num Socket_B_box_num_x:=3;
VAR num Socket_B_box_num_y:=3;
VAR num Socket_B_box_num_z:=3;
!Socket_B_box_x<=>Width Socket_B_box_y<=>Long Socket_B_box_z<=>Hight
VAR num Socket_B_box_x:=400;
VAR num Socket_B_box_y:=400;
VAR num Socket_B_box_z:=400;
VAR num Socket_B_box_offset_x:=0;
VAR num Socket_B_box_offset_y:=0;
VAR num Socket_B_box_offset_z:=50;
VAR num B_box_wile:=0;
VAR num B_while:=2;
!-------------B
PROC main()
AccSet 60,60;
! MoveJ Bxian_mafangweizhi,v500,z50,tool0;
! MoveJ Bxian_mafangwait,v500,z50,tool0;
! MoveJ Bxian_mafangweizhi,v500,z50,tool0;
go_home:=TRUE;
WHILE TRUE DO
A_box_wile:=0;
B_box_wile:=0;
IF di_A_while_num>1 THEN
A_while:=di_A_while_num;
SetGO do_A_while_num,(A_while);
ENDIF
IF di_B_while_num>1 THEN
B_while:=di_B_while_num;
SetGO do_B_while_num,(B_while);
ENDIF
IF di_go_home=1 THEN
go_home:=TRUE;
ENDIF
IF id_sys_reset=1 THEN
Set do_sys_reset;
Reset do_Astock_num_update;
Reset do_Aflage_update;
Reset do_Bstock_num_update;
Reset do_Bflage_update;
WaitTime 0.3;
Reset do_sys_reset;
ENDIF
IF go_home=TRUE THEN
gohome_point;
go_home:=FALSE;
ENDIF
IF di_Aflage=1 THEN
Aflage:=TRUE;
else
Aflage:=FALSE;
ENDIF
IF di_Bflage=1 THEN
Bflage:=TRUE;
else
Bflage:=FALSE;
ENDIF
!-------------A
WHILE A_box_wile<A_while DO
A_box_wile:=A_box_wile+1;
IF Aflage=TRUE THEN
Set do_A_cycle;
!IF di_Astock_num=0 THEN
! AND di_Apallet=0 and di_Abpallet=1
! Set do03_GripUp;
! Reset do04_GripDown;
! WaitTime 1;
! !A_reclaimer_pallet;
! Reset do03_GripUp;
! Set do04_GripDown;
! WaitTime 1;
IF di_Socket_A_box_x<>0 AND di_Socket_A_box_y<>0 AND di_Socket_A_box_z<>0 AND di_Socket_A_box_num_x<>0 AND di_Socket_A_box_num_y<>0 AND di_Socket_A_box_num_z<>0 THEN
IF di_Socket_A_box_x>300 and di_Socket_A_box_y>300 and di_Socket_A_box_z>300 and di_Socket_A_box_num_x>1 and di_Socket_A_box_num_y>1 and di_Socket_A_box_num_z>1 THEN
Socket_A_box_x:=di_Socket_A_box_x;
Socket_A_box_y:=di_Socket_A_box_y;
Socket_A_box_z:=di_Socket_A_box_z;
Socket_A_box_num_x:=di_Socket_A_box_num_x;
Socket_A_box_num_y:=di_Socket_A_box_num_y;
Socket_A_box_num_z:=di_Socket_A_box_num_z;
SetGO do_Socket_A_box_x,di_Socket_A_box_x;
SetGO do_Socket_A_box_y,di_Socket_A_box_y;
SetGO do_Socket_A_box_z,di_Socket_A_box_z;
SetGO do_Socket_A_box_num_x,di_Socket_A_box_num_x;
SetGO do_Socket_A_box_num_y,di_Socket_A_box_num_y;
SetGO do_Socket_A_box_num_z,di_Socket_A_box_num_z;
Amaterial:=TRUE;
ENDIF
ENDIF
Socket_A_box_offset_x:=Socket_A_box_x*((Socket_A_box_num_x-1)/2);
Socket_A_box_offset_y:=Socket_A_box_y*((Socket_A_box_num_y-1)/2);
A_reclaimer_offset_x:=(Socket_A_box_x-sucker_x)/2;
A_reclaimer_offset_y:=(Socket_A_box_y-sucker_y)/2;
IF di_Astock_num>=Socket_A_box_num_x*Socket_A_box_num_y*Socket_A_box_num_z THEN
Amaterial:=FALSE;
Aflage:=FALSE;
SetGO do_Astock_num,(0);
Set do_Astock_num_update;
Set do_Aflage_update;
WaitTime 0.3;
Reset do_Astock_num_update;
Reset do_Aflage_update;
ENDIF
!ENDIF
IF Amaterial=TRUE AND di_Apallet=1 and di_Amaterial=1 THEN
SetGO do_Astock_num,(di_Astock_num+1);
Set do_Astock_num_update;
WaitTime 0.3;
Reset do_Astock_num_update;
Axian_stock(di_Astock_num-1);
ENDIF
Reset do_A_cycle;
ENDIF
ENDWHILE
!-------------A
!-------------B
WHILE B_box_wile<B_while DO
B_box_wile:=B_box_wile+1;
IF Bflage=TRUE THEN
Set do_B_cycle;
!IF di_Bstock_num=0 THEN
!AND di_Bpallet=0 AND di_Abpallet=1
! Set do03_GripUp;
! Reset do04_GripDown;
! WaitTime 1;
! !B_reclaimer_pallet;
! Reset do03_GripUp;
! Set do04_GripDown;
! WaitTime 1;
IF di_Socket_B_box_x<>0 AND di_Socket_B_box_y<>0 AND di_Socket_B_box_z<>0 AND di_Socket_B_box_num_x<>0 AND di_Socket_B_box_num_y<>0 AND di_Socket_B_box_num_z<>0 THEN
IF di_Socket_B_box_x>300 AND di_Socket_B_box_y>300 AND di_Socket_B_box_z>300 AND di_Socket_B_box_num_x>1 AND di_Socket_B_box_num_y>1 AND di_Socket_B_box_num_z>1 THEN
Socket_B_box_x:=di_Socket_B_box_x;
Socket_B_box_y:=di_Socket_B_box_y;
Socket_B_box_z:=di_Socket_B_box_z;
Socket_B_box_num_x:=di_Socket_B_box_num_x;
Socket_B_box_num_y:=di_Socket_B_box_num_y;
Socket_B_box_num_z:=di_Socket_B_box_num_z;
SetGO do_Socket_B_box_x,di_Socket_B_box_x;
SetGO do_Socket_B_box_y,di_Socket_B_box_y;
SetGO do_Socket_B_box_z,di_Socket_B_box_z;
SetGO do_Socket_B_box_num_x,di_Socket_B_box_num_x;
SetGO do_Socket_B_box_num_y,di_Socket_B_box_num_y;
SetGO do_Socket_B_box_num_z,di_Socket_B_box_num_z;
Bmaterial:=TRUE;
ENDIF
ENDIF
Socket_B_box_offset_x:=Socket_B_box_x*((Socket_B_box_num_x-1)/2);
Socket_B_box_offset_y:=Socket_B_box_y*((Socket_B_box_num_y-1)/2);
B_reclaimer_offset_x:=(Socket_B_box_x-sucker_x)/2;
B_reclaimer_offset_y:=(sucker_y-Socket_B_box_y)/2;
IF di_Bstock_num>=Socket_B_box_num_x*Socket_B_box_num_y*Socket_B_box_num_z THEN
Bmaterial:=FALSE;
Bflage:=FALSE;
SetGO do_Bstock_num,(0);
Set do_Bstock_num_update;
Set do_Bflage_update;
WaitTime 0.3;
Reset do_Bstock_num_update;
Reset do_Bflage_update;
ENDIF
!ENDIF
IF Bmaterial=TRUE AND di_Bpallet=1 and di_Bmaterial=1 THEN
SetGO do_Bstock_num,(di_Bstock_num+1);
Set do_Bstock_num_update;
WaitTime 0.3;
Reset do_Bstock_num_update;
Bxian_stock(di_Bstock_num-1);
ENDIF
Reset do_B_cycle;
ENDIF
ENDWHILE
!-------------B
ENDWHILE
ENDPROC
PROC gohome_point()
safe_point:=CRobT();
safe_point_flage:=FALSE;
WHILE safe_point_flage=FALSE DO
safe_point_dow:=safe_point_dow+100;
MoveJ offs(safe_point,0,0,safe_point_dow),v200,fine,tool0;
safe_point:=CRobT();
IF safe_point.trans.z>2000 THEN
safe_point_flage:=TRUE;
safe_point_dow:=0;
ENDIF
ENDWHILE
MoveJ yuandian,v500,z30,tool0;
Reset do03_GripUp;
Set do04_GripDown;
Set do_go_home_flage;
WaitTime 0.3;
Reset do_go_home_flage;
Reset do_A_cycle;
Reset do_B_cycle;
go_home:=false;
ENDPROC
PROC Axian_stock(num stock_num)
A_x:=(stock_num MOD (Socket_A_box_num_x*Socket_A_box_num_y) DIV Socket_A_box_num_y)*Socket_A_box_x-Socket_A_box_offset_x;
A_y:=((stock_num MOD (Socket_A_box_num_x*Socket_A_box_num_y)) MOD Socket_A_box_num_y)*(-Socket_A_box_y)+Socket_A_box_offset_y;
A_z:=((stock_num div (Socket_A_box_num_x*Socket_A_box_num_y))+1)*Socket_A_box_z-40;
MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y,Socket_A_box_z+956),v1500,fine,tool0;
WaitDI di_Amaterial,1;
MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y,Socket_A_box_z),v800,fine,tool0;
Set do_rob_Vacuum_open;
WaitTime 0.5;
Reset do_rob_Vacuum_open;
MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y,Socket_A_box_z+550),v800,fine,tool0;
MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y+A_y-230,Socket_A_box_z+956),v800,fine,tool0;
MoveL Offs(Axian_mafangweizhi,A_x+30,A_y-60,A_z+Socket_A_box_offset_z),v800,fine,tool0;
MoveJ Offs(Axian_mafangweizhi,A_x,A_y,A_z),v200,fine,tool0;
WaitTime 0.5;
Set do_rob_Vacuum_close;
WaitTime 0.3;
Reset do_rob_Vacuum_close;
MoveJ Offs(Axian_mafangweizhi,A_x,A_y,A_z+Socket_A_box_offset_z),v200,fine,tool0;
MoveJ Offs(Axian_mafangweizhi,0,0,2300),v800,fine,tool0;
ENDPROC
PROC Bxian_stock(num stock_num)
B_x:=(stock_num MOD (Socket_B_box_num_x*Socket_B_box_num_y) DIV Socket_B_box_num_y)*Socket_B_box_x-Socket_B_box_offset_x;
B_y:=((stock_num MOD (Socket_B_box_num_x*Socket_B_box_num_y)) MOD Socket_B_box_num_y)*(Socket_B_box_y)-Socket_B_box_offset_y;
B_z:=((stock_num div (Socket_B_box_num_x*Socket_B_box_num_y))+1)*Socket_B_box_z-40;
MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y,Socket_B_box_z+956),v1500,fine,tool0;
WaitDI di_Bmaterial,1;
MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y,Socket_B_box_z-25),v800,fine,tool0;
Set do_rob_Vacuum_open;
WaitTime 0.5;
Reset do_rob_Vacuum_open;
MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y,Socket_B_box_z+550),v800,fine,tool0;
MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y+B_y+230,Socket_B_box_z+956),v800,fine,tool0;
MoveL Offs(Bxian_mafangweizhi,B_x+30,B_y+60,B_z+Socket_B_box_offset_z),v800,fine,tool0;
MoveJ Offs(Bxian_mafangweizhi,B_x,B_y,B_z),v200,fine,tool0;
WaitTime 0.5;
Set do_rob_Vacuum_close;
WaitTime 0.3;
Reset do_rob_Vacuum_close;
MoveJ Offs(Bxian_mafangweizhi,B_x,B_y,B_z+Socket_B_box_offset_z),v200,fine,tool0;
MoveJ Offs(Bxian_mafangweizhi,0,0,2300),v800,fine,tool0;
ENDPROC
PROC A_reclaimer_pallet()
MoveJ qutuopanweizhiup,v500,z50,tool0;
MoveJ qutuopanweizhimiddle,v200,z50,tool0;
A_reclaimer_pallet_dow_flage:=FALSE;
WHILE A_reclaimer_pallet_dow_flage=FALSE DO
A_reclaimer_pallet_dow:=A_reclaimer_pallet_dow-5;
MoveJ offs(qutuopanweizhimiddle,0,0,A_reclaimer_pallet_dow),v30,fine,tool0;
IF di01_PalCheck=1 THEN
WaitTime 0.5;
Set do6_PalclampInout;
Reset do5_PalclampOut;
WaitTime 3;
A_reclaimer_pallet_dow_flage:=TRUE;
A_reclaimer_pallet_dow:=0;
ENDIF
ENDWHILE
MoveJ qutuopanweizhiup,v500,z50,tool0;
MoveJ offs(Axian_mafangweizhi,0,0,2100),v800,fine,tool0;
MoveJ offs(Axian_mafangweizhi,0,0,-120),v500,fine,tool0;
Set do5_PalclampOut;
Reset do6_PalclampInout;
WaitTime 3;
MoveJ offs(Axian_mafangweizhi,0,0,2100),v500,fine,tool0;
ENDPROC
PROC B_reclaimer_pallet()
MoveJ qutuopanweizhiup,v200,z50,tool0;
MoveJ qutuopanweizhimiddle,v200,z50,tool0;
B_reclaimer_pallet_dow_flage:=FALSE;
WHILE B_reclaimer_pallet_dow_flage=FALSE DO
B_reclaimer_pallet_dow:=B_reclaimer_pallet_dow-5;
MoveJ offs(qutuopanweizhimiddle,0,0,B_reclaimer_pallet_dow),v30,fine,tool0;
IF di01_PalCheck=1 THEN
WaitTime 0.5;
Set do6_PalclampInout;
Reset do5_PalclampOut;
WaitTime 3;
B_reclaimer_pallet_dow_flage:=TRUE;
B_reclaimer_pallet_dow:=0;
ENDIF
ENDWHILE
MoveJ qutuopanweizhiup,v200,z50,tool0;
MoveJ offs(Bxian_mafangweizhi,0,0,2100),v200,fine,tool0;
MoveJ offs(Bxian_mafangweizhi,0,0,-120),v200,fine,tool0;
Set do5_PalclampOut;
Reset do6_PalclampInout;
WaitTime 3;
MoveJ offs(Bxian_mafangweizhi,0,0,2100),v200,fine,tool0;
ENDPROC
ENDMODULE