*-----------------------------------------------------------------------------
*  3D head
*-----------------------------------------------------------------------------
*  Module Description
*
*-----------------------------------------------------------------------------

#define /s    KL_actual_value_fixed                    K +0
#define /s    KL_gear_range_1                          K +0
#define /s    KL_mode_procent_rated_current            K +0
#define /s    KL_torque_limitation                     K -1
#define /s    KL_handwheel_axis_bitcoded               K +10
#define /s    KL_inactive                              K -1

#define /s    KL_current_edit_field                    K +4
#define /s    KL_3D_head                               K +16
#define /s    KL_3D_head_edit                          K +17

#define /s    ML_3D_head_edit                          M
#define /s    ML_stack_synchronize                     M
#define /s    ML_timer_off                             M
#define /s    ML_3D_head_display_has_been_active       M
#define /s    ML_3D_head_enter_has_been_active         M
#define /s    ML_3D_head_initial                       M
#define /s    ML_selection_head_position               M

#define /s    ML_head_pos                              M[16]
#define /c    ML_head_pos_axis_1_selected              M &ML_head_pos + 0
#define /c    ML_head_pos_axis_2_selected              M &ML_head_pos + 1
#define /c    ML_spindle_pos_axis_1_selected           M &ML_head_pos + 2
#define /c    ML_spindle_pos_axis_2_selected           M &ML_head_pos + 3
#define /c    ML_axis_1_min_torque                     M &ML_head_pos + 4
#define /c    ML_axis_2_min_torque                     M &ML_head_pos + 5
#define /c    ML_axis_1_unclampig                      M &ML_head_pos + 6
#define /c    ML_axis_2_unclampig                      M &ML_head_pos + 7

#define /s    BL_active_3D_head_axis                   B

#define /s    WL_current_edit_field                    W
#define /s    WL_last_edit_field                       W
#define /s    WL_stack_synchronize                     W

#define /s    DL_active_handwheel_axis                 D


#include Mac_Lib.inc

EXTERN Absolute

GLOBAL Head_3D

;----------------------------
LBL Head_3D
;----------------------------

  CM     Axis_assignment

#if CFG_Reference_value_selection = "active"

  L      DG_controlled_axes
  A      KG_axes_with_c_axis_mode
  >      K+0
  IFT
    LN     MG_3D_head_setup_ok
    S      PN_error_3D_head_setup
  ENDI

  L      MG_M36_reference_3D_head_axes
  A      MG_pulse_M_function
  O      NP_M4172_1st_PLC_run_after_power_on
  S      ML_3D_head_initial

  L      MG_M36_reference_3D_head_axes
  A      MG_machine_parameter_are_read
  AN     PN_M4134_strobe_G_range_S_speed
  S      PN_M4092_quit_M_function

#if KG_3D_head_axis_1 >= 0

  L      ML_selection_head_position
  CMT    Save_spindle_position

  L      MG_machine_parameter_are_read
  A      ML_3D_head_initial
  O      ML_head_pos_axis_1_selected
  IFT
    R      ML_head_pos_axis_1_selected
    S      ML_selection_head_position

    PS     KG_3D_head_axis_1

    L      DG_hirth_axes
    BT     KG_3D_head_axis_1
    IFT
      L      DG_MP430_hirth_increm_X[KG_3D_head_axis_1]
      /      K+2
      +      DG_3D_head_axis_1_position
      /      DG_MP430_hirth_increm_X[KG_3D_head_axis_1]
      X      DG_MP430_hirth_increm_X[KG_3D_head_axis_1]
      PSW

      L      DG_3D_head_axis_1_position
      <      K+0
      IFT
        PLW
        -      DG_MP430_hirth_increm_X[KG_3D_head_axis_1]
        PSW
      ENDI

    ELSE
      PS     DG_3D_head_axis_1_position
    ENDI

    CM     9147
    M_display_module_error

*$WS
*    CM     Spindel_Position_speichern
  ENDI
