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

MODULE FORCECAL (SYSMODULE)

 !*******************************************************************
 ! 
 ! Force calibration routine
 !
 ! Copyright (c) ABB Robotics Products AB 1996.  All rights reserved
 !
 ! Module name ..........: FORCECAL
 !
 !
 !
 !*******************************************************************
 
!---------------------------------------
! 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_FORCECAL := "4.0";

  ! Definition of max tip force
  PERS num MAX_TIP_FORCE := 6000;

  ! Definition of force_time
  PERS num force_time := 1;

  ! Definition of current process data
  ! Current gun name
  PERS string curr_gun_name := "SERVOGUN";

  ! 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.
  ! B - Current values will be presented on the TP and a check will be made
  ! to determine if the requested force is too high for the gun, if it is the
  ! force will be calculated with the MaxProgForce function.
  ! 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 and activate the new calibration.
  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 ret_thickness;
    VAR num max_torque;
    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 := "";

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

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

      TEST in_key
      CASE 4:
        !Present current values and give possibility to change each of them
        val_string := "";
        num_of_calib := GetTPVal(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_gun_name,MaxMotorTorque,max_torque;
        max_val := STCalcForce(curr_gun_name, max_torque);
        MAX_TIP_FORCE := max_val;
        TPWrite TxtManMaxProgF1;
        TPWrite TxtManMaxProgF2+ValToStr(max_val)+" N";
        TPWrite "   ";
        TPWrite TxtManAcceptVal;
        TPWrite TxtManSelOK;
        TPWrite TxtManSelCancel;
        TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,TxtCancel,TxtOK;
        TEST in_key
        CASE 4:
          max_val := GetTPVal(max_val, TxtManMaxForce, TxtManSelVal, TxtManCurrForce, 0, MAX_TIP_FORCE, MAX_TIP_FORCE \force, val_string);
        ENDTEST
        val_string := " mm";
        thickness := GetTPVal(thickness, TxtManSelThick, TxtManSetThick, TxtManCurrThick, 0, 100, 20, val_string);
        val_string := " s";
        force_time := GetTPVal(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(force_time)+" s";
        TPReadFK in_key,stEmpty,TxtCancel,stEmpty,stEmpty,stEmpty,TxtOK;
        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 TxtManCurrForce+ValToStr(force_val)+" N";
              TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,stEmpty,TxtOK;
              TEST in_key
              CASE 5:
                ! Close and open gun, enter measured force value
                ! Add the switch \RetThickness if you want the return value from STClose.
                ! STClose curr_gun_name, force_val, thickness \RetThickness:=ret_thickness;
                STClose curr_gun_name, force_val, thickness;
                WaitTime force_time;
                STOpen curr_gun_name;
                curr_motortorque{i} := Round(STCalcTorque (curr_gun_name, force_val)\Dec:=3);
                TPErase;
                TPWrite TxtManCurrTorque+ValToStr(curr_motortorque{i})+" Nm";
                TPWrite "";
                TPWrite TxtManAcceptVal;
                TPWrite TxtManAckMeas;
                TPWrite TxtManRedoMeas;
                TPReadFK in_key,stEmpty,stEmpty,stEmpty,stEmpty,TxtRetry,TxtOK;
                TEST in_key
                CASE 4:
                  ! Stay in while loop.
                  TPErase;
                CASE 5:
                  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,TxtOK;
          TPErase;
          !Activate the new calibration
          TPWrite TxtManQueryAct1;
          TPWrite TxtManQueryAct2;
          TPWrite " ";
          TPReadFK in_key,stEmpty,TxtCancel,stEmpty,stEmpty,stEmpty,TxtOK;
          TEST in_key
          CASE 5:
            !Save data to MOC.CFG
            WriteCfgData InstPathName+curr_gun_name,AttributeName1,num_of_calib;
            FOR i FROM 1 TO num_of_calib DO
              WriteCfgData InstPathName+curr_gun_name,AttributeName3+ValToStr(i),curr_motortorque{i};
              WriteCfgData InstPathName+curr_gun_name,AttributeName2+ValToStr(i),input_val{i};
            ENDFOR
            change_data := FALSE;
            STRecalib curr_gun_name;
            !Recalculate maximum programmable force for the selected gun.
            ReadCfgData InstPathName+curr_gun_name,MaxMotorTorque,max_torque;
            MAX_TIP_FORCE := STCalcForce(curr_gun_name, max_torque);
          ENDTEST
        ENDTEST
      ENDTEST
    ENDWHILE

  ENDPROC

  ! Communicates with the user and returns a numeric value.
  ! Called from the ManForceCalib routine.
  LOCAL FUNC num GetTPVal(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 " ";
      TPWrite TxtManSelOK;
      TPWrite TxtManSelCancel;
      TPReadFK in_key,stEmpty,TxtCancel,stEmpty,stEmpty,stEmpty,TxtOK;
      TEST in_key
      CASE 5:
        repeat := FALSE;
      ENDTEST
    ENDWHILE
    TPErase;
    RETURN in_val;

  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

ENDMODULE