MODULE Module1
    CONST robtarget Target_30:=[[0,0,0],[1,0,0,0],[-2,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
    CONST robtarget Target_20:=[[0,0,0],[1,0,0,0],[0,-1,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PROC Path_10()
    MoveL Target_20,v1000,z100,tcp_super_hvataljke\WObj:=ulazniWob;
    MoveL Target_30,v1000,z100,tcp_super_hvataljke\WObj:=izlazniWob;
ENDPROC

PROC main() !metoda koja se poziva kada se pritisne dugme start

    !dimenzije ulaznog skladista
    VAR num a_u := 2;
    VAR num b_u := 3;
    VAR num c_u := 1;

    !dimenzije izlaznog skladista
    VAR num a_i := 2;
    VAR num b_i := 3;
    VAR num c_i := 1;

    !proverava da li obe dimenzija imaju isti broj elemenata, da bi mogao sve predmete rasporediti iz
    !jednog u drugo skladista
    IF (a_u*b_u*c_u)<>0 AND (a_i*b_i*c_i)<>0 AND((a_u*b_u*c_u)=(a_i*b_i*c_i)) then
    pomeraj  a_u,  b_u , c_u,  a_i,  b_i,  c_i,true; !prvo ih prenosimo iz ulaznog u izlazno skladiste
     WaitTime 5;
    pomeraj  a_i,  b_i , c_i,  a_u,  b_u,  c_u,false;!a zatim ih vracamo nazad u ulazno
    endif
ENDPROC


!metoda koja vrsi premestanje predmeta
PROC pomeraj( num a_u, num b_u ,num c_u, num a_i, num b_i, num c_i,bool status)
!da bi robot znao s kog na koji sto da pomera predmeta, postoji promenjiva status
!       status==true oznacava da robot ide s prvog stola na drugi
!       status==false znaci da robot ide s drugog stola na prvi(kada treba da vrati predmete nazad)
!       posto on u sustini ne zna koji je koji sto
    VAR num ukupno;
    VAR num i;
    VAR num x;
    VAR num y;
    VAR num z;
    VAR num k;
    VAR num i_i;
    i_i:=0;
    ukupno := (a_u*b_u*c_u)-1;  !racunamo broj predmeta u matrici
    FOR i FROM ukupno TO 0 DO   !krecemo se petljom od krajnjeg predmeta na vrhu gomile, do predmeta na dnu
           k:=i MOD (a_u*b_u);      !zbog toga sto matrica ima vise nivoa(Z osu) pri divisionu/deljenju za y kordinatu
                    !moze doci do buga, pa se uvodi k promenjiva da se sve svede na isti nivo
           x := 100 + 100 * (k MOD a_u);    !racunamo x kordinatu po modulu
           y := 100 + 100 * (k DIV a_u);    !racunamo y kordinatu pomocu deljenja
           z :=-30-20*(i DIV (a_u*b_u));    !i z kordinatu ili visinu na kojoj se predmet nalazi
           prvoskladiste x,y,z,status;      !pozivamo metodu koja ce reci roboti da ode na tu kordinatu i uzme predmet

           k:=i_i MOD (a_i*b_i);    !racunamo kordinate izlaznog skladista, tj gde bi trebao da ostavi predemt
           x := 100 + 100 * (k MOD a_i);
           y := 100 + 100 * (k DIV a_i);
           z :=-30-20*(i_i DIV (a_i*b_i));
            drugoskladiste x,y,z,status;    !pozivamo metodu koja ce reci robotu da odnese predmet
                        !i ostavi ga na drugom stolu
            i_i:=i_i+1; !u petlji imamo jos jedan brojac koji ce predmete na drugom stolu, gde ih robot postavlja
            !stavljati od dna do vrha
    ENDFOR
ENDPROC

PROC prvoskladiste(num x,num y,num z,bool status)
    IF status then  !ako ide iz ulaznog u izlazno skladiste da uzme predmet
      MoveJ Offs(Target_20, x, y, -80), v500, z0, tcp_super_hvataljke\WObj:=ulazniWob;
      MoveL Offs(Target_20, x, y, z), v100, z0, tcp_super_hvataljke\WObj:=ulazniWob;
      WaitRob \InPos;
      ! zatvori hvataljku
      SetDO signal_hvataljke, 1;
      WaitTime 3;
      MoveL Offs(Target_20, x, y, -80), v100, z0,  tcp_super_hvataljke\WObj:=ulazniWob;
    else    !ako vraca predmete iz izlaznog u ulazno skladiste, tj ide prvo u izlazno skladiste
      MoveJ Offs(Target_30, x, y, -80), v500, z0, tcp_super_hvataljke\WObj:=izlazniWob;
      MoveL Offs(Target_30, x, y, z), v100, z0, tcp_super_hvataljke\WObj:=izlazniWob;
      WaitRob \InPos;
      ! zatvori hvataljku
      SetDO signal_hvataljke, 1;
      WaitTime 3;
      MoveL Offs(Target_30, x, y, -80), v100, z0,  tcp_super_hvataljke\WObj:=izlazniWob;
    ENDIF
ENDPROC


PROC drugoskladiste(num x,num y,num z,bool status)
    IF status THEN !ako predmete ostavlja u izlaznom skladistu
            MoveJ Offs(Target_30, x, y, -80), v500, z0, tcp_super_hvataljke\WObj:=izlazniWob;
            MoveL Offs(Target_30, x, y, z), v100, z0, tcp_super_hvataljke\WObj:=izlazniWob;
            WaitRob \InPos;
            ! otvori hvataljku
            SetDO signal_hvataljke, 0;
            WaitTime 3;
            MoveL Offs(Target_30, x, y, -80), v100, z0,  tcp_super_hvataljke\WObj:=izlazniWob;
    ELSE !ako predmete ostavlja u ulaznom skladistu
            MoveJ Offs(Target_20, x, y, -80), v500, z0, tcp_super_hvataljke\WObj:=ulazniWob;
            MoveL Offs(Target_20, x, y, z), v100, z0, tcp_super_hvataljke\WObj:=ulazniWob;
            WaitRob \InPos;
            ! otvori hvataljku
            SetDO signal_hvataljke, 0;
            WaitTime 3;
            MoveL Offs(Target_20, x, y, -80), v100, z0,  tcp_super_hvataljke\WObj:=ulazniWob;
    endif
ENDPROC


ENDMODULE