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

MODULE PAL_USRT(SYSMODULE)
  !######################################################
  !#							#
  !# 		P U B L I C   R O U T I N E S  		#
  !#							#
  !######################################################
  CONST string pal_usrt_rev:="$Revision: 1.4 $";
  !
  !------------------------------------------------------
  !	name:	tcnfstack1
  !	descr:	Routine to reach configuration position
  !		
  !	arg:	none
  !------------------------------------------------------
  PROC tcnfstack1()
    MoveJ rstack1,v100,fine,tpointer\WObj:=wstack1;
  ENDPROC

  PROC tcnfstack2()
    MoveJ rstack2,v100,fine,tpointer\WObj:=wstack2;
  ENDPROC

  PROC tcnfstack3()
    MoveJ rstack3,v100,fine,tpointer\WObj:=wstack3;
  ENDPROC

  PROC tcnfstack4()
    MoveJ rstack4,v100,fine,tpointer\WObj:=wstack4;
  ENDPROC

  PROC tcnfinfeeder1()
    MoveJ rinfeeder1,v100,fine,tpointer\WObj:=winfeeder1;
  ENDPROC

  PROC tcnfpalstat1()
    MoveJ rpalstat1,v100,fine,tpointer\WObj:=wpalstat1;
  ENDPROC

  PROC tcnfinfeeder2()
    MoveJ rinfeeder2,v100,fine,tpointer\WObj:=winfeeder2;
  ENDPROC

  PROC tcnfpalstat2()
    MoveJ rpalstat2,v100,fine,tpointer\WObj:=wpalstat2;
  ENDPROC

  PROC tcnfinfeeder3()
    MoveJ rinfeeder3,v100,fine,tpointer\WObj:=winfeeder3;
  ENDPROC

  PROC tcnfpalstat3()
    MoveJ rpalstat3,v100,fine,tpointer\WObj:=wpalstat3;
  ENDPROC

  PROC tcnfinfeeder4()
    MoveJ rinfeeder4,v100,fine,tpointer\WObj:=winfeeder4;
  ENDPROC

  PROC tcnfpalstat4()
    MoveJ rpalstat4,v100,fine,tpointer\WObj:=wpalstat4;
  ENDPROC

  PROC tcnfinfeeder5()
    MoveJ rinfeeder5,v100,fine,tpointer\WObj:=winfeeder5;
  ENDPROC

  PROC tcnfpalstat5()
    MoveJ rpalstat5,v100,fine,tpointer\WObj:=wpalstat5;
  ENDPROC

  !------------------------------------------------------
  !	name:	palcycle_confirm
  !	descr:	Routine to confirm that user tries to
  !		start a new pallet cycle while another
  !		is running.
  !		
  !	arg:	cycle of type string (new cycle)
  !		cycle_memo of type string (running cycle)
  !------------------------------------------------------
  FUNC bool palcycle_confirm(
    string cycle,
    string cycle_memo)

    !Routine not used in standard package
    RETURN FALSE;
  ENDFUNC

  !------------------------------------------------------
  !	name:	clearout
  !	descr:	Moves the tool to a safety height
  !		
  !	arg:	height of type num
  !------------------------------------------------------
  PROC clearout(
    num height)

    VAR robtarget saftarget:=[[1051.02,1502.83,450.95],[0,0.001713,0.999999,0],[0,0,0,0],[9E+009,9E+009,9E+009,9E+009,9E+009,9E+009]];
    VAR num radius;
    VAR num coeff;

    saftarget:=CRobT(\Tool:=tpallet\WObj:=wobj0);
    radius:=Sqrt((saftarget.trans.x*saftarget.trans.x)+(saftarget.trans.y*saftarget.trans.y));
    saftarget.trans.z:=height;
    IF radius>2400 THEN
      coeff:=Sqrt((2400*2400)/(radius*radius));
      saftarget.trans.x:=saftarget.trans.x*coeff;
      saftarget.trans.y:=saftarget.trans.y*coeff;
    ELSEIF radius<1300 THEN
      coeff:=Sqrt((1300*1300)/(radius*radius));
      saftarget.trans.x:=saftarget.trans.x*coeff;
      saftarget.trans.y:=saftarget.trans.y*coeff;
    ENDIF
    MoveL saftarget,v500,fine,tpallet\WObj:=wobj0;
  ERROR
    RAISE;
  ENDPROC

  !------------------------------------------------------
  !	name:	Station_Empty
  !	descr:	Checks that the pallet station is really
  !		evacuated.
  !		(Controlled by boolean SECURE_FIRST)
  !		
  !	arg:	station of type num
  !------------------------------------------------------
  FUNC bool Station_Empty(
    num station)

    !Routine not used in standard package
    RETURN FALSE;
  ENDFUNC
  
  !------------------------------------------------------
  !	name:	CustomToolData
  !	descr:	User routine where possible to customize
  !		tooldata.
  !	arg:	context of type num
  !		name of type string
  !		tool of type tooldata
  !------------------------------------------------------
  PROC CustomToolData(
    num context,
    string name,
    INOUT tooldata tool)

    ! if necessary, customize tool data here
  ENDPROC

  !------------------------------------------------------
  !	name:	CustomWobjData
  !	descr:	User routine where possible to customize
  !		wobjdata.
  !	arg:	context of type num
  !		name of type string
  !		wobj of type wobjdata
  !------------------------------------------------------
  PROC CustomWobjData(
    num context,
    string name,
    INOUT wobjdata wobj)

    ! if necessary, customize wobj data here
  ENDPROC

  !------------------------------------------------------
  !	name:	CustomConfData
  !	descr:	User routine where possible to customize
  !		confdata.
  !	arg:	context of type num
  !		name of type string
  !		configdata of type confdata
  !------------------------------------------------------
  PROC CustomConfData(
    num context,
    string name,
    INOUT confdata config)

    ! if necessary, customize config data here
  ENDPROC

  !------------------------------------------------------
  !	name:	CustomExtAxData
  !	descr:	User routine where possible to customize
  !		ext axis data.
  !	arg:	context of type num
  !		name of type string
  !		external axis data of type extaxes
  !------------------------------------------------------
  PROC CustomExtAxData(
    num context,
    string name,
    INOUT extjoint extaxes)

    ! if necessary, customize external joint data here
  ENDPROC
  
  !------------------------------------------------------
  !	name:	CustomEveryLoop
  !	descr:	User routine that executes every loop,
  !		possibility to add specific code.
  !		Placing code in this routine may affect the
  !		execution speed.
  !	arg:	none
  !------------------------------------------------------
  PROC CustomEveryLoop()
    !
    WHILE CheckAirPressure() =FALSE DO
      PalReportError ERR_AIRSUPPLY \PAL_WAIT_ACK;
    ENDWHILE
  ERROR
    RAISE;
  ENDPROC
  
  !------------------------------------------------------
  !	name:	CustomInit
  !	descr:	User routine that executes at 
  !		initialization.
  !		
  !	arg:	none
  !------------------------------------------------------
  PROC CustomInit()
    Reset doOkToSendPlcMsg;
    Reset doOkToReadPlcMsg;
    Reset doOkToSendMmiMsg;
    Reset doOkToReadMmiMsg;
    
    CustomBefRestart;
    WaitTime 0.5;
    
  ERROR
    RAISE;
  ENDPROC
    
  !------------------------------------------------------
  !	name:	CustomInitPlc
  !	descr:	User routine that executes at 
  !		initialization in order to initiate the
  !		PLC equipment.
  !		
  !	arg:	none
  !------------------------------------------------------
  PROC CustomInitPlc()
    TPWrite "PLC init routine...";
    !Initiate the PLC communication
    SendMsgToPlc MSG_INIT_PLC;
    WaitTime 0.5;
  ERROR
    RAISE;
  ENDPROC
  
  !------------------------------------------------------
  !	name:	CustomBefRestart
  !	descr:	User routine that executes before restart
  !		of program if error has occured.
  !		
  !	arg:	none
  !------------------------------------------------------
  PROC CustomBefRestart()
  !
    pal_newstart:=TRUE;
    
    FOR i FROM 1 TO NB_STATION DO
      SetAbsStatHeight i;
    ENDFOR
    !Check that the gripper is empty
    ChkGripToolEmpty;
    SetGripperIdle TRUE;
    CheckGripIdle TRUE;
    clearout 1800-GripHeight;
  ERROR
    RAISE;
  ENDPROC
  
  !------------------------------------------------------
  !	name:	CustomStopProg
  !	descr:	User routine that executes before stop
  !		of program if error has occured.
  !		Possibility to override and perform 
  !		restart instead
  !	arg:	none
  !------------------------------------------------------
  FUNC bool CustomStopProg(num errorcode)
    !Possibility to check a specific error code
    !and return FALSE if the program shall restart
    !instead of stop
    !
    !IF errorcode = xxx RETURN FALSE;
    RETURN TRUE;
  ERROR
    RAISE;
  ENDFUNC
  
  !------------------------------------------------------
  !	name:	GetSafetyHeight
  !	descr:	Support the calculation of the safety
  !		height. Takes the current station and
  !		calulates the safety height to next
  !		station.
  !		Possible to set up inhibitation of 
  !		safety move by assigning inhibSafety:
  !		0 - No inhibit
  !		1 - Inbibit safety move from station
  !		2 - Inhibit safety move to station
  !		3 - Inhibit safety move from/to station
  !	arg:	context of type num
  !		lastStation of type num
  !		newStation of type num	
  !		safetyHeight of type INOUT num
  !		optional argument inhibSafety 
  !		of type INOUT num	
  !------------------------------------------------------
  PROC GetSafetyHeight(
    num context,
    num lastStation,
    num newStation,
    INOUT num safetyHeight
    \INOUT num inhibSafety)
    
    VAR num tmpStationAbsh{MAX_STATION}:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
    GetAllStationAbs tmpStationAbsh;
    safetyHeight:=PalSafetyHeight(tmpStationAbsh,station_layout,lastStation,newStation);
  ERROR
    RAISE;
  ENDPROC
  
  !------------------------------------------------------
  !	name:	GetRetractRadius
  !	descr:	Handles the calulation of retract radius.
  !		
  !	arg:	context of type num
  !		lastStation of type num
  !		newStation of type num
  !		optional argument radius of type INOUT num	
  !------------------------------------------------------
  PROC GetRetractRadius(
    num context,
    num lastStation,
    num newStation,
    INOUT num radius)

    radius:=1400;
    
  ERROR
    RAISE;
  ENDPROC
  
  
ENDMODULE
