*-----------------------------------------------------------------------------
*  Temperature compensation for iBAG spindle
*-----------------------------------------------------------------------------
*  Module Description
*    - Only Z axis with compensation
*    - +/- 10vdc  X48 in 1/100 volts  ->  10um /    1 voltage
*                          10m volts       1um /  0.1 voltage
*                                        0.1um / 0.01 voltage = 10mV
*    - PT100 sensor is connect with X48 pin 9(I+), 10(I-), 11(U+) and 12(U-)
*      for spindle front bearing temperature monitoring
*    - Analog input connect with X48 pin 14(+/-10v) and pin 15(0v)
*      for spindle displacement compensation
*    - W580 is PN_W580_lag_error_compensation_Z in 0.1um
*-----------------------------------------------------------------------------

#define       KL_S_shaft_comp_scale                    K+1   ;0.1um=1mV
#define       KL_S_shaft_voltage_offset                K+0
#define       KL_S_shaft_plus_volt_max                 K+1000 ;+/- 11 vdc
#define       KL_S_shaft_minus_volt_max                K-1000 ;+/- 11 vdc
#define       KL_number_of_measuring                   K+100
#define       KL_S_shaft_analog_port                   K+64
#define       KL_Pt100_bearing_temp_port               K+69

#define /s    BL_average                               B

#define /s    ML_compensating                          M

#define /s    DL_S_shaft_voltage_sum                   D
#define /s    DL_S_shaft_voltage_average               D

#include Mac_Lib.inc

GLOBAL Temperature_comp

;----------------------------
LBL Temperature_comp
;----------------------------

  LN     MG_spindle_active
  EMT

*  PS     KL_Pt100_bearing_temp_port
*  CM     9003
*  PL     WG_S_front_bearing_temp
*   M_display_module_error(KG_error_module_9003)

  LN     MG_power_delayed_on
  CMT    Reset_displacement_comp
  EMT

*  L      WG_S_front_bearing_temp
*  >      DG_S1_max_temperature
*  S      PN_error_S_bearing_temp_max

  PS     KL_S_shaft_analog_port
  CM     9003
  PL     WG_S_shaft_voltage
   M_display_module_error(KG_error_module_9003)

  L      WG_S_shaft_voltage
  +      KL_S_shaft_voltage_offset
  =      WG_S_shaft_voltage

  L      MG_TC_manual
  O      MG_TC_help
  O      MG_TC_automatic
  =      MG_S_shaft_comp_pause

  L      MG_S_shaft_comp_pause
  ON     MG_T_compensation_active
  CMT    Reset_displacement_comp
  EMT

  L      ML_compensating
  IFT
    L      DL_S_shaft_voltage_sum
    /      KL_number_of_measuring
    =      DL_S_shaft_voltage_average

    L      DL_S_shaft_voltage_average
    >      KL_S_shaft_plus_volt_max
    IFT
      L      KL_S_shaft_plus_volt_max
      =      DL_S_shaft_voltage_average
    ENDI

    L      DL_S_shaft_voltage_average
    <      KL_S_shaft_minus_volt_max
    IFT
      L      KL_S_shaft_minus_volt_max
      =      DL_S_shaft_voltage_average
    ENDI

    L      DL_S_shaft_voltage_average
    X      KL_S_shaft_comp_scale
    X      K-1
    =      DG_S_shaft_compensation

    L      DG_S_shaft_compensation
    =      PN_W580_lag_error_compensation_Z

    L      K+0
    =      BL_average
    =      DL_S_shaft_voltage_average
    =      DL_S_shaft_voltage_sum

    L      MG_marker_one
    R      ML_compensating
  ENDI

  LN     ML_compensating
  IFT
    L      WG_S_shaft_voltage
    +      DL_S_shaft_voltage_sum
    =      DL_S_shaft_voltage_sum

    INC    BL_average

    L      BL_average
    <      KL_number_of_measuring
    SN     ML_compensating
    EMT
  ENDI

EM

LBL Reset_displacement_comp

  L      K+0
  =      PN_W580_lag_error_compensation_Z

  L      K+0
  =      BL_average
  =      DL_S_shaft_voltage_average
  =      DL_S_shaft_voltage_sum
  =      DG_S_shaft_compensation

EM