#endif

#if KG_3D_head_axis_2 >= 0

  L      MG_machine_parameter_are_read
  A      ML_3D_head_initial
  O      ML_head_pos_axis_2_selected
  IFT
    R      ML_head_pos_axis_2_selected
    S      ML_selection_head_position

    PS     KG_3D_head_axis_2

    L      DG_hirth_axes
    BT     KG_3D_head_axis_2
    IFT
      L      DG_MP430_hirth_increm_X[KG_3D_head_axis_2]
      /      K+2
      +      DG_3D_head_axis_2_position
      /      DG_MP430_hirth_increm_X[KG_3D_head_axis_2]
      X      DG_MP430_hirth_increm_X[KG_3D_head_axis_2]
      PSW

      L      DG_3D_head_axis_2_position
      <      K+0
      IFT
        PLW
        -      DG_MP430_hirth_increm_X[KG_3D_head_axis_2]
        PSW
      ENDI
    ELSE
      PS     DG_3D_head_axis_2_position
    ENDI

    CM     9147
    M_display_module_error

*    CM     Save_spindle_position

  ENDI
#endif
#endif

  L      MG_machine_parameter_are_read
  R      ML_3D_head_initial

  L      MG_3D_head_display
  O      MG_3D_head_enter
  IFT
*$WS Problem still in NC software 340 422 04 available
* (Softkey type Radio)
    L      MG_3D_head_display
    AN     ML_3D_head_display_has_been_active
    R      MG_3D_head_enter

    L      MG_3D_head_display
    =      ML_3D_head_display_has_been_active

    L      MG_3D_head_enter
    AN     ML_3D_head_enter_has_been_active
    R      MG_3D_head_display

    L      MG_3D_head_enter
    =      ML_3D_head_enter_has_been_active

    L      PN_error_3D_head_setup
    R      PN_error_3D_head_setup

    L      NP_W274_code_pushed_key
    ==     KG_key_enter
    A      MG_3D_head_enter
    IFT
      S      MG_3D_head_setup_ok

      L      WL_current_edit_field
      W=     ML_head_pos

      L      MG_3D_head_enter
      R      MG_3D_head_enter
      S      MG_3D_head_display
    ENDI

    PS     KL_current_edit_field
    CM     9211
    PL     WL_current_edit_field
    M_display_module_error

  ENDI

  L      MG_marker_one
  R      TS_3D_head_timeout
  S      TS_3D_head_timer_universal

  L      NP_M4151_mode_electronic_handwheel
  O      NP_M4156_mode_soft_key_manual_operation
*$WS HR420 ?
  IFT
    PS     KL_handwheel_axis_bitcoded
    CM     9035
    PL     DL_active_handwheel_axis
    M_display_module_error

    L      DL_active_handwheel_axis
    BT     KG_3D_head_axis_1
    S      PN_M4576_suppress_handwheel_pulses

#if KG_3D_head_axis_2 >= 0
    L      DL_active_handwheel_axis
    BT     KG_3D_head_axis_2
    S      PN_M4576_suppress_handwheel_pulses
#endif
  ENDI

  CASE   BG_step_3D_head
    CM     Activate                    ;00
    CM     G_range_1                   ;01
    CM     G_range_1_ok                ;02
    CM     S_start_3D_pos              ;03
    CM     S_posit_ok                  ;04
    CM     Head_time                   ;05
    CM     Head_release_on             ;06
    CM     Head_in_position            ;07
    CM     Head_time                   ;08
    CM     Head_torque_limited         ;09
    CM     Head_time                   ;10
    CM     Head_release_off            ;11
    CM     Head_inactive               ;12
    CM     G_range_old                 ;13
    CM     G_range_old_ok              ;14
    CM     Head_end                    ;15
  ENDC

  PLL
  S      TS_3D_head_timeout
  IFT
    INC    BG_step_3D_head
  ENDI

  LN     MG_3D_head_active
  EMT

  L      MG_key_NC_start_p_pulse
  R      MG_3D_head_interruption

  L      MG_key_NC_start
  O      MG_pulse_CE_key
  S      TS_3D_head_timeout

  LN     TR_3D_head_Timeout
  AN     TS_3D_head_timeout
  =      PN_error_3D_head_timeout
  S      MG_3D_head_interruption

  L      MG_key_NC_stop
  O      MG_pulse_external_stop
  S      MG_3D_head_interruption

  LN     MG_control_operational
  O      PN_error_protection_working_area
  CMT    Head_reset

