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

MODULE PAL_TOOL(SYSMODULE)
  !######################################################
  !# PalletWare module PAL_TOOL				#
  !# This module holds the control routines of the	#
  !# tool equipment. 					#
  !#							#
  !# Grip equipment: 10 vacuum zones, mechanical pallet	#
  !#		     gripper, search equipment.		#
  !######################################################
  CONST string pal_tool_rev:="$Revision: 1.4 $";
  !######################################################
  !#							#
  !# 		G  L O B A L   D A T A	  		#
  !#							#
  !######################################################
  ! Gripper type definition
  PERS string GripperType:="Vacuum Gripper";
  ! Height of the clearout for defined gripper
  PERS num GripHeight:=200;
  ! Gripper associated tool datas
  PERS tooldata tpointer:=[TRUE,[[0,0,360],[1,0,0,0]],[65,[120,0,350],[1,0,0,0],2,2,2]];
  PERS tooldata tprodfront:=[TRUE,[[0,600,145],[1,0,0,0]],[65,[120,0,350],[1,0,0,0],2,2,2]];
  PERS tooldata tprodcenter:=[TRUE,[[0,0,145],[1,0,0,0]],[65,[120,0,350],[1,0,0,0],2,2,2]];
  PERS tooldata tprodback:=[TRUE,[[0,-600,145],[1,0,0,0]],[65,[120,0,350],[1,0,0,0],2,2,2]];
  PERS tooldata tpallet:=[TRUE,[[0,0,180],[1,0,0,0]],[65,[120,0,350],[1,0,0,0],2,2,2]];
  PERS tooldata tsearch:=[TRUE,[[0,0,210],[1,0,0,0]],[65,[120,0,350],[1,0,0,0],2,2,2]];
  
  !######################################################
  !#							#
  !# 		L O C A L   D A T A	  		#
  !#							#
  !######################################################
  !tool orient correction for placing pallet
  LOCAL PERS num Pal_Orient:=0;
  ! Memory of the pallet gripper closing
  LOCAL PERS bool GripPal:=FALSE;
  !Pallet gripper number in tool array, PAL_CELL.MOD
  LOCAL VAR num PalletGripNbArr:=4;
  !######################################################
  !#							#
  !# 		P U B L I C   R O U T I N E S  		#
  !#							#
  !######################################################

  !------------------------------------------------------
  !	name:	SetGripperIdle
  !	descr:	Sets all the gripper I/O in idle state	
  !	arg:	bool
  !------------------------------------------------------
  PROC SetGripperIdle(
    bool ToFeeder)

    Reset doActSearch;
    Set doDeactSearch;
    Reset doCloseGripper;
    Set doOpenGripper;
    SetGO goVacuumZoneOn,0;
    Reset doVacuumOn;
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	CheckGripIdle
  !	descr:	Checks if the gripper I/O is in idle 
  !		state or not
  !	arg:	bool
  !------------------------------------------------------
  PROC CheckGripIdle(
    bool ToFeeder)
  
    VAR bool timeout:=FALSE;
    
    IF ToFeeder THEN
      WaitDI diSearchDeact,1\MaxTime:=2\TimeFlag:=timeout;
      IF timeout PalReportError ERR_DEACT_SRC_EQ \PAL_OPER_CHOICE;
      WaitDI diGripOpen,1\MaxTime:=2\TimeFlag:=timeout;
      IF timeout PalReportError ERR_GRIP_OPENING \PAL_OPER_CHOICE;
    ELSE 
      !WaitDI diSearchAct,1\MaxTime:=2\TimeFlag:=timeout;
      !IF timeout PalReportError ERR_ACT_SRC_EQ \PAL_OPER_CHOICE;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	CheckAirPressure
  !	descr:	Check if the air pressure is ok
  !		
  !	arg:	-
  !------------------------------------------------------
  FUNC bool CheckAirPressure()
    IF diAirPressOk <> 1 RETURN FALSE;
    RETURN TRUE;
  ERROR
    RAISE;
  ENDFUNC

  !------------------------------------------------------
  !	name:	ActGripSearch
  !	descr:	Configure the grip tool for search
  !		
  !	arg:	grip_zone of type num
  !------------------------------------------------------
  PROC ActGripSearch(
    num grip_zone)

    VAR bool timeout:=FALSE;

    SetDO doDeactSearch,0;
    SetDO doActSearch,1;
    WaitDI diSearchAct,1\MaxTime:=2\TimeFlag:=timeout;
    IF timeout PalReportError ERR_ACT_SRC_EQ \PAL_OPER_CHOICE;
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	DeactGripSearch
  !	descr:	Deactivate the grip tool for search
  !		
  !	arg:	grip_zone of type num
  !------------------------------------------------------
  PROC DeactGripSearch(
    num grip_zone)

    VAR bool timeout:=FALSE;

    Reset doActSearch;
    Set doDeactSearch;
    WaitDI diSearchDeact,1\MaxTime:=2\TimeFlag:=timeout;
    IF timeout PalReportError ERR_DEACT_SRC_EQ \PAL_OPER_CHOICE;
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	RunSearch
  !	descr:	Deactivate the grip tool for search
  !		
  !	arg:	CalcTarget of type INOUT robtarget
  !		Destination of type robtarget
  !		CalcSpeed of type speeddata
  !		SearchWobj of type INOUT wobjdata
  !------------------------------------------------------
  PROC RunSearch(
    INOUT robtarget CalcTarget,
    robtarget Destination,
    speeddata CalcSpeed,
    INOUT wobjdata SearchWobj)

    SearchL\Stop,diSearchHit\Flanks,CalcTarget,Destination,CalcSpeed,tsearch\WObj:=SearchWobj;
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	act_grip_zone
  !	descr:	Activate grip zone according to grip_zone
  !		data. If grip zone equals PalletGripNbArr, 
  !		ie the pallet gripper, the routine
  !		invokes pick_pallet, otherwise it invokes
  !		pick_boxes.
  !		
  !	arg:	grip_zone of type num
  !------------------------------------------------------
  PROC act_grip_zone(
    num grip_zone)

    IF GetCurToolNb(GetCurContext())=PalletGripNbArr THEN
      pick_pallet;
    ELSE
      pick_boxes grip_zone;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	deact_grip_zone
  !	descr:	Deactivate grip zone according to grip_zone
  !		data. If grip zone equals PalletGripNbArr, 
  !		ie the pallet gripper, the routine
  !		invokes release_pallet, otherwise it invokes
  !		release_boxes.
  !		
  !	arg:	grip_zone of type num
  !------------------------------------------------------
  PROC deact_grip_zone(
    num grip_zone)

    IF GetCurToolNb(GetCurContext())=PalletGripNbArr THEN
      release_pallet;
    ELSE
      release_boxes grip_zone;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	check_grip_zone
  !	descr:	Checks the status on grip zone.
  !		Report error if evaluation differs from
  !		expected result.
  !		
  !	arg:	grip_zone of type num
  !		activated of type switch
  !		deactivated of type switch
  !------------------------------------------------------
  PROC check_grip_zone(
    num grip_zone
    \switch activated
    |switch deactivated)

    VAR bool timeout:=FALSE;

    IF Present(activated) THEN
      IF GetCurToolNb(GetCurContext())=PalletGripNbArr THEN
        !gripper closed
        WaitDI diGripClosed,1\MaxTime:=1\TimeFlag:=timeout;
        IF timeout PalReportError ERR_GRIP_CLOSING \PAL_OPER_CHOICE;
      ELSE
        !vaccum on
      ENDIF
    ELSE
      IF GetCurToolNb(GetCurContext())=PalletGripNbArr THEN
        !gripper opened
        WaitDI diGripOpen,1\MaxTime:=1\TimeFlag:=timeout;
        IF timeout PalReportError ERR_GRIP_OPENING \PAL_OPER_CHOICE;
      ELSE
        !vaccum off
      ENDIF
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	MoveToWorkPos
  !	descr:	Handles the robot motion from the
  !		approach position to the grip/release
  !		position. If the product is the pallet
  !		it performs special motion procedures.	
  !	arg:	optional argument pick of type switch
  !		context of type num
  !		product name of type string
  !		product index of type num
  !		position of type robtarget
  !		delta x,y,z of type num	
  !------------------------------------------------------
  PROC MoveToWorkPos(
    \switch pick,
    num context,
    string prodName,
    num prodIndex,
    robtarget position,
    num delta_x,
    num delta_y,
    num delta_z)

    VAR num CheckPos:=0;
    VAR num i:=0;
    VAR robtarget prePos;
    VAR robtarget searchTarget;
    VAR robtarget place_pallet;
    VAR speeddata CalcSpeed:=[1000,250,5000,1000];
    VAR num ztool:=0;
    VAR bool timeout:=FALSE;

    IF GetCurLayer(context)=1 THEN
      CalcSpeed.v_tcp:=search_speed;
      IF GetPalSizeY(context)<850 THEN
        ztool:=PlacePalHeight{1};
      ELSEIF GetPalSizeY(context)<1050 THEN
        ztool:=PlacePalHeight{2};
      ELSE
        ztool:=PlacePalHeight{3};
      ENDIF
      prePos:=Offs(position,0,100,30+ztool);
      MoveL prePos,v1000,fine,cur_tool\WObj:=cur_wobj;
      searchTarget:=Offs(position,0,-25,30+ztool);
      MoveL searchTarget,v500,fine,cur_tool\WObj:=cur_wobj;
      WaitUntil PalletPlace(GetDefPalStn(context));
      !move to placing position
      place_pallet:=Offs(searchTarget,0,0,-30);
      MoveL RelTool(place_pallet,0,0,0\Rz:=Pal_Orient),CalcSpeed,fine,cur_tool\WObj:=cur_wobj;
      deact_grip_zone 1;
      PalRequestBoxes context,1;
      check_grip_zone GetCurGripzone(context)\deactivated;
      MoveL Offs(place_pallet,0,0,delta_z),cur_speed,fine,cur_tool\WObj:=cur_wobj;
      SetGripperIdle TRUE;
    ELSE
      IF Present(pick) THEN
        !move to pre-picking position
        prePos:=Offs(position,delta_x,delta_y,delta_z);
        MoveL prePos,cur_speed,cur_zone,cur_tool\WObj:=cur_wobj;
        !move to picking position
        cur_speed.v_tcp:=article_medsp{prodIndex};
        MoveL position,cur_speed,fine,cur_tool\WObj:=cur_wobj;
        act_grip_zone GetCurGripzone(context);
        check_grip_zone GetCurGripzone(context)\activated;
      ELSE
        !move to pre-placing position
        prePos:=Offs(position,delta_x,delta_y,delta_z);
        MoveL prePos,cur_speed,cur_zone,cur_tool\WObj:=cur_wobj;
        !move to placing position
        cur_speed.v_tcp:=article_lowsp{prodIndex};
        MoveL position,cur_speed,fine,cur_tool\WObj:=cur_wobj;
        deact_grip_zone GetCurGripzone(context);
        check_grip_zone GetCurGripzone(context)\deactivated;
      ENDIF
    ENDIF
  ERROR
    RAISE;
  ENDPROC
  
  !######################################################
  !#							#
  !# 		L O C A L   R O U T I N E S  		#
  !#							#
  !######################################################
  
  LOCAL PROC pick_pallet()
    Reset doOpenGripper;
    Set doCloseGripper;
  ERROR
    RAISE;
  ENDPROC

  LOCAL PROC release_pallet()
    Reset doCloseGripper;
    Set doOpenGripper;
  ERROR
    RAISE;
  ENDPROC

  LOCAL PROC pick_boxes(
    num grip_zone)

    Set doVacuumOn;
    SetGO goVacuumZoneOn,grip_zone;
  ERROR
    RAISE;
  ENDPROC
  
  LOCAL PROC release_boxes(
    num grip_zone)

    SetGO goVacuumZoneOn,grip_zone;
  ERROR
    RAISE;
  ENDPROC

ENDMODULE
