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

MODULE SG_EX1 (SYSMODULE)

 !*******************************************************************
 ! 
 ! Example module for control of a servo gun
 !
 ! Copyright (c) ABB Robotics Products AB 1996.  All rights reserved
 !
 ! Module name ..........: SG_EX1
 !
 !
 !
 !*******************************************************************
 
!---------------------------------------
! G L O B A L   D A T A
!--------------------------------------- 

  ! Description: 
  ! CONST string Version_XXXXXXXX := "1.0";
  !
  !          XXXXXXXX - Module name
  !                 1 - version number  (changed of SEROP) 
  !                 2 - revision number (changed of SEROP)
  CONST string Version_SG_EX1 := "4.1";

  ! Current tipwear
  PERS num curr_tip_wear := 0;

  ! Current tipforce
  PERS num curr_tip_force := 0;

  ! Current plate thickness
  PERS num curr_pl_thick := 0;

  ! Current plate tolerace
  PERS num curr_pl_toler := 0;

  ! Measured thickness
  PERS num curr_thickness := 0;

  ! Possible return values from the user routines
  CONST num SG_OK := 0;
  CONST num SG_RETRY := 1;
  CONST num SG_CANCEL := 2;

  ! Current tool, used in the ServiceCalib routine.
  PERS tooldata current_tool:=[TRUE, [[0, 0, 0], [1, 0, 0, 0]],[-1, [0, 0, 0],[1, 0, 0, 0], 0, 0, 0]];
  ! Current wobj, used in the ServiceCalib routine.
  LOCAL PERS wobjdata current_wobj:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
  ! Current robtarget, used in the ServiceCalib routine.
  VAR robtarget current_pos;