EM

;----------------------------
LBL Activate                           ;00

  L      MG_marker_one
  S      TS_3D_head_timeout

  PS     MG_marker_zero

  L      DG_S_axis_activation
  BT     KG_3D_head_axis_1
  S      MG_3D_head_active
  IFT
    S      MG_3D_head_axis_1_active

    PL     ML_stack_synchronize
    PS     MG_marker_one

    L      KG_3D_head_axis_1
    =      BL_active_3D_head_axis

    L      DG_3D_head_axis_1_S_position
    =      DG_3D_head_S_position
  ELSE
    L      DG_S_axis_activation
    BT     KG_3D_head_axis_2
    S      MG_3D_head_active
    IFT
      S      MG_3D_head_axis_2_active

      PL     ML_stack_synchronize
      PS     MG_marker_one

      L      KG_3D_head_axis_2
      =      BL_active_3D_head_axis

      L      DG_3D_head_axis_2_S_position
      =      DG_3D_head_S_position
    ENDI
  ENDI

EM

;----------------------------
LBL G_range_1                          ;01

  PS     MG_marker_one

  L      NP_W256_G_code_spindle
  <>     KL_gear_range_1
  S      MG_gear_range_1_from_3D_head

EM

;----------------------------
LBL G_range_1_ok                       ;02

  L      MG_Sn_I_gear_range_1
  AN     MG_Sn_I_gear_range_2
  AN     MG_gear_range_1_from_3D_head
  ON     MG_spindle_with_gear
  AN     MG_3D_head_interruption
  PSL

  L      MG_3D_head_interruption
  =      MG_win_3D_head_gear_range

EM

;----------------------------
LBL S_start_3D_pos                     ;03

  L      MG_marker_one
  S      MG_3D_head_spindle_orientation_on
  PSL

EM

;----------------------------
LBL S_posit_ok                         ;04

  L      MG_3D_head_interruption
  =      MG_win_3D_head_spindle_position

  L      DG_3D_head_S_position
  -      DG_position_axis_S_actual
  PSW
  CM     Absolute
  PLW
  <      DG_pos_window_S
  O[
    L      DG_3D_head_S_position
    ==     K+0
    A[
      L      K+3600000
      -      DG_position_axis_S_actual
      <      DG_pos_window_S
    ]
  ]
  AN     MG_3D_head_spindle_orientation_on
  AN     MG_3D_head_interruption
  S      MG_spindle_off_orientation
  PSL
  EMF

  L      DG_S_axis_activation
  BT     KG_3D_head_axis_1
  IFT
    L      DG_max_torque_axis_1
    >      K+0
    IFT
      PS     KL_mode_procent_rated_current
      PS     KG_3D_head_axis_1
      PS     DG_max_torque_axis_1
      CM     9128
    M_display_module_error
    ENDI
  ELSE
    L      DG_max_torque_axis_2
    >      K+0
    IFT
      PS     KL_mode_procent_rated_current
      PS     KG_3D_head_axis_2
      PS     DG_max_torque_axis_2
      CM     9128
    M_display_module_error
    ENDI
  ENDI

EM

;----------------------------
LBL Head_release_on                    ;06

*$WS NC software bug CC424 and 2 encoder systems
  L      MG_spindle_off
  IFT
    L      DG_S_axis_release
    BS     BL_active_3D_head_axis
    =      DG_S_axis_release

    PS     MG_marker_one
  ELSE
    PS     MG_marker_zero
  ENDI

