%%%
  VERSION:1
  LANGUAGE:ENGLISH
%%%

MODULE PAL_USRR(SYSMODULE)
  !######################################################
  !# PalletWare module PAL_USRR				#
  !# This module holds the station user routines and 	#
  !# stack search routine. 				#
  !#							#
  !# 							#
  !######################################################
  CONST string pal_usrr_rev:="$Revision: 1.4 $";
  CONST robtarget statoutrt:=[[676.94,382.07,334.01],[2E-006,0.000235,-1,1.3E-005],[0,0,0,0],[9E+009,9E+009,9E+009,9E+009,9E+009,9E+009]];
  !  Station user routines
  PROC rfeeder_in(
    num context)

    CheckGripIdle TRUE;
    IF SingleCycle THEN
      !Possibility to abort if the feeder is not ready
      WHILE PalFeederReady(GetDefFeeder(context))=FALSE DO
        IF DOutput(doAttend)=1 PalReportError ERR_FEED_ABORT \PAL_OPER_CHOICE;
        WaitTime 0.2;
      ENDWHILE
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  PROC rfeeder_out(
    num context)

    IF NOT FeederEmpty{context} THEN
      PalRequestBoxes\pick,context,1;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  PROC rpstack_in(
    num context)

    CheckGripIdle FALSE;
    ResetAllStacks;
  ERROR
    RAISE;
  ENDPROC

  PROC rpstack_out(
    num context)
  !
  ERROR
    RAISE;
  ENDPROC

  PROC rpallet_in(
    num context)
    !
  ERROR
    RAISE;
  ENDPROC

  PROC rpallet_out(
    num context)
    
    SetGripperIdle TRUE;
  ERROR
    RAISE;
  ENDPROC

  !  Cycle user routines
  PROC rpal_start(
    num context,
    num arg1)
  
  !
  
  ERROR
    RAISE;
  ENDPROC

  PROC rpal_end(
    num context,
    num arg1)

    VAR bool tout;

    PalStartOutfeed GetDefPalStn(context);
    IF StopAtPalEnd{context} THEN
      CycleNum{context}:=0;
      PalcycleDefault context;
      StopAtPalEnd{context}:=FALSE;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  PROC rlayer_start(
    num context,
    num arg1)

    IF NextLayerType(context)=STACK_LAYER THEN
      reinit_search:=TRUE;
      ResetAllStacks;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  PROC rlayer_end(
    num context,
    num arg1)

    IF CycNbLay{context}=GetCurProdLayer(context) THEN
      Terminate context;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  PROC rbef_pic_plac(
    num context,
    num arg1)
    
    VAR bool Flag1;

    SetGripperIdle NextLayerType(context)=FEEDER_LAYER;
  ERROR
    RAISE;
  ENDPROC

  PROC rbetw_pic_plac(
    num context,
    num arg1)

    !If handling pallet, wait for station ready
    IF GetCurLayer(context)=1 THEN
      WaitUntil PalletApproach(GetDefPalStn(context));
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  PROC rafter_pic_plac(
    num context,
    num arg1)

    IF FeederEmpty{context} THEN
      CycleNum{context}:=0;
      FeederEmpty{context}:=FALSE;
      Evacuate context;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  !  Stack search user routines
  PROC rstack_search(
    num context,
    num ProductIndex,
    num StackIndex)

    VAR robtarget CalcTarget;
    VAR speeddata CalcSpeed:=[1000,250,5000,1000];
    VAR num ztool;

    IF GetPalSizeY(context)<850 THEN
      ztool:=PickPalHeight{1};
    ELSEIF GetPalSizeY(context)<1050 THEN
      ztool:=PickPalHeight{2};
    ELSE
      ztool:=PickPalHeight{3};
    ENDIF
    IF GetCurHeight(StackIndex)<(station_minh{StackIndex}+article_h{ProductIndex}) THEN
      SetCurHeight StackIndex,station_maxh{StackIndex};
      CalcTarget:=Offs(computed_pos,0,0,GetCurHeight(StackIndex));
      MoveL CalcTarget,cur_speed,fine,cur_tool\WObj:=cur_wobj;
      DeactGripSearch GetCurGripzone(context);
      !Stack empty
      PalReportError ERR_STACK_EMPTY \PAL_WAIT_ACK;
      ActGripSearch GetCurGripzone(context);
    ENDIF
    IF GetCurHeight(StackIndex)<(station_minh{StackIndex}+max(100,article_h{ProductIndex}*3)) THEN
      !Stack low level
      PalReportWarning WRN_STACK_LO_LEV, StackIndex;
    ENDIF
    tsearch.tframe.trans.x:=cur_tool.tframe.trans.x;
    tsearch.tframe.trans.y:=cur_tool.tframe.trans.y;
    IF station_appz{StackIndex}>GetCurHeight(StackIndex) THEN
      MoveL Offs(computed_pos,0,0,station_appz{StackIndex}),cur_speed,z20,cur_tool\WObj:=cur_wobj;
    ENDIF
    MoveL Offs(computed_pos,0,0,GetCurHeight(StackIndex)),cur_speed,fine,tsearch\WObj:=cur_wobj;
    reinit_search:=FALSE;
    ActGripSearch (GetCurGripzone(context));
    deact_grip_zone GetCurGripzone(context);
    check_grip_zone GetCurGripzone(context)\deactivated;
    CalcSpeed.v_tcp:=search_speed;
    RunSearch CalcTarget,Offs(computed_pos,0,0,station_minh{StackIndex}),CalcSpeed,cur_wobj;
    SetCurHeight StackIndex,CalcTarget.trans.z+50;
    DeactGripSearch GetCurGripzone(context);
    MoveL Offs(CalcTarget,0,0,ztool),CalcSpeed,fine,cur_tool\WObj:=cur_wobj;
    act_grip_zone GetCurGripzone(context);
    PalletSetLoad context;
    check_grip_zone GetCurGripzone(context)\activated;
    CalcSpeed.v_tcp:=article_lowsp{ProductIndex};
    MoveL\Conc,Offs(CalcTarget,0,0,article_pickh{ProductIndex}),CalcSpeed,z10,cur_tool\WObj:=cur_wobj;
    
    CalcSpeed.v_tcp:=article_highsp{ProductIndex};
    IF station_appz{StackIndex}>GetCurHeight(StackIndex) THEN
      MoveL Offs(computed_pos,0,0,station_appz{StackIndex}),CalcSpeed,z20,cur_tool\WObj:=cur_wobj;
    ENDIF
  ERROR
    IF ERRNO=ERR_WHLSEARCH THEN
      SetCurHeight StackIndex, station_maxh{StackIndex};
      CalcTarget:=Offs(computed_pos,0,0,GetCurHeight(StackIndex));
      MoveL CalcTarget,cur_speed,fine,cur_tool\WObj:=cur_wobj;
      DeactGripSearch GetCurGripzone(context);
      PalReportError ERR_NO_SRC_HIT\PAL_WAIT_ACK;
      ActGripSearch GetCurGripzone(context);
      RETRY;
    ELSE
      RAISE;
    ENDIF
  ENDPROC
  
  
  
ENDMODULE
