*-----------------------------------------------------------------------------
*  Spindle Orientation
*-----------------------------------------------------------------------------
*  Module Description
*
*-----------------------------------------------------------------------------

#define /s    KL_shortest_path                         K +0
#define /s    KL_direction_M03                         K +1
#define /s    KL_direction_M04                         K -1
#define /s    KL_offset_CYCL13                         K +3
#define /s    KL_0_rpm_from_MP3520_1                   K +0
#define /s    KL_angle_0_degree                        K +0
#define /s    KL_spindle_360_degree                    K +3600000

#define /s    ML_M04_has_been_active                   M
#define /s    ML_TC_pos_spindle_initialized            M

#define /s    BL_case_orientation                      B

#define /s    DL_offset_modulo_range                   D
#define /s    DL_stack_synchronize                     D

#include Mac_Lib.inc

EXTERN Absolute

GLOBAL Spindle_Orient

;----------------------------
LBL Spindle_Orient
;----------------------------

  LN     MG_machine_parameter_are_read
  R      ML_TC_pos_spindle_initialized

  L      MG_machine_parameter_are_read
  AN     ML_TC_pos_spindle_initialized
  IFT
    S      ML_TC_pos_spindle_initialized

    L      DG_TC_pos_spindle
    +      DG_pos_window_S
    MOD    K+3600000
    <=[
      L      DG_pos_window_S
      X      K+2
    ]
    IFT
      L      K+100000
      =      DL_offset_modulo_range
    ELSE
      L      K+0
      =      DL_offset_modulo_range
    ENDI
  ENDI

  L      MG_TC_help
  O      MG_TC_automatic
  RN     MG_spindle_in_TC_position
  IFT
    L      DG_TC_pos_spindle
    +      DL_offset_modulo_range
    MOD    K+3600000
    -[
      L      DG_position_axis_S_actual
      +      DL_offset_modulo_range
      MOD    K+3600000
    ]
    PSW
    CM     Absolute
    PLW
    <      DG_pos_window_S
    A      NP_M4000_S_in_position
    AN     NP_M4018_S_no_reference
    A      MG_spindle_stand_still
    =      MG_spindle_in_TC_position
  ENDI

  CASE   BL_case_orientation
    CM     Orient_on                   ;00
    CM     Drive_control_on            ;01
    CM     Orient_Start                ;02
    CM     S_in_position               ;03
    CM     M_quit                      ;04
  ENDC

  PLL
  IFT
    INC    BL_case_orientation
  ENDI

  L      PN_M4005_S_M03_nominal_and_status
  R      ML_M04_has_been_active

  L      PN_M4006_S_M04_nominal_and_status
  S      ML_M04_has_been_active

  LN     MG_spindle_off_orientation
  AN     MG_spindle_orientation_active
  EMT

  L      PN_M4005_S_M03_nominal_and_status
  O      PN_M4006_S_M04_nominal_and_status
  AN     PN_M4008_S_nominal_off_status_inverse
  O[
    L      PN_M4093_quit_TOOL_CALL
    AN     NP_M4520_TOOL_CALL_2nd_strobe
  ]
  O[
    L      MG_key_spindle_M5
    A      MG_manual_mode
  ]
  O      MG_key_S_jog_right_cw
  O      MG_key_S_jog_left_ccw
  O      MG_M05_spindle_off
  O      MG_spindle_off_orientation
  O      MG_auto_manu_mode_change
  O      MG_pulse_internal_stop
  O      MG_pulse_END_PGM_M02_M30
  O      NP_M4158_mode_block_scan
  O      NP_M4070_strobe_G_code
  O      MG_M41_spindle_1
  O      MG_M42_spindle_2
  ON     I_ESB_servo_drive_release
  ON     MG_servo_drive_release_spindle
  ON     MG_control_operational
  S      PN_M4012_S_open_control_loop
  R      MG_spindle_off_orientation
  R      MG_spindle_orientation_active
  CMT    Orient_reset

EM