EM

;----------------------------
LBL Head_in_position                   ;07

  L      MG_marker_one
  R      MG_3D_head_interruption
  S      TS_3D_head_timeout

  L      NP_D1100_axes_release
  BT     BL_active_3D_head_axis
  R      PN_M4576_suppress_handwheel_pulses

  LD     MG_X_in_hirth_position
  ON     DG_hirth_axes
  A      DG_activate_control_loop_open
  A      NP_D1104_axes_in_position
  AN     DG_clamping_mode_off
  AN     DG_PLC_posit_hirth_necessary
  BT     BL_active_3D_head_axis
  AN     MG_key_NC_stop
  PSL
  EMF

  L      BL_active_3D_head_axis
  =X

  L      DG_keys_inhibit
  BSX
  =      DG_keys_inhibit

  L      BG_step_axis_X[X]
  <>     K+5
  IFT
    PL     ML_stack_synchronize
    PS     MG_marker_zero
  ENDI

EM

;----------------------------
LBL Head_time                          ;05 ;08 ;10

  LN     TR_3D_head_timer_universal
  O      ML_timer_off
  =      TS_3D_head_timer_universal
  PSL

EM

;----------------------------
LBL Head_torque_limited                ;09

* Dependent on the 3D head mechanics it can be also necessary to store
* spindle and axis position after clamping the 3D head axis.
* (Step 12)

  CM     Save_axis_position
  CM     Save_spindle_position

  PS     MG_marker_one

  L      MG_3D_head_interruption
  =      MG_win_3D_head_torque

  L      BL_active_3D_head_axis
  ==     KG_3D_head_axis_1
  IFT
    L      DG_3D_head_axis_1_min_torque
    >      K+0
    SN     ML_timer_off
    IFT
      PS     KL_mode_procent_rated_current
      PS     KG_3D_head_axis_1
      PS     DG_3D_head_axis_1_min_torque
      CM     9128
      M_display_module_error
    ENDI
  ELSE
    L      DG_3D_head_axis_2_min_torque
    >      K+0
    SN     ML_timer_off
    IFT
      PS     KL_mode_procent_rated_current
      PS     KG_3D_head_axis_2
      PS     DG_3D_head_axis_2_min_torque
      CM     9128
    M_display_module_error
    ENDI
  ENDI

EM

;----------------------------
LBL Head_release_off                   ;11

  PS     MG_marker_one

  L      DG_S_axis_release
  BC     BL_active_3D_head_axis
  =      DG_S_axis_release

EM

;----------------------------
LBL Head_inactive                      ;12

  LN     DG_S_axis_activation
  BT     BL_active_3D_head_axis
  PSL
  EMF

* Dependent on the 3D head mechanics it can be also necessary to store
* spindle and axis position before clamping the 3D head axis.
* (Step 9)

*  CM     Save_axis_position
*  CM     Save_spindle_position

EM

;----------------------------
LBL G_range_old                        ;13

  PS     MG_marker_one

  L      NP_W256_G_code_spindle
  <>     KL_gear_range_1
*$WS
*  A      MG_Spindel_mit_Getriebe
  S      MG_gear_range_from_3D_head

EM

;----------------------------
LBL G_range_old_ok                     ;14

  LN     MG_gear_range_from_3D_head
  AN     MG_3D_head_interruption
  PSL

  L      MG_3D_head_interruption
  =      MG_win_3D_head_gear_range

EM

;----------------------------
LBL Head_end                           ;15

  PS     MG_marker_zero

