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

MODULE SWDEFINE(SYSMODULE, READONLY)

  !---------------------------------------
  ! G L O B A L  D A T A
  !--------------------------------------- 

  ! Description: 
  ! CONST string Version_XXXXXXXX := "1.2";
  !
  !          XXXXXXXX - Module name
  !                 1 - version number  (changed of SEROP) 
  !                 2 - revision number (changed of SEROP)
  CONST string Version_SWDEFINE:="4.2";

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

  ! Called when the robot is restarted (warm start) from
  ! the TP or by power on.
  !
  ! Define all signals for each gun.
  ! e.g.
  ! DefGunIO 1\DOEqualize:=g1_equalize\DIForceComplete:=force_complete;
  ! DefInternalIO 1,g1_process_fault\DOProcessRun:=g1_process_run;
  ! DefTimerIO 1,g1_start_weld,g1_enable_curr,g1_weld_complete,g1_weld_fault\DOParity:=g1_parity\GOWeldProgram:=g1_weld_prog;
  PROC SwPowerOn()
 
    DefGunIO 1\DOEqualize:=g1_equalize\DIForceComplete:=force_complete;
    DefGunIO 2\DOEqualize:=g2_equalize\DIForceComplete:=force_complete;

    DefInternalIO 1,g1_process_fault\DOProcessRun:=g1_process_run;
    DefInternalIO 2,g2_process_fault\DOProcessRun:=g2_process_run;

    DefTimerIO 1,g1_start_weld,g1_enable_curr,g1_weld_complete,g1_weld_fault\DOParity:=g1_parity\GOWeldProgram:=g1_weld_prog;
    DefTimerIO 2,g2_start_weld,g2_enable_curr,g2_weld_complete,g2_weld_fault\DOParity:=g2_parity\GOWeldProgram:=g2_weld_prog;
 
  ENDPROC

  ! Called when execution is started from the beginning.
  ! of the program
  PROC SwStart()
  
  ENDPROC

  ! Called when execution is started from the position.
  ! where it was stopped
  PROC SwReStart()
  
  ENDPROC

  ! Called when the program is stopped.
  PROC SwStop()
  
  ENDPROC

  ! Called when the robot is quick stopped (E-stop).
  PROC SwQStop()
  
  ENDPROC

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

  ! These routines can be used to perform actions inside the Spot routines.

  ! Called in the beginnning of each Spot shell routine.
  ! Define user data.
  ! e.g.
  ! curr_gundata{GunNum}.gun_name := user_gundata.gun_name;
  ! curr_gundata{GunNum}.pre_close_time := user_gundata.pre_close_time;
  ! curr_gundata{GunNum}.pre_equ_time := user_gundata.pre_equ_time;
  ! curr_gundata{GunNum}.weld_counter := user_gundata. weld_counter;
  ! DefineGunData;
  PROC DefineUsrData(num GunNum)

  ENDPROC

  ! Called at the end of each Spot shell routine.
  ! Update user data.
  ! e.g.
  ! user_gundata.weld_counter := curr_gundata{GunNum}.weld_counter;
  ! user_gundata.curr_tip_wear := curr_gundata{GunNum}.curr_tip_wear;
  PROC UpdateUsrData(num GunNum)

  ENDPROC

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

  ! The following routines are found under the menu Special, Call service routine.

  ! Performs a manual weld with the selected gun and the current spotdata.
  PROC ManSpot()

    VAR num active_guns := 0;
    VAR num gun_num:=0;
    VAR bool change_data:=TRUE;
    VAR num in_key;

    FOR i FROM 1 TO SW_NOF_GUNS DO
      IF (IsGunActive(curr_gundata{i}.gun_name)) THEN
        gun_num := i;
        active_guns := active_guns+1;
      ENDIF
    ENDFOR

    IF active_guns > 1 THEN
      gun_num:=GetGunNum();
    ELSEIF active_guns < 1 THEN
      TPErase;
      TPWrite TxtGenGunTitle;
      TPWrite "1."+TxtGenNoName;
      TPWrite TxtGenChName;
      TPWrite "2."+TxtManMecNoAct;
      TPWrite TxtManActUnit;
      TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
      TPErase;
      RETURN;
    ENDIF

    DefineGunData;

    WHILE change_data DO
      TPErase;
      TPWrite TxtManSpotData;
      TPWrite " ";
      TPWrite " prog_no         = "+ValToStr(curr_spotdata{gun_num}.prog_no);
      TPWrite " tip_force       = "+ValToStr(curr_spotdata{gun_num}.tip_force);
      TPWrite " plate_thickness = "+ValToStr(curr_spotdata{gun_num}.plate_thickness);
      TPWrite " plate_tolerance = "+ValToStr(curr_spotdata{gun_num}.plate_tolerance);

      TPReadFK in_key,TxtGenCurrGun+curr_gundata{gun_num}.gun_name,TxtManCancel,stEmpty,stEmpty,TxtManChange,TxtManSpotWeld;

      TPErase;

      TEST in_key
      CASE 4:
        ChangeCurrSpot (gun_num);
      CASE 5:
        change_data:=FALSE;
      DEFAULT:
        RETURN;
      ENDTEST
    ENDWHILE

    Spot gun_num,curr_spotdata{gun_num};

  ENDPROC

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

    VAR num active_guns := 0;
    VAR num gun_num:=0;
    VAR num in_key;

    FOR i FROM 1 TO SW_NOF_GUNS DO
      IF (IsGunActive(curr_gundata{i}.gun_name)) THEN
        gun_num := i;
        active_guns := active_guns+1;
      ENDIF
    ENDFOR

    IF active_guns > 1 THEN
      gun_num:=GetGunNum();
    ELSEIF active_guns < 1 THEN
      TPErase;
      TPWrite TxtGenGunTitle;
      TPWrite "1."+TxtGenNoName;
      TPWrite TxtGenChName;
      TPWrite "2."+TxtManMecNoAct;
      TPWrite TxtManActUnit;
      TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
      TPErase;
      RETURN;
    ENDIF

    DefineGunData;

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

    TPReadFK in_key,TxtGenCurrGun+curr_gundata{gun_num}.gun_name,TxtManCancel,"A","B","C",stEmpty;

    TPErase;

    TEST in_key
    CASE 2:
      Calibrate gun_num\ToolChg;
    CASE 3:
      Calibrate gun_num\TipChg;
    CASE 4:
      Calibrate gun_num\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.
  PROC ManServiceCalib()

    VAR num active_guns := 0;
    VAR num gun_num:=0;
    VAR num in_key;
    VAR bool change_data:=TRUE;

    FOR i FROM 1 TO SW_NOF_GUNS DO
      IF (IsGunActive(curr_gundata{i}.gun_name)) THEN
        gun_num := i;
        active_guns := active_guns+1;
      ENDIF
    ENDFOR

    IF active_guns > 1 THEN
      gun_num:=GetGunNum();
    ELSEIF active_guns < 1 THEN
      TPErase;
      TPWrite TxtGenGunTitle;
      TPWrite "1."+TxtGenNoName;
      TPWrite TxtGenChName;
      TPWrite "2."+TxtManMecNoAct;
      TPWrite TxtManActUnit;
      TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
      TPErase;
      RETURN;
    ENDIF

    DefineGunData;

    WHILE change_data DO
      TPErase;
      TPWrite TxtManRevCUpdate;
      TPWrite TxtManRevCInfo1;
      TPWrite TxtManRevCInfo2;
      TPWrite TxtManFineCalib;
      TPWrite TxtManFCalInfo1;
      TPWrite TxtManFCalInfo2;
      TPReadFK in_key,TxtManServCalib+": "+curr_gundata{gun_num}.gun_name,TxtManCancel,stEmpty,stEmpty,"1","2";
      TPErase;

      TEST in_key
      CASE 4:
        ServiceCalib gun_num\RevCUpdate;
        change_data:=FALSE;
      CASE 5:
        TPWrite TxtManFineCalib;
        TPWrite " ";
        TPReadFK in_key,TxtManAckFineCal,TxtManCancel,stEmpty,stEmpty,stEmpty,"OK";
        TPErase;

        TEST in_key
        CASE 1:
        CASE 5:
          ServiceCalib gun_num\FineCalib;
          change_data:=FALSE;
        ENDTEST
      DEFAULT:
        RETURN;
      ENDTEST
    ENDWHILE

  ENDPROC

  ! Performs a manual force calibration with the selected gun.
  ! A - Select number of calibrations to be made, max tip_force
  ! and thickness for the sensor. A check will be made
  ! to determine if the requested force is too high for the gun.
  ! The force will be calculated with the STCalcForce function.
  ! B - Current values will be presented on the TP.
  ! Then the routine will loop through the calibrations increasing the force
  ! every time the gun closes until the maximum force is established with TP
  ! interaction between every gun closure.
  ! When the number of calibrations has been done it will be presented to the user,
  ! Pressing OK at this time will update the cfg.
  PROC ManForceCalib()

    VAR string InstPathName:="/MOC/SG_PROCESS/";
    VAR string AttributeName1:="no_of_active_db_posts";
    VAR string AttributeName2:="squeeze_force_";
    VAR string AttributeName3:="squeeze_torque_";
    VAR string MaxMotorTorque:="max_motor_torque";
    VAR bool change_data:=TRUE;
    VAR bool redo:=TRUE;
    VAR num num_of_calib:=5;
    VAR num curr_motortorque{10};
    VAR num thickness:=20;
    VAR num max_torque;
    VAR num active_guns := 0;
    VAR num gun_num:=0;
    VAR num force_val:=0;
    VAR num measured_force{10};
    VAR num incr_val:=0;
    VAR num max_val:=5000;
    VAR num input_val{10};
    VAR num in_key;
    VAR string val_string:="";

    FOR i FROM 1 TO SW_NOF_GUNS DO
      IF (IsGunActive(curr_gundata{i}.gun_name)) THEN
        gun_num := i;
        active_guns := active_guns+1;
      ENDIF
    ENDFOR

    IF active_guns > 1 THEN
      gun_num:=GetGunNum();
    ELSEIF active_guns < 1 THEN
      TPErase;
      TPWrite TxtGenGunTitle;
      TPWrite "1."+TxtGenNoName;
      TPWrite TxtGenChName;
      TPWrite "2."+TxtManMecNoAct;
      TPWrite TxtManActUnit;
      TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
      TPErase;
      RETURN;
    ENDIF

    DefineGunData;

    WHILE change_data DO
      TPErase;
      TPWrite TxtManForceCal;
      TPWrite " ";
      TPWrite " A. Update force calibration data";
      TPWrite " ";
      TPWrite " B. Run calibration";

      TPReadFK in_key,TxtGenCurrGun+curr_gundata{gun_num}.gun_name,stEmpty,stEmpty,stEmpty,"A","B";

      TEST in_key
      CASE 4:
        !Present current values and give the operator possibility to change each of them.
        val_string:="";
        num_of_calib:=GetTPVal(gun_num,num_of_calib,TxtManNumCalib,TxtManSelNum,TxtManNoCalibP,2,10,5,val_string);
        val_string:=" N";
        !Calculate maximum programmable force for the selected gun.
        ReadCfgData InstPathName+curr_gundata{gun_num}.gun_name,MaxMotorTorque,max_torque;
        max_val:=STCalcForce(curr_gundata{gun_num}.gun_name,max_torque);
        MAX_TIP_FORCE:=max_val;
        TPWrite TxtManMaxProgF1;
        TPWrite TxtManMaxProgF2+ValToStr(max_val)+" N";
        TPWrite "   ";
        TPWrite TxtManAcceptVal;
        TPReadFK in_key,TxtManSelOK,stEmpty,stEmpty,stEmpty,TxtManChange,"OK";
        TEST in_key
        CASE 4:
          max_val:=GetTPVal(gun_num,max_val,TxtManMaxForce,TxtManSelVal,TxtManCurrForce,0,MAX_TIP_FORCE,MAX_TIP_FORCE\force,val_string);
        ENDTEST
        val_string:=" mm";
        thickness:=GetTPVal(gun_num,thickness,TxtManSelThick,TxtManSetThick,TxtManCurrThick,0,100,20,val_string);
        int_forcedata{gun_num}.plate_thickness:=thickness;
        val_string:=" s";
        man_forcedata{gun_num}.force_time:=GetTPVal(gun_num,man_forcedata{gun_num}.force_time,TxtManSelTime,TxtManSetTime,TxtManCurrTime,1,10,1,val_string);
      CASE 5:
        !Present force calibration values to be used
        TPErase;
        TPWrite TxtManCurrVal;
        TPWrite "   ";
        TPWrite TxtManNoCalibP+ValToStr(num_of_calib);
        TPWrite TxtManCurrForce+ValToStr(max_val)+" N";
        TPWrite TxtManCurrThick+ValToStr(thickness)+" mm";
        TPWrite TxtManCurrTime+ValToStr(man_forcedata{gun_num}.force_time)+" s";
        TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,TxtManChange,"OK";
        TEST in_key
        CASE 5:
          !Perform calibration
          incr_val:=max_val/num_of_calib;
          force_val:=incr_val;
          TPErase;
          FOR i FROM 1 TO num_of_calib DO
            redo:=TRUE;
            WHILE redo DO
              measured_force{i}:=force_val;
              TPWrite TxtManCurrCalib1;
              TPWrite TxtManCurrCalib2+ValToStr(i)+" of "+ValToStr(num_of_calib);
              TPWrite " ";
              TPWrite TxtManCurrForce+ValToStr(force_val)+" N";
              TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
              TEST in_key
              CASE 5:
                !Close and open gun, enter measured force value if OK.
                ActClose gun_num\force_val:=force_val;
                WaitTime man_forcedata{gun_num}.force_time;
                ActOpen gun_num;
                curr_motortorque{i}:=Round(STCalcTorque(curr_gundata{gun_num}.gun_name,force_val)\Dec:=3);
                TPErase;
                TPWrite TxtManCurrForce+ValToStr(force_val)+" N";
                TPWrite TxtManCurrTorque+ValToStr(curr_motortorque{i})+" Nm";
                TPWrite "";
                TPWrite TxtManAckMeas;
                TPWrite TxtManRedoMeas;
                TPReadFK in_key,TxtManAcceptVal,stEmpty,stEmpty,stEmpty,TxtRetry,"OK";
                TEST in_key
                CASE 4:
                  !Repeat measurement.
                  TPErase;
                CASE 5:
                  TPErase;
                  TPWrite TxtManCurrForce+ValToStr(force_val)+" N";
                  TPWrite TxtManCurrTorque+ValToStr(curr_motortorque{i})+" Nm";
                  TPWrite " ";
                  TPWrite " ";
                  TPReadNum input_val{i},TxtManInValue;
                  TPErase;
                  force_val:=force_val+incr_val;
                  redo:=FALSE;
                ENDTEST
              ENDTEST
            ENDWHILE
          ENDFOR
          !Present number of calibrations done
          TPWrite TxtManNoCalib+ValToStr(num_of_calib);
          TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,TxtManNext;
          CreateTable num_of_calib,curr_motortorque,input_val;
          TPErase;
          TPWrite " ";
          !Activate the new calibration
          TPReadFK in_key,TxtManQueryRest1+" "+TxtManQueryRest2,TxtManCancel,stEmpty,stEmpty,stEmpty,"OK";
          TEST in_key
          CASE 5:
            !Save data to MOC.CFG
            WriteCfgData InstPathName+curr_gundata{gun_num}.gun_name,AttributeName1,num_of_calib;
            FOR i FROM 1 TO num_of_calib DO
              WriteCfgData InstPathName+curr_gundata{gun_num}.gun_name,AttributeName3+ValToStr(i),curr_motortorque{i};
              WriteCfgData InstPathName+curr_gundata{gun_num}.gun_name,AttributeName2+ValToStr(i),input_val{i};
            ENDFOR
            change_data:=FALSE;
            !Recalibrate the gun
            STRecalib curr_gundata{gun_num}.gun_name;
            !Recalculate maximum programmable force for the selected gun.
            ReadCfgData InstPathName+curr_gundata{gun_num}.gun_name,MaxMotorTorque,max_torque;
            MAX_TIP_FORCE := STCalcForce(curr_gundata{gun_num}.gun_name, max_torque);
          ENDTEST
        ENDTEST
      ENDTEST
    ENDWHILE

  ERROR
    IF ERRNO=ERR_RECAL_SGUN THEN
      ErrWrite TxtGenGunTitle+"("+curr_gundata{gun_num}.gun_name+")",TxtManRecalErr1\RL2:=TxtManRecalErr2\RL3:=TxtManRecalErr3\RL4:=TxtManRecalErr4;
    ENDIF
    RAISE;

  ENDPROC

  ! Performs a manual gun close according to the values in man_forcedata.
  PROC ManCloseGun()

    VAR num active_guns := 0;
    VAR num gun_num:=0;
    VAR bool change_data:=TRUE;
    VAR num in_key;

    FOR i FROM 1 TO SW_NOF_GUNS DO
      IF (IsGunActive(curr_gundata{i}.gun_name)) THEN
        gun_num := i;
        active_guns := active_guns+1;
      ENDIF
    ENDFOR

    IF active_guns > 1 THEN
      gun_num:=GetGunNum();
    ELSEIF active_guns < 1 THEN
      TPErase;
      TPWrite TxtGenGunTitle;
      TPWrite "1."+TxtGenNoName;
      TPWrite TxtGenChName;
      TPWrite "2."+TxtManMecNoAct;
      TPWrite TxtManActUnit;
      TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
      TPErase;
      RETURN;
    ENDIF

    DefineGunData;

    WHILE change_data DO
      TPErase;
      TPWrite TxtManSetFData;
      TPWrite " ";
      TPWrite " tip_force     = "+ValToStr(man_forcedata{gun_num}.tip_force);
      TPWrite " thickness     = "+ValToStr(man_forcedata{gun_num}.plate_thickness);
      TPWrite " tolerance     = "+ValToStr(man_forcedata{gun_num}.plate_tolerance);
      TPWrite " force_time    = "+ValToStr(man_forcedata{gun_num}.force_time);

      TPReadFK in_key,TxtGenCurrGun+curr_gundata{gun_num}.gun_name,TxtManCancel,stEmpty,stEmpty,TxtManChange,TxtManSetFApply;

      TPErase;

      TEST in_key
      CASE 4:
        ChangeCurrForce gun_num;
      CASE 5:
        change_data:=FALSE;
      DEFAULT:
        RETURN;
      ENDTEST
    ENDWHILE

    ActClose gun_num\Force:=man_forcedata{gun_num};

  ENDPROC

  ! Performs a manual gun open with the selected gun.
  PROC ManOpenGun()

    VAR num active_guns := 0;
    VAR num gun_num:=0;
    VAR bool change_data := TRUE;
    VAR num in_key;

    FOR i FROM 1 TO SW_NOF_GUNS DO
      IF (IsGunActive(curr_gundata{i}.gun_name)) THEN
        gun_num := i;
        active_guns := active_guns+1;
      ENDIF
    ENDFOR

    IF active_guns > 1 THEN
      gun_num:=GetGunNum();
    ELSEIF active_guns < 1 THEN
      TPErase;
      TPWrite TxtGenGunTitle;
      TPWrite "1."+TxtGenNoName;
      TPWrite TxtGenChName;
      TPWrite "2."+TxtManMecNoAct;
      TPWrite TxtManActUnit;
      TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
      TPErase;
      RETURN;
    ENDIF

    DefineGunData;

    WHILE change_data DO
      TPErase;
      TPWrite TxtGenCurrGun+curr_gundata{gun_num}.gun_name;

      TPReadFK in_key,TxtManSelOK,TxtManCancel,stEmpty,stEmpty,stEmpty,"OK";

      TPErase;

      TEST in_key
      CASE 5:
        change_data := FALSE;
      DEFAULT:
        RETURN;
      ENDTEST
    ENDWHILE

    ActOpen gun_num;

  ENDPROC

  ! Performs a manual gun close/opening according to the values in man_forcedata.
  PROC ManSetForce()

    VAR num active_guns := 0;
    VAR num gun_num:=0;
    VAR bool change_data:=TRUE;
    VAR num in_key;

    FOR i FROM 1 TO SW_NOF_GUNS DO
      IF (IsGunActive(curr_gundata{i}.gun_name)) THEN
        gun_num := i;
        active_guns := active_guns+1;
      ENDIF
    ENDFOR

    IF active_guns > 1 THEN
      gun_num:=GetGunNum();
    ELSEIF active_guns < 1 THEN
      TPErase;
      TPWrite TxtGenGunTitle;
      TPWrite "1."+TxtGenNoName;
      TPWrite TxtGenChName;
      TPWrite "2."+TxtManMecNoAct;
      TPWrite TxtManActUnit;
      TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
      TPErase;
      RETURN;
    ENDIF

    DefineGunData;

    WHILE change_data DO
      TPErase;
      TPWrite TxtManSetFData;
      TPWrite " ";
      TPWrite " tip_force     = "+ValToStr(man_forcedata{gun_num}.tip_force);
      TPWrite " thickness     = "+ValToStr(man_forcedata{gun_num}.plate_thickness);
      TPWrite " tolerance     = "+ValToStr(man_forcedata{gun_num}.plate_tolerance);
      TPWrite " force_time    = "+ValToStr(man_forcedata{gun_num}.force_time);

      TPReadFK in_key,TxtGenCurrGun+curr_gundata{gun_num}.gun_name,TxtManCancel,stEmpty,stEmpty,TxtManChange,TxtManSetFApply;

      TPErase;

      TEST in_key
      CASE 4:
        ChangeCurrForce gun_num;
      CASE 5:
        change_data:=FALSE;
      DEFAULT:
        RETURN;
      ENDTEST
    ENDWHILE

    SetForce gun_num,man_forcedata{gun_num};

  ENDPROC

  ! Search for servoguns in the system, and add the name to curr_gundata.
  PROC ManAddGunName()

    VAR bool change_data:=TRUE;
    VAR bool redo:=TRUE;
    ! 1 = Skip robot, start with external axis
    VAR num gun_num := 1;
    VAR num in_key;
    VAR num input_val;
    VAR string gun_name:="";

    WHILE change_data DO
      ! Search for mechanical units.
      WHILE GetNextMechUnit(gun_num,gun_name) DO
        redo:=TRUE;
        ! Check if it is a servogun.
        IF (STIsServoTool(gun_name)) THEN
          TPErase;
          TPWrite TxtManFoundGun+" "+gun_name;
          TPWrite " ";
          TPWrite TxtManQueryGun;
          TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,TxtManCancel,"OK";
          TPErase;
          TEST in_key
          CASE 4:
            ! Try next gun if there is one, else quit.
          CASE 5:
            WHILE redo DO
              TPWrite TxtManGunDef1;
              TPWrite TxtManGunDef2+" "+ValToStr(SW_NOF_GUNS);
              TPReadNum input_val,TxtManSelGunNo1;
              IF input_val<=SW_NOF_GUNS THEN
                TPErase;
                TPWrite TxtManSelGunNo2+" "+gun_name+" "+TxtManSelGunNo3+" "+TxtManGunNo{input_val};
                TPWrite " ";
                TPWrite TxtManGunData1+"{"+ValToStr(input_val)+"}"+TxtManGunData2+" "+TxtManGunData3;
                ! Update curr_gundata.
                curr_gundata{input_val}.gun_name:=gun_name;
                DefineGunData;
                TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
                ! Try next gun if there is one, else quit.
                redo:=FALSE;
              ELSE
                TPErase;
                TPWrite TxtGenNoEquip;
                TPWrite TxtManGunNoErr1+" "+ValToStr(SW_NOF_GUNS)+" "+TxtManGunNoErr2;
                TPWrite TxtManGunNoErr3;
                TPWrite " ";
                TPWrite TxtManSelNewGun;
              ENDIF
            ENDWHILE
          DEFAULT:
            RETURN;
          ENDTEST
        ELSE
          ! No servoguns exists in the system.
          TPErase;
          TPWrite TxtManNoGun;
          TPWrite " ";
          TPWrite TxtManAddMocCfg;
          TPReadFK in_key,TxtManSelOK,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
        ENDIF
      ENDWHILE
      RETURN;
    ENDWHILE
  ENDPROC

  ! Checks a robot position if it is suitable for a tip wear measurement
  PROC ManCheckMeasPos()

    VAR TouchUpdata touchUp_data :=[50,5,10,10,FALSE];
    
    IF opposite_z THEN
      touchUp_data.neg_direction:=TRUE;
    ENDIF
    
    touchUp_data.touch_up_force := m_touch_force;
    
    TupPosCheck touchUp_data;

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

  ! Changes the value of the current spotdata.
  LOCAL PROC ChangeCurrSpot(num gun_num)

    VAR bool change_value:=TRUE;
    VAR string key_string{5}:=[TxtManReturn,"","","",TxtManNext];
    VAR num curr_round:=1;
    VAR num in_key;

    WHILE change_value DO
      TPErase;
      TPWrite TxtManSpotDataDe;
      TPWrite " ";
      IF (curr_round=1) THEN
        TPWrite " A. prog_no         = "+ValToStr(curr_spotdata{gun_num}.prog_no);
        key_string{2}:="A";
        TPWrite " B. tip_force       = "+ValToStr(curr_spotdata{gun_num}.tip_force);
        key_string{3}:="B";
        TPWrite " C. plate_thickness = "+ValToStr(curr_spotdata{gun_num}.plate_thickness);
        key_string{4}:="C";
      ELSE
        TPWrite " D. plate_tolerance = "+ValToStr(curr_spotdata{gun_num}.plate_tolerance);
        key_string{2}:="D";
        key_string{3}:="";
        key_string{4}:="";
      ENDIF

      TPReadFK in_key,stEmpty,key_string{1},key_string{2},key_string{3},key_string{4},key_string{5};

      TPErase;

      TEST in_key
      CASE 1:
        change_value:=FALSE;
      CASE 2:
        IF (curr_round=1) THEN
          curr_spotdata{gun_num}.prog_no:=GetValue(TxtManSpotDataDe,TxtManSpotProg,ValToStr(curr_spotdata{gun_num}.prog_no));
        ELSE
          curr_spotdata{gun_num}.plate_tolerance:=GetValue(TxtManSpotDataDe,TxtManSpotToler,ValToStr(curr_spotdata{gun_num}.plate_tolerance));
        ENDIF
      CASE 3:
        IF (curr_round=1) curr_spotdata{gun_num}.tip_force:=GetValue(TxtManSpotDataDe,TxtManSpotForce,ValToStr(curr_spotdata{gun_num}.tip_force));
      CASE 4:
        IF (curr_round=1) curr_spotdata{gun_num}.plate_thickness:=GetValue(TxtManSpotDataDe,TxtManSpotThick,ValToStr(curr_spotdata{gun_num}.plate_thickness));
      CASE 5:
        IF (curr_round=1) THEN
          Incr curr_round;
        ELSE
          Decr curr_round;
        ENDIF
      DEFAULT:
        RETURN;
      ENDTEST
    ENDWHILE

  ENDPROC

  ! Changes the value of the man_forcedata.
  LOCAL PROC ChangeCurrForce(num gun_num)

    VAR bool change_value:=TRUE;
    VAR string key_string{5}:=[TxtManReturn,"","","",TxtManNext];
    VAR num curr_round:=1;
    VAR num in_key;

    WHILE change_value DO
      TPErase;
      TPWrite TxtManSpotDataDe;
      TPWrite " ";
      IF (curr_round=1) THEN
        TPWrite " A. tip_force             = "+ValToStr(man_forcedata{gun_num}.tip_force);
        key_string{2}:="A";
        TPWrite " B. plate_thickness       = "+ValToStr(man_forcedata{gun_num}.plate_thickness);
        key_string{3}:="B";
        TPWrite " C. plate_tolerance       = "+ValToStr(man_forcedata{gun_num}.plate_tolerance);
        key_string{4}:="C";
      ELSE
        TPWrite " D. force_time            = "+ValToStr(man_forcedata{gun_num}.force_time);
        key_string{2}:="D";
        key_string{3}:="";
        key_string{4}:="";
      ENDIF

      TPReadFK in_key,stEmpty,key_string{1},key_string{2},key_string{3},key_string{4},key_string{5};

      TPErase;

      TEST in_key
      CASE 1:
        change_value:=FALSE;
      CASE 2:
        IF (curr_round=1) THEN
          man_forcedata{gun_num}.tip_force:=GetValue(TxtManSetFDataDe,TxtManSetFForce,ValToStr(man_forcedata{gun_num}.tip_force));
        ELSE
          man_forcedata{gun_num}.force_time:=GetValue(TxtManSetFDataDe,TxtManSetFTout,ValToStr(man_forcedata{gun_num}.force_time));
        ENDIF
      CASE 3:
        IF (curr_round=1) man_forcedata{gun_num}.plate_thickness:=GetValue(TxtManSetFDataDe,TxtManSetFThick,ValToStr(man_forcedata{gun_num}.plate_thickness));
      CASE 4:
        IF (curr_round=1) man_forcedata{gun_num}.plate_tolerance:=GetValue(TxtManSetFDataDe,TxtManSetFToler,ValToStr(man_forcedata{gun_num}.plate_tolerance));
      CASE 5:
        IF curr_round=1 THEN
          Incr curr_round;
        ELSE
          Decr curr_round;
        ENDIF
      DEFAULT:
        RETURN;
      ENDTEST
    ENDWHILE

  ENDPROC

  ! Retreives the active gun(s).
  ! Returns FALSE if the gun is deactivated and
  ! if the gun does not exist.
  LOCAL FUNC bool IsGunActive(string gun_name)

    VAR mecunit tempmec;
    VAR bool active_unit;

    GetDataVal gun_name,tempmec;
    active_unit:=IsMechUnitActive(tempmec);

    RETURN active_unit;

  ERROR
    ! No gun with this name exists.
    IF ERRNO = ERR_SYM_ACCESS THEN
      SkipWarn;
      RETURN FALSE;
    ENDIF

  ENDFUNC

  ! Get the gun number. The choice is made by the operator.
  LOCAL FUNC num GetGunNum()

    VAR bool select_gun:=TRUE;
    VAR string key_string{5}:=["","","","",""];
    VAR num gun_num:=0;
    VAR num curr_round:=1;
    VAR num in_key;

    key_string{1}:=TxtManCancel;

    WHILE select_gun DO
      key_string{5}:=TxtManNext;
      TPErase;
      TPWrite TxtManSelGun;
      TPWrite " ";
      FOR i FROM 2 TO 4 DO
        gun_num:=(curr_round-1)*3+i-1;
        IF gun_num=SW_NOF_GUNS key_string{5}:="";
        IF gun_num>SW_NOF_GUNS THEN
          key_string{i}:="";
        ELSE
          key_string{i}:=TxtManGunNo{gun_num};
          TPWrite key_string{i}+" - "+curr_gundata{gun_num}.gun_name;
        ENDIF
      ENDFOR

      TPReadFK in_key,stEmpty,key_string{1},key_string{2},key_string{3},key_string{4},key_string{5};

      TEST in_key
      CASE 1:
        RETURN 0;
      CASE 2,3,4:
        select_gun:=FALSE;
        RETURN ((curr_round-1)*3+in_key-1);
      CASE 5:
        IF gun_num>SW_NOF_GUNS THEN
          curr_round:=1;
        ELSE
          Incr curr_round;
        ENDIF
      ENDTEST
    ENDWHILE

  ENDFUNC

  ! Returns a numeric value entered on TP.
  ! Called from the GetTPVal function.
  LOCAL FUNC num GetValue(string head,
                          string command,
                          string value)

    VAR num ret_value;

    TPErase;
    TPWrite head;
    TPWrite " ";
    TPWrite TxtManCurrValue+value;
    TPWrite " ";
    TPReadNum ret_value,command;
    RETURN ret_value;

  ENDFUNC

  ! Communicates with the user and returns a numeric value.
  ! Called from the ManForceCalib routine.
  LOCAL FUNC num GetTPVal(num gun_num,
                          num in_val,
                          string tp_string1,
                          string tp_string2,
                          string tp_string3,
                          num limit_val1,
                          num limit_val2,
                          num default_val
                          \switch force,
                          string value)

    VAR bool repeat:=TRUE;
    VAR num in_key;

    WHILE repeat DO
      in_val:=GetValue(tp_string1,tp_string2,ValToStr(in_val));
      TPErase;
      IF NOT Present(force) THEN
        IF in_val<limit_val1 OR in_val>limit_val2 THEN
          in_val:=default_val;
          TPWrite TxtManInvVal;
          TPWrite "  ";
        ENDIF
      ELSE
        IF in_val>limit_val2 THEN
          in_val:=default_val;
          TPWrite TxtManInvVal;
          TPWrite "  ";
        ENDIF
      ENDIF
      TPWrite tp_string3+ValToStr(in_val)+value;
      TPWrite " ";
      TPReadFK in_key,TxtManSelOK,stEmpty,stEmpty,stEmpty,TxtManChange,"OK";
      TEST in_key
      CASE 5:
        repeat:=FALSE;
      ENDTEST
    ENDWHILE
    TPErase;
    RETURN in_val;

  ENDFUNC

  ! Routine used to create table, called from the ManForceCalib routine.
  LOCAL PROC CreateTable(num num_of_values,
                         num table1{*},
                         num table2{*})

    VAR num op_answer;
    VAR num n;
    VAR num i;
    VAR num line;

    op_answer:=0;
    i:=1;
    WHILE i<>0 DO
      TPErase;
      TPWrite "Cal(No)   Torque(Nm)        Force(N)";
      n:=5;
      IF 5*i>num_of_values n:=num_of_values-5*(i-1);
      FOR j FROM 1 TO n DO
        line:=j+5*(i-1);
        TPWrite BuildTableLine(line,table1{line},table2{line});
      ENDFOR
      TPReadFK op_answer,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,TxtManNext;
      IF num_of_values>5*i THEN
        i:=i+1;
      ELSE
        i:=0;
      ENDIF
    ENDWHILE

  ENDPROC

  ! Function that writes on the TP. Called from the CreateTable routine.
  LOCAL FUNC string BuildTableLine(num linenumber,
                                   num table1value,
                                   num table2value)

    CONST num MaxCharForce:=16;
    CONST num MaxCharTorque:=16;
    CONST num MaxCharLineNum:=4;
    VAR num strl_force;
    VAR num strl_torque;
    VAR num strl_linenum;
    VAR string string_to_show;
    VAR string forcestring;
    VAR string torquestring;
    VAR string numberstring;

    strl_force:=StrLen(ValToStr(table1value));
    strl_torque:=StrLen(ValToStr(table2value));
    strl_linenum:=StrLen(ValToStr(linenumber));

    forcestring:=AddSpace(MaxCharForce,strl_force,ValToStr(table1value));
    torquestring:=AddSpace(MaxCharTorque,strl_torque,ValToStr(table2value));
    numberstring:=AddSpace(MaxCharLineNum,strl_linenum,ValToStr(linenumber));
    string_to_show:=numberstring+forcestring+torquestring;
    RETURN string_to_show;

  ENDFUNC

  ! Function to add space between columns. Called from the BuildTableLine function.
  LOCAL FUNC string AddSpace(num totallength,
                             num stringlength,
                             string value)

    VAR string outstring;

    FOR i FROM 1 TO totallength-stringlength DO
      outstring:=outstring+" ";
    ENDFOR
    outstring:=outstring+value;
    RETURN outstring;

  ENDFUNC

  !This routine can be used to update the robtarget when
  ! executing in step by step execution in manual mode.
  ! This routine is also called from the kernal, so do not
  ! change the name.
  !
  PROC UpdateRobtarget(robtarget ToPoint,
                       gunnum Gun,
                       PERS tooldata Tool
                       \PERS wobjdata WObj)

    VAR num key1;
    VAR num key2;
    VAR num key3;
    VAR num choice;
    VAR num step_to_move:=0;
    VAR bool first_time;
    VAR num axis_value;
    VAR string MocJoint:="/MOC/JOINT/";
    VAR string LogicalAxis:="logical_axis";
    VAR string step_text:="Small";
    VAR num curr_axis;
    VAR bool rtvalue;
    VAR num delta;
    VAR bool from_another_c:=FALSE;
    VAR bool do_setup:= TRUE;
    VAR bool small_value:=TRUE;
    VAR num step_from_system;
      
    step_from_system:=touch_up_stepL;
        
    ! Check witch gun that is active in the instruction.
    ReadCfgData MocJoint+int_gundata{Gun}.gun_name,LogicalAxis,curr_axis;
    
    ! get the current axis value 
    axis_value:=GetCurrExtAxis(ToPoint,curr_axis);
    
    TPErase;
    TPWrite TxtUpdateRobt1;
    TPWrite " ";
    TPWrite TxtUpdateRobt2;
    TPWrite " ";
    TPWrite TxtUpdateRobt3;
    TPWrite TxtUpdateRobt11;
    TPReadFK choice,stEmpty,TxtManChange,stEmpty,stEmpty,stEmpty,TxtManNext;
    WHILE choice<5 DO
      WHILE do_setup DO
        TEST choice
        CASE 1:
          IF step_to_move>0 OR step_to_move<0 THEN
            first_time:=FALSE;
          ELSE
            first_time:=TRUE;
          ENDIF
          TPErase;
          TPWrite TxtUpdateRobt5;
          TPWrite TxtUpdateRobt6;
          TPWrite TxtUpdateRobt8;
          TPWrite " ";
          TPWrite TxtUpdateRobt4;
          TPWrite TxtUpdateRobt12;
          TPReadFK key1,"Current step length "+ValToStr(step_from_system)+" mm",TxtInc,TxtDec,step_text,TxtShift,TxtStop;
          WHILE key1<4 DO
            TEST key1
            CASE 1:
              TPErase;
              !Move the fixed electrode
              IF first_time THEN
                IF opposite_z THEN
                  !step_to_move:=touch_up_step;
                  step_to_move:=step_from_system;
                  MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ELSE
                  !step_to_move:=-1*touch_up_step;
                  step_to_move:=-1*step_from_system;
                  MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ENDIF
              ELSE
                IF opposite_z THEN
                  IF from_another_c THEN
                    step_to_move:=0;
                    from_another_c:=FALSE;
                  ENDIF
                  !step_to_move:=step_to_move+touch_up_step;
                  step_to_move:=step_to_move+step_from_system;
                  MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ELSE
                  IF from_another_c THEN
                    step_to_move:=0;
                    from_another_c:=FALSE;
                  ENDIF 
                  !step_to_move:=step_to_move-1*touch_up_step;
                  step_to_move:=step_to_move-1*step_from_system;
                  MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ENDIF
              ENDIF
            CASE 2:
              TPErase;
              !Move the fixed electrode,enter this function for the first time
              IF first_time THEN
                IF opposite_z THEN
                  !step_to_move:=-1*touch_up_step;
                  step_to_move:=-1*step_from_system;
                  MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ELSE
                  !step_to_move:=touch_up_step;
                  step_to_move:=step_from_system;
                  MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ENDIF
              ELSE
                !One more move with the fixed electrode
                IF opposite_z THEN
                  IF from_another_c THEN
                    step_to_move:=0;
                    from_another_c:=FALSE;
                  ENDIF 
                    !step_to_move:=step_to_move-1*touch_up_step;
                    step_to_move:=step_to_move-1*step_from_system;
                    MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ELSE
                  IF from_another_c THEN
                    step_to_move:=0;
                    from_another_c:=FALSE;
                  ENDIF
                  !step_to_move:=step_to_move+touch_up_step;
                  step_to_move:=step_to_move+step_from_system;
                  MoveL RelTool(ToPoint,0,0,step_to_move),v100,fine,Tool\WObj?WObj;
                ENDIF
              ENDIF
            CASE 3:
              IF small_value THEN
                step_from_system:=touch_up_stepS;
                !touch_up_step:=0.1;
                small_value:=FALSE;
                step_text:="Large";
              ELSE
                !touch_up_step:=step_from_system;
                step_from_system:=touch_up_stepL;
                small_value:=TRUE;
                step_text:="Small";
              ENDIF  
            ENDTEST
            first_time:=FALSE;
            TPErase;
            TPWrite TxtUpdateRobt5;
            TPWrite TxtUpdateRobt6;
            TPWrite TxtUpdateRobt8;
            TPWrite " ";
            TPWrite TxtUpdateRobt4;
            TPWrite TxtUpdateRobt12;
            TPReadFK key1,"Current step length "+ValToStr(step_from_system)+" mm",TxtInc,TxtDec,step_text,TxtShift,TxtStop;
          ! End while key1<4
          ENDWHILE
            
          IF key1=4 THEN
            TPErase;
            TPWrite TxtUpdateRobt10;
            TPWrite TxtUpdateRobt6;
            TPWrite TxtUpdateRobt8;
            TPWrite " ";
            TPWrite TxtUpdateRobt4;
            TPWrite TxtUpdateRobt13;
            TPReadFK key2,"Current step length "+ValToStr(step_from_system)+" mm",TxtInc,TxtDec,step_text,TxtShift,TxtStop;
            WHILE key2<4 DO
              TEST key2
              CASE 1:
                TPErase;
                first_time:=TRUE;
                !axis_value:=axis_value+1;
                axis_value:=axis_value+step_from_system;
                ToPoint:=CRobT();
                MoveCurrAxis ToPoint,curr_axis,axis_value,Tool\WObj?WObj;
              CASE 2:
                TPErase;
                first_time:=TRUE;
                !axis_value:=axis_value-1;
                axis_value:=axis_value-step_from_system;
                ToPoint:=CRobT();
                MoveCurrAxis ToPoint,curr_axis,axis_value,Tool\WObj?WObj;
              CASE 3:
                IF small_value THEN
                  step_from_system:=touch_up_stepS;
                  !touch_up_step:=0.1;
                  small_value:=FALSE;
                  step_text:="Large";
                ELSE
                  step_from_system:=touch_up_stepL;
                  small_value:=TRUE;
                  step_text:="Small";
                ENDIF  
              ! End test key2  
              ENDTEST
              TPErase;
              TPWrite TxtUpdateRobt10;
              TPWrite TxtUpdateRobt6;
              TPWrite TxtUpdateRobt8;
              TPWrite " ";
              TPWrite TxtUpdateRobt4;
              TPWrite TxtUpdateRobt13;
              TPReadFK key2,"Current step length "+ValToStr(step_from_system)+" mm",TxtInc,TxtDec,step_text,TxtShift,TxtStop;
            ! End while key2<3
            ENDWHILE
          !Finish with setup from the first meny (fixed electrode)  
          ! key1=5
          ELSE
            TPErase;
            TPReadFK key3,TxtUpdateRobt14,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
            TPErase;
            Stop;
            RETURN;
          ENDIF
            ! Shift back for more changes on the fixed electrode 
            IF key2=4 THEN
              TPErase;
              from_another_c:=TRUE;
              !Finish with setup from the second meny (movable electrode)    
            ELSE
              TPErase;
              TPReadFK key3,TxtUpdateRobt14,stEmpty,stEmpty,stEmpty,stEmpty,"OK";
              TPErase;
              Stop;
              RETURN;
            ENDIF          
        ! End test choice    
        ENDTEST
      ! End while do_setup      
      ENDWHILE    
      TPErase;
      TPWrite TxtUpdateRobt1;
      TPWrite " ";
      TPWrite TxtUpdateRobt2;
      TPWrite " ";
      TPWrite TxtUpdateRobt3;
      TPWrite TxtUpdateRobt11;
      TPReadFK choice,stEmpty,TxtManChange,stEmpty,stEmpty,stEmpty,TxtManNext;
    ! End choice<5
    ENDWHILE

    !Goto next weld position without change anything
    IF choice=5 THEN
      TPErase;
      RETURN;
    ENDIF

  ENDPROC

  PROC MoveCurrAxis(INOUT robtarget curr,
                    num axis,
                    num axis_value,
                    PERS tooldata Tool
                    \PERS wobjdata WObj)

    TEST axis
    CASE 7:
      curr.extax.eax_a:=axis_value;
      MoveL curr,v100,fine,Tool\WObj?WObj;
    CASE 8:
      curr.extax.eax_b:=axis_value;
      MoveL curr,v100,fine,Tool\WObj?WObj;
    CASE 9:
      curr.extax.eax_c:=axis_value;
      MoveL curr,v100,fine,Tool\WObj?WObj;
    CASE 10:
      curr.extax.eax_d:=axis_value;
      MoveL curr,v100,fine,Tool\WObj?WObj;
    CASE 11:
      curr.extax.eax_e:=axis_value;
      MoveL curr,v100,fine,Tool\WObj?WObj;
    CASE 12:
      curr.extax.eax_f:=axis_value;
      MoveL curr,v100,fine,Tool\WObj?WObj;
    DEFAULT:
    ENDTEST

  ENDPROC

  FUNC num GetCurrExtAxis(robtarget curr,
                          num axis)

    TEST axis
    CASE 7:
      RETURN curr.extax.eax_a;
    CASE 8:
      RETURN curr.extax.eax_b;
    CASE 9:
      RETURN curr.extax.eax_c;
    CASE 10:
      RETURN curr.extax.eax_d;
    CASE 11:
      RETURN curr.extax.eax_e;
    CASE 12:
      RETURN curr.extax.eax_f;
    DEFAULT:
      Stop;
    ENDTEST

  ENDFUNC

  PROC SetCurrExtAxAbs(INOUT robtarget curr,
                       num axis,
                       num axis_value)

    TEST axis
    CASE 7:
      curr.extax.eax_a:=axis_value;
    CASE 8:
      curr.extax.eax_b:=axis_value;
    CASE 9:
      curr.extax.eax_c:=axis_value;
    CASE 10:
      curr.extax.eax_d:=axis_value;
    CASE 11:
      curr.extax.eax_e:=axis_value;
    CASE 12:
      curr.extax.eax_f:=axis_value;
    DEFAULT:
      Stop;
    ENDTEST

  ENDPROC

  PROC SetCurrExtAxRel(INOUT robtarget curr,
                       num axis,
                       num axis_value)

    TEST axis
    CASE 7:
      curr.extax.eax_a:=curr.extax.eax_a+axis_value;
    CASE 8:
      curr.extax.eax_b:=curr.extax.eax_b+axis_value;
    CASE 9:
      curr.extax.eax_c:=curr.extax.eax_c+axis_value;
    CASE 10:
      curr.extax.eax_d:=curr.extax.eax_d+axis_value;
    CASE 11:
      curr.extax.eax_e:=curr.extax.eax_e+axis_value;
    CASE 12:
      curr.extax.eax_f:=curr.extax.eax_f+axis_value;
    DEFAULT:
      Stop;
    ENDTEST

  ENDPROC
  
  !-------------------------------------------------------------------
  ! F I X E D  T I P  W E A R  C O M P E N S A T I O N  R O U T I N E 
  !-------------------------------------------------------------------  
  ! Fixed tip wear compensation
  PROC ReCalcTCP(gunnum Gun
                 \switch Reference
                 |switch TipChange
                 |switch TipWear,
                 INOUT tooldata Tool)
  
    VAR num tip_wear_value:=0;
    VAR num pos_adj:=0;
    VAR num sign:=1;
    ! used to calculate the displacement in position z, new TCP
    VAR pose adjust:=[[0,0,0],[1,0,0,0]];
    VAR num status:=1;
    VAR num input_value:=0;
         
    IF opposite_z THEN
      sign :=-1;
    ENDIF 
        
    IF ((NOT Present(Reference)) AND (NOT Present(TipChange)) 
       AND (NOT Present(TipWear))) THEN
      ErrWrite TxtGenGunTitle,TxtOthOptArg;
      RAISE ERR_SW_PARAM;
    ENDIF
    
    ! check the tip_wear _ratio value
    IF curr_gundata{Gun}.tip_wear_ratio < 0 OR curr_gundata{Gun}.tip_wear_ratio > 100 THEN
      TPErase;
      TPWrite TxtReCalcMess1;
      TPWrite TxtReCalcMess2; 
      TPWrite TxtReCalcMess3;
      TPWrite TxtReCalcMess4;
      TPWrite TxtReCalcMess5;
      TPReadFK input_value,"","","","","","OK";
      IF input_value = 5 THEN
        RETURN;
      ENDIF
    ENDIF
        
    IF Present(Reference) THEN
      curr_gundata{Gun}.curr_wear_fix := 0;
      ! store the reference TCP and values
      tw_ref_tool{Gun} := Tool; 
      tw_ref_dist{Gun,1}:=0;
      tw_ref_dist{Gun,2}:=0;
    ELSEIF Present(TipChange) THEN
      ! set curr_wear_fix = 0
      curr_gundata{Gun}.curr_wear_fix:=0;
      ! Compensation of variations of new tips, calculate the new pose of the TCP
      adjust.trans:=[0,0,sign*tw_ref_dist{Gun,2}];
      ! calculate the new frame
      Tool.tframe:=PoseMult(tw_ref_tool{Gun}.tframe,adjust);
    ELSEIF Present(TipWear) THEN 
      ! calculate the tip wear for the fixed tip
      tip_wear_value:=curr_gundata{Gun}.curr_tip_wear*(curr_gundata{Gun}.tip_wear_ratio/100);
      pos_adj:= tip_wear_value - curr_gundata{Gun}.curr_wear_fix;
  
      ! update curr_wear_fix
      UpdateFixTipData tip_wear_value,pos_adj,Gun,status\TipWear;
      IF status =1 THEN
      !Tip wear compensation, calculate the new pose of the TCP
      ! new displacement in position z 
      adjust.trans:=[0,0,-sign*tip_wear_value];
      ! calculate a new frame
      Tool.tframe:=PoseMult(tw_ref_tool{Gun}.tframe,adjust);
      ENDIF
    ENDIF

  ERROR
    IF ERRNO=ERR_SW_PARAM RAISE;

  ENDPROC

ENDMODULE