;----------------------------
LBL Orient_on                          ;00

  L      MG_M19_spindle_orientation_0_degree
  O      MG_M20_spindle_orientation_CYCL_13
  O      MG_spindle_on_orientation
  O      MG_TC_spindle_orientation_on
  AN     MG_3D_head_active
  AN     MG_spindle_C_axis_mode_active
  O      MG_C_axis_spindle_orientation_on
  O      MG_3D_head_spindle_orientation_on
  A      MG_spindle_with_ROD
  A      MG_gear_range_ok
  AN     PN_error_reference_necessary
  PSL

EM

;----------------------------
LBL Drive_control_on                   ;01

  L      MG_marker_one
  S      MG_spindle_orientation_active

  L      NP_M4018_S_no_reference
  A      MG_spindle_reference_using_sensor
  S      MG_spindle_reference_active

  L      NP_M4018_S_no_reference
  AN     MG_spindle_reference_using_sensor
  =      PN_M4011_S_speed_from_MP3520_0

  L      MG_spindle_digital
  IFT
    L      DG_servo_drives_ready
    BT     KG_spindle
    AN     NP_M4018_S_no_reference
    PSL
  ELSE
    LN     TR_servo_drive_release_S
    AN     NP_M4018_S_no_reference
    PSL
  ENDI

EM

;----------------------------
LBL Orient_Start                       ;02

  L      KL_angle_0_degree
  =      PN_D760_add_TS_centre_offset_3D_ROT
  PSW

#if CFG_3D_head = "active"
  L      DG_controlled_axes
  BT     KG_3D_head_axis_1
  IFT
    PL     DL_stack_synchronize

    L      K+3600000
    -      DG_3D_head_axis_1_position
*$WS /     transmission ratio MP1054
#if KG_3D_head_axis_2 >= 0
    +      DG_3D_head_axis_2_position
#endif
    MOD    K+3600000
    =      PN_D760_add_TS_centre_offset_3D_ROT
    PSW
  ENDI

  L      MG_3D_head_spindle_orientation_on
  IFT
    PL     DL_stack_synchronize

    L      DG_3D_head_S_position
    <      K+0
    IFT
      L      K+3600000
      +      DG_3D_head_S_position
      =      DG_3D_head_S_position
    ENDI

    PS     DG_3D_head_S_position
  ENDI
#endif

#if CFG_S_in_axes_mode = "active"
  L      MG_C_axis_spindle_orientation_on
  IFT
    PL     DL_stack_synchronize
    PS     DG_C_axis_S_position
  ENDI
#endif

  L      MG_TC_spindle_orientation_on
  IFT
    PL     DL_stack_synchronize
    PS     DG_TC_pos_spindle
  ENDI

  PS     KL_0_rpm_from_MP3520_1

  L      MG_touch_probe_active
  O      MG_3D_head_spindle_orientation_on
  IFT
    PS     KL_shortest_path
  ELSE
    L      ML_M04_has_been_active
    IFT
      PS     KL_direction_M04
    ELSE
      PS     KL_direction_M03
    ENDI
  ENDI

  L      MG_M20_spindle_orientation_CYCL_13
  IFT
    PLW
    +      KL_offset_CYCL13
    PSW
  ENDI

  CM     9171
  M_display_module_error

  LN     NP_M4203_error_module_9xxx
  PSL

EM

;----------------------------
LBL S_in_position                      ;03

  L      MG_control_in_operation_on
  AN     MG_NC_stop_active
  O      MG_manual_mode
  A      NP_M4000_S_in_position
  PSL

EM

;----------------------------
LBL M_quit                             ;04

  PS     MG_marker_zero

  LN     NP_M4072_strobe_M_function
  O      MG_3D_head_spindle_orientation_on
  O      MG_C_axis_spindle_orientation_on
  JPT    Orient_reset

  L      MG_M19_spindle_orientation_0_degree
  O      MG_M20_spindle_orientation_CYCL_13
  S      PN_M4092_quit_M_function

EM

;----------------------------
LBL Orient_reset

  L      MG_marker_one
  R      MG_spindle_on_orientation
  R      MG_TC_spindle_orientation_on
  R      MG_3D_head_spindle_orientation_on
  R      MG_C_axis_spindle_orientation_on
  R      PN_M4011_S_speed_from_MP3520_0

  L      K+0
  =      BL_case_orientation

EM