LBL Head_reset

  L      DG_max_torque_axis_1
  +      DG_3D_head_axis_1_min_torque
  >      K+0
  IFT
    PS     KL_mode_procent_rated_current
    PS     KG_3D_head_axis_1
    PS     KL_torque_limitation
    CM     9128
    M_display_module_error
  ENDI

  L      DG_max_torque_axis_2
  +      DG_3D_head_axis_2_min_torque
  >      K+0
  IFT
    PS     KL_mode_procent_rated_current
    PS     KG_3D_head_axis_2
    PS     KL_torque_limitation
    CM     9128
    M_display_module_error
  ENDI

  L      MG_marker_one
  R      MG_3D_head_interruption
  R      MG_3D_head_active
  R      MG_3D_head_axis_1_active
  R      MG_3D_head_axis_2_active
  R      ML_timer_off

  L      K+0
  =      BG_step_3D_head

  L      DG_S_axis_release
  BC     KG_3D_head_axis_1
  BC     KG_3D_head_axis_2
  =      DG_S_axis_release

  L      DG_S_axis_activation
  BC     KG_3D_head_axis_1
  BC     KG_3D_head_axis_2
  =      DG_S_axis_activation

EM

;----------------------------
LBL Axis_assignment

#if KG_3D_head_axis_1 = &axis_4
  L      O_unclamping_4
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_4
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_4
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_4

  L      BG_step_axis_4
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_5
  L      O_unclamping_5
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_5
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_5
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_5

  L      BG_step_axis_5
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_6
  L      O_unclamping_6
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_6
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_6
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_6

  L      BG_step_axis_6
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_7
  L      O_unclamping_7
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_7
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_7
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_7

  L      BG_step_axis_7
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_8
  L      O_unclamping_8
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_8
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_8
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_8

  L      BG_step_axis_8
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_9
  L      O_unclamping_9
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_9
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_9
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_9

  L      BG_step_axis_9
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_10
  L      O_unclamping_10
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_10
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_10
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_10

  L      BG_step_axis_10
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_11
  L      O_unclamping_11
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_11
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_11
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_11

  L      BG_step_axis_11
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_12
  L      O_unclamping_12
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_12
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_12
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_12

  L      BG_step_axis_12
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_13
  L      O_unclamping_13
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_13
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_13
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_13

  L      BG_step_axis_13
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_14
  L      O_unclamping_14
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_14
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_14
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_14

  L      BG_step_axis_14
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_15
  L      O_unclamping_15
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_15
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_15
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_15

  L      BG_step_axis_15
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_16
  L      O_unclamping_16
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_16
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_16
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_16

  L      BG_step_axis_16
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_17
  L      O_unclamping_17
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_17
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_17
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_17

  L      BG_step_axis_17
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_1 = &axis_18
  L      O_unclamping_18
  =      MG_3D_head_axis_1_output_unclamping

  L      I_unclamped_axis_18
  =      MG_3D_head_axis_1_unclamped

  L      I_clamped_axis_18
  =      MG_3D_head_axis_1_clamped

  L      MG_3D_head_axis_1_service_unclamping
  =      MG_3D_head_unclamp_18

  L      BG_step_axis_18
  =      BG_step_axis_1_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_4
  L      O_unclamping_4
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_4
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_4
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_4

  L      BG_step_axis_4
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_5
  L      O_unclamping_5
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_5
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_5
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_5

  L      BG_step_axis_5
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_6
  L      O_unclamping_6
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_6
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_6
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_6

  L      BG_step_axis_6
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_7
  L      O_unclamping_7
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_7
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_7
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_7

  L      BG_step_axis_7
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_8
  L      O_unclamping_8
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_8
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_8
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_8

  L      BG_step_axis_8
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_9
  L      O_unclamping_9
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_9
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_9
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_9

  L      BG_step_axis_9
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_10
  L      O_unclamping_10
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_10
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_10
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_10

  L      BG_step_axis_10
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_11
  L      O_unclamping_11
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_11
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_11
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_11

  L      BG_step_axis_11
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_12
  L      O_unclamping_12
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_12
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_12
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_12

  L      BG_step_axis_12
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_13
  L      O_unclamping_13
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_13
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_13
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_13

  L      BG_step_axis_13
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_14
  L      O_unclamping_14
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_14
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_14
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_14

  L      BG_step_axis_14
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_15
  L      O_unclamping_15
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_15
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_15
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_15

  L      BG_step_axis_15
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_16
  L      O_unclamping_16
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_16
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_16
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_16

  L      BG_step_axis_16
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_17
  L      O_unclamping_17
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_17
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_17
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_17

  L      BG_step_axis_17
  =      BG_step_axis_2_3D_head