!----------------------------------------------
! S H E L L   R O U T I N E S
!----------------------------------------------

  ! Performes a gun calibration, the type of calibration is
  ! selected by a switch.
  PROC Calibrate (string Gun
                  \switch ToolChg | switch TipChg | switch TipWear)

    VAR num ret_tip_wear;
    VAR num calib_mode;

    IF Present(ToolChg) THEN
      STCalib Gun \ToolChg \RetTipWear:=ret_tip_wear;
      UpdateCalibData ret_tip_wear \ToolChg;
    ENDIF
    IF Present(TipChg) THEN
      STCalib Gun \TipChg \RetTipWear:=ret_tip_wear;
      UpdateCalibData ret_tip_wear \TipChg;
    ENDIF
    IF Present(TipWear) THEN
      STCalib Gun \TipWear \RetTipWear:=ret_tip_wear;
      UpdateCalibData ret_tip_wear \TipWear;
    ENDIF

  BACKWARD
    RETURN;

  ERROR
    IF ERRNO = ERR_NO_SGUN THEN
      ErrWrite TxtErrGunTitle, TxtErrNoName;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTACT THEN
      ErrWrite TxtErrGunTitle,TxtErrMecNoAct;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTINIT THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtFineCalib;
    ELSEIF ERRNO = ERR_SGUN_NOTSYNC THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtRevCUpdate;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTOPEN THEN
      ErrWrite TxtErrGunTitle, TxtGunNotOpen;
    ENDIF
    RAISE;

  ENDPROC

  ! Performs a service calibration for the selected gun.
  ! FineCalib. Service calibration after fine calibration.
  ! RevCUpdate. Service calibration after revoultion counter update.
  PROC ServiceCalib (string Gun
  		     \switch FineCalib | switch RevCUpdate )

    VAR string MocJoint := "/MOC/JOINT/";
    VAR string LogicalAxis := "logical_axis";
    VAR num curr_axis;
    VAR num tip_wear;

    StopMove;
    StorePath;

    ! Update current tool and robtaget.
    current_tool := CTool();
    current_wobj := CWObj();
    current_pos := CRobT();

    ! Check witch gun to synchronize.
    ReadCfgData MocJoint+Gun,LogicalAxis,curr_axis;

    ! Set external axis value to 0mm.
    TEST curr_axis
    CASE 7:
      current_pos.extax.eax_a := 0;
    CASE 8:
      current_pos.extax.eax_b := 0;
    CASE 9:
      current_pos.extax.eax_c := 0;
    CASE 10:
      current_pos.extax.eax_d := 0;
    CASE 11:
      current_pos.extax.eax_e := 0;
    CASE 12:
      current_pos.extax.eax_f := 0;
    DEFAULT:
    ENDTEST

    ! Move the gun to zero position.
    MoveJ current_pos,v100,FINE,current_tool \WObj:=current_wobj;

    ! The gun performs a tool change calibration.
    IF Present(FineCalib) STCalib Gun \TipChg;
    ! The gun performs a tool change calibration.
    IF Present(RevCUpdate) STCalib Gun \ToolChg;

    ! The gun performs a tip wear calibration.
    STCalib Gun \TipWear \RetTipWear:=tip_wear;
    IF Present(FineCalib) UpdateCalibData tip_wear \TipWear;

    ! Set external axis value to 8mm.
    TEST curr_axis
    CASE 7:
      current_pos.extax.eax_a := 8;
    CASE 8:
      current_pos.extax.eax_b := 8;
    CASE 9:
      current_pos.extax.eax_c := 8;
    CASE 10:
      current_pos.extax.eax_d := 8;
    CASE 11:
      current_pos.extax.eax_e := 8;
    CASE 12:
      current_pos.extax.eax_f := 8;
    DEFAULT:
    ENDTEST

    ! Open the gun to 8 mm.
    MoveJ current_pos,vmax,FINE,current_tool \WObj:=current_wobj;

    RestoPath;
    StartMove;

  BACKWARD
    RETURN;

  ERROR
    RestoPath;
    IF ERRNO = ERR_NO_SGUN THEN
      ErrWrite TxtErrGunTitle, TxtErrNoName;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTACT THEN
      ErrWrite TxtErrGunTitle,TxtErrMecNoAct;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTINIT THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtFineCalib;
    ELSEIF ERRNO = ERR_SGUN_NOTSYNC THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtRevCUpdate;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTOPEN THEN
      ErrWrite TxtErrGunTitle, TxtGunNotOpen;
    ENDIF
    RAISE;

  ENDPROC

  ! Closes the gun and applies a predefined force
  ! The routine has an error handling mechanism to handle
  ! plate_thickness and tolerance.
  PROC CloseGun (string Gun,
                 num TipForce,
                 num PlateThickness,
                 num PlateTolerance)

    VAR bool repeat:=TRUE;
    VAR num curr_tipforce;
    VAR bool get_data := FALSE;
    VAR num tmp_tolerance;
    VAR num status := SG_OK;

    curr_tipforce := TipForce;

    WHILE repeat DO
      STClose Gun, curr_tipforce, PlateThickness \RetThickness:=curr_thickness;
      IF ((PlateTolerance <> 0) AND ((curr_thickness < (PlateThickness - PlateTolerance)) OR
        (curr_thickness > (PlateThickness + PlateTolerance)))) THEN
        STOpen Gun;
        status:=TipPosError (PlateTolerance, PlateThickness, curr_thickness);

        TEST status
        CASE SG_CANCEL:
          tmp_tolerance := PlateTolerance;
          PlateTolerance := 0;
          get_data := TRUE;
        DEFAULT:
        ENDTEST
      ELSE
        IF get_data PlateTolerance := tmp_tolerance;
        repeat:=FALSE;
      ENDIF
    ENDWHILE

    ! Update current process data.
      curr_tip_force := curr_tipforce;
      curr_pl_thick := PlateThickness;
      curr_pl_toler := PlateTolerance;

  BACKWARD
    RETURN;

  ERROR
    IF ERRNO = ERR_NO_SGUN THEN
      ErrWrite TxtErrGunTitle, TxtErrNoName;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTACT THEN
      ErrWrite TxtErrGunTitle,TxtErrMecNoAct;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTINIT THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtFineCalib;
    ELSEIF ERRNO = ERR_SGUN_NOTSYNC THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtRevCUpdate;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTOPEN THEN
      ErrWrite\W, TxtErrGunTitle, TxtGunNotOpen;
    ENDIF
    RAISE;

  ENDPROC

  ! Opens the gun to the position it had before the close order
  PROC OpenGun (string Gun)

    STOpen Gun;

  BACKWARD
    RETURN;

  ERROR
    IF ERRNO = ERR_NO_SGUN THEN
      ErrWrite TxtErrGunTitle, TxtErrNoName;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTACT THEN
      ErrWrite TxtErrGunTitle,TxtErrMecNoAct;
    ENDIF
    IF ERRNO = ERR_SGUN_NOTINIT THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtFineCalib;
    ELSEIF ERRNO = ERR_SGUN_NOTSYNC THEN
      ErrWrite TxtErrGunTitle,TxtServCal1\RL2:=TxtServCal2\RL3:=TxtRevCUpdate;
    ENDIF
    RAISE;

  ENDPROC