#endif

#if KG_3D_head_axis_2 = &axis_18
  L      O_unclamping_18
  =      MG_3D_head_axis_2_output_unclamping

  L      I_unclamped_axis_18
  =      MG_3D_head_axis_2_unclamped

  L      I_clamped_axis_18
  =      MG_3D_head_axis_2_clamped

  L      MG_3D_head_axis_2_service_unclamping
  =      MG_3D_head_unclamp_18

  L      BG_step_axis_18
  =      BG_step_axis_2_3D_head
#endif

EM

;----------------------------
LBL Save_axis_position

  L      BL_active_3D_head_axis
  =X
  ==     KG_3D_head_axis_1
  IFT
    L      DG_position_axis_X_ref[X]
    =      DG_3D_head_axis_1_position
  ELSE
    L      DG_position_axis_X_ref[X]
    =      DG_3D_head_axis_2_position
  ENDI

#if KG_3D_head_axis_2 >= 0
  LN     DG_controlled_axes
  BT     KG_3D_head_axis_2
  IFT
    L      K+0
    =      DG_3D_head_axis_2_position
  ENDI
#endif

EM

;----------------------------
LBL Save_spindle_position

  L      MG_marker_one
  R      ML_selection_head_position

  L      KG_3D_head_axis_1
  =X

  L      DG_hirth_axes
  BTX
  IFT
    L      DG_MP430_hirth_increm_X[X]
    /      K+2
    +      K+3600000
    +      DG_position_axis_X_ref[X]
    /      DG_MP430_hirth_increm_X[X]
    X      DG_MP430_hirth_increm_X[X]
    MOD    K+3600000
    =      DG_3D_head_axis_1_S_position
  ELSE
    L      DG_position_axis_S_actual
    =      DG_3D_head_axis_1_S_position
    =      DG_3D_head_axis_2_S_position
  ENDI

#if KG_3D_head_axis_2 >= 0
  L      KG_3D_head_axis_2
  =X

  L      DG_hirth_axes
  BTX
  IFT
    L      DG_MP430_hirth_increm_X[X]
    /      K+2
    +      K+3600000
    +      DG_position_axis_X_ref[X]
    /      DG_MP430_hirth_increm_X[X]
    X      DG_MP430_hirth_increm_X[X]
    MOD    K+3600000
    =      DG_3D_head_axis_2_S_position
  ELSE
    L      DG_position_axis_S_actual
    =      DG_3D_head_axis_1_S_position
    =      DG_3D_head_axis_2_S_position
  ENDI

#endif

#if KG_3D_head_axis_2 >= 0
  LN     DG_controlled_axes
  BT     KG_3D_head_axis_2
  IFT
    L      K+0
    =      DG_3D_head_axis_2_S_position
  ENDI
#endif

* If counting direction of the spindle and the head axis do not
* correspond please use minus sign for DG_3D_head_axis_x_S_position.

  L      K+7200000
  +      DG_3D_head_axis_1_S_position
  +      DG_3D_head_axis_2_S_position
  MOD    K+3600000
  PSW
  +      DG_3D_head_offset_axis_1_S_pos
  =      DG_3D_head_axis_1_S_position

#if KG_3D_head_axis_2 >= 0
  PLW
  +      DG_3D_head_offset_axis_2_S_pos
  =      DG_3D_head_axis_2_S_position

  LN     DG_controlled_axes
  BT     KG_3D_head_axis_2
  IFT
    L      K+0
    =      DG_3D_head_axis_2_S_position
  ENDI
#else
  PL     WL_stack_synchronize
#endif

EM