!-------------------------------------------------------
! O T H E R   R O U T I N E S  A N D  F U N C T I O N S
!-------------------------------------------------------

  ! Called at the end of the Calibrate instruction.
  ! Update the current tip_wear.
  PROC UpdateCalibData (num CurrTipWear
  			\switch ToolChg | switch TipChg | switch TipWear)

    curr_tip_wear := CurrTipWear;

  ENDPROC

!----------------------------------------------
! E R R O R   R O U T I N E S
!----------------------------------------------

  ! Error recovery function when error in the
  ! achieved tip position.
  ! Press retry to close the gun again, and press
  ! ignore to skip the error and close the gun.
  LOCAL FUNC num TipPosError (num Tolerance,
                              num Thickness,
                              num CurrThickness)

    VAR string tip_pos_err_text := "";
    VAR num input_key;
    VAR num status := SG_OK;
    VAR bool repeat:=TRUE;

    IF CurrThickness < -0.2 THEN
      tip_pos_err_text := TxtPlateGunPos;
    ELSEIF CurrThickness < 0.2 THEN
      tip_pos_err_text := TxtPlateMissing;
    ELSE
      tip_pos_err_text := TxtPlateImproper;
    ENDIF

    WHILE repeat DO
      TPErase;
      TPWrite tip_pos_err_text;
      TPWrite TxtGenCurrGun+curr_gun_name;
      TPWrite " ";
      TPWrite TxtPlateTol+ValToStr(Tolerance)+" mm";
      TPWrite TxtPlateExpTh+ValToStr(Thickness)+" mm";
      TPWrite TxtPlateAchTh+ValToStr(CurrThickness)+" mm";

      TPWrite TxtInfoTP;
      TPReadFK input_key,"","","","",TxtIgnore,TxtRetry;
      TPErase;

      TEST input_key
      CASE 4:
        TPWrite TxtContAnyWay;
        TPWrite "";
        TPReadFK input_key,"",TxtCancel,"","","",TxtOK;
        TPErase;
        TEST input_key
        CASE 5:
          ErrWrite\W, TxtCurrErrAbort, tip_pos_err_text;
          repeat:=FALSE;
          status := SG_CANCEL;
        ENDTEST
      CASE 5:
        repeat:=FALSE;
        status := SG_RETRY;
      DEFAULT:
        status := SG_CANCEL;
      ENDTEST
    ENDWHILE

    RETURN status;

  ENDFUNC

!---------------------------------------
! M A N U A L  A C T I O N S
!---------------------------------------

  ! Performs a manual calibration with the selected gun.
  PROC ManCalib()

    VAR num in_key;

    TPErase;
    TPWrite TxtManCalibCh;
    TPWrite " ";
    TPWrite " A. ToolChg";
    TPWrite " B. TipChg";
    TPWrite " C. TipWear";

    TPReadFK in_key,stEmpty,TxtCancel,"A","B","C",stEmpty;

    TPErase;

    TEST in_key
    CASE 2:
      Calibrate curr_gun_name \ToolChg;
    CASE 3:
      Calibrate curr_gun_name \TipChg;
    CASE 4:
      Calibrate curr_gun_name \TipWear;
    DEFAULT:
      RETURN;
    ENDTEST

  ENDPROC

  ! Performs a manual service calibration for the selected gun.
  ! 1.Synchronize the tip position after revoultion counter update.
  ! 2.Initialize the servo gun position after fine calibration.
  ! When the calibrations are done the gun will open to 8 mm.
  PROC ManServiceCalib()

    VAR num in_key;

    TPErase;
    TPWrite TxtRevCUpdate;
    TPWrite TxtManRevCUpdate;
    TPWrite TxtFineCalib;
    TPWrite TxtManFineCalib;

    TPReadFK in_key,TxtManServCalib,TxtCancel,stEmpty,stEmpty,"1","2";
    TPErase;

    TEST in_key
    CASE 4:
      ServiceCalib curr_gun_name \RevCUpdate;
    CASE 5:
      ServiceCalib curr_gun_name \FineCalib;
    DEFAULT:
      RETURN;
    ENDTEST

  ENDPROC

ENDMODULE