*-----------------------------------------------------------------------------
*  TC Magazine controlled by PLC axis
*-----------------------------------------------------------------------------
*  Module Description
*    PLC status (tool table):
*    #Bit 1 (%00000010) - oversized tool (no pocket out after TOOL DEF)
*                       - (no pocket out after TOOL DEF)
*-----------------------------------------------------------------------------

#define /s    KL_absolute                              K +0
#define /s    KL_TC_mag_axis_inactive                  K -1
#define /s    KL_string_count                          K +3

#define /s    ML_TC_mag_diagnosis                      M
#define /s    ML_debug_TC_mag                          M

#define /s    BL_WZ_mag_index                          B
#define /s    BL_TC_mag_axis_index                     B
#define /s    BL_string_index                          B

#define /s    WL_line_dialog_file                      W
#define /s    WL_stack_synchronize                     W

EXTERN Absolute

GLOBAL TC_mag_cw_turn
GLOBAL TC_mag_ccw_turn
GLOBAL Tool_magazine

#include Mac_Lib.inc

;----------------------------
LBL Tool_magazine
;----------------------------

  LN     MG_TC_auto_active
  EMT

  CM     TC_mag_n_read_input

  LN     NP_M4074_strobe_TOOL_DEF
  R      PN_M4094_quit_TOOL_DEF

  L      K+0
  =      BL_WZ_mag_index

  L      MG_marker_one
  R      MG_TC_mag_n_posit_active
  R      MG_TC_magazine_n_start_active
  R      TS_TC_magazine_timeout

  REPEAT
    L      STG_TC_mag[BL_WZ_mag_index].BG_TC_magazine_axis_index
    =      BL_TC_mag_axis_index
    <>     KL_TC_mag_axis_inactive
    IFT
      CM     Status_axis

      LN     STG_TC_mag[BL_WZ_mag_index].MG_status[KG_reference_ok]
      S      STG_TC_mag[BL_WZ_mag_index].MG_posit_0_degree

      L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_reference_ok]
      AN     STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
      A      STG_TC_mag[BL_WZ_mag_index].MG_posit_0_degree
      R      STG_TC_mag[BL_WZ_mag_index].MG_posit_0_degree
      IFT
        L      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_pocket_distance
        =      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position

        CM     TC_mag_start
      ENDI

      LN     STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
      A      STG_TC_mag[BL_WZ_mag_index].ML_posit_has_been_active_Tmag
      CMT    Decode_P_number

      L      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_actual_pocket
      ==     K+0
      A      MG_power_delayed_on
      CMT    Decode_P_number
      S      MG_standby_tool_status_reading

      L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
      =      STG_TC_mag[BL_WZ_mag_index].ML_posit_has_been_active_Tmag

      L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_auxiliary_axis]
      A      MG_commissioning
      CMT    Override

      LN     MG_control_operational
      CMT    TC_mag_stop

      L      BG_TC_mag_index_Tool_Call
      ==     BL_WZ_mag_index
      A      NP_M4074_strobe_TOOL_DEF
      AN     STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
      AN     MG_SK_TC_mag_load_active
      AN     STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Def
      S      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Def
      IFT
        L      NP_W262_tool_pocket_number
        =      STG_TC_mag[BL_WZ_mag_index].WL_Tool_Def_pocket_number
        L      NP_W264_tool_number
        =      STG_TC_mag[BL_WZ_mag_index].WL_Tool_Def_tool_number
      ENDI

      L      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_start
      R      TS_TC_magazine_timeout

      CASE   STG_TC_mag[BL_WZ_mag_index].BG_step_TC_magazine
        CM     Magazine_start          ;00
        CM     Tool_def_quit           ;01
        CM     Pocket_put_in           ;02
        CM     Pocket_pos_in           ;03
        CM     T_magazine_start        ;04
        CM     T_mag_in_position       ;05
        CM     Decode_P_no             ;06
        CM     Standby_T_read          ;07
        CM     Pocket_in_out           ;08
        CM     Pocket_posit            ;09
        CM     Magazine_end            ;10
      ENDC

      PLL
      AN     STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
      S      TS_TC_magazine_timeout
      =      STG_TC_mag[BL_WZ_mag_index].ML_step_start
      IFT
        INC    STG_TC_mag[BL_WZ_mag_index].BG_step_TC_magazine
      ENDI

      L      STG_TC_mag[BL_WZ_mag_index].BG_step_TC_magazine
      <>     STG_TC_mag[BL_WZ_mag_index].BL_step_TC_magazine
      ON     ML_TC_mag_diagnosis
      A      MG_TC_magazine_diagnosis
      IFT
        L      K +0
        =      BL_string_index

        L      STG_TC_mag[BL_WZ_mag_index].BG_step_TC_magazine
        =      STG_TC_mag[BL_WZ_mag_index].BL_step_TC_magazine
        REPEAT
          L      STG_TC_mag[BL_WZ_mag_index].BL_step_TC_magazine
          =X

          L      KF Diagnostic[X]
          =      WL_line_dialog_file
          =X

          L      S#D0[X]
          =      SG_diagnostics_line_1[BL_string_index]

          INC    STG_TC_mag[BL_WZ_mag_index].BL_step_TC_magazine
          INC    BL_string_index
          INCX

          L      BL_string_index
          >=     KL_string_count
        UNTILT
      ENDI

      L      STG_TC_mag[BL_WZ_mag_index].BG_step_TC_magazine
      =      STG_TC_mag[BL_WZ_mag_index].BL_step_TC_magazine

      L      MG_TC_magazine_diagnosis
      =      ML_TC_mag_diagnosis

      L      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_start
      S      MG_TC_magazine_n_start_active
      IFT
        L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_error]
        =      PN_error_positioning_TC_magacine

        L      MG_key_NC_start_p_pulse
        R      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption

        L      MG_key_NC_start
        O      MG_pulse_CE_key
        S      TS_TC_magazine_timeout

        LN     TS_TC_magazine_timeout
        AN     TR_TC_magazine_timeout
        =      PN_error_TC_magazine_timeout
        S      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption

        L      MG_pulse_external_stop
        AN     NP_M4157_mode_restore_position
        O      MG_key_NC_stop
        S      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption

        L      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
        O      MG_pulse_END_PGM_M02_M30
        O      MG_guard_open
        R      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Def

        L      MG_pulse_internal_stop
        O      MG_pulse_TC_help
        ON     MG_control_operational
        CMT    TC_magazine_reset
      ENDI
    ENDI

    INC    BL_WZ_mag_index

    L      BL_WZ_mag_index
    >=     WG_TC_magazine_number
    O      ML_debug_TC_mag
  UNTILT

  CM     TC_mag_n_write_output

EM

;----------------------------
LBL Magazine_start                     ;00

  L      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Def
  O      STG_TC_mag[BL_WZ_mag_index].MG_WZ_mag_load_unload_start
  A      MG_TC_standby_position
  AN     MG_TC_automatic
  O      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Call
  O      STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  S      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_start

  L      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_start
  PSL
  EMF

  L      STG_TC_mag[BL_WZ_mag_index].MG_WZ_mag_load_unload_start
  IFT
    L      STG_TC_mag[BL_WZ_mag_index].WG_TC_mag_load_nominal_pocket
    =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  ENDI

  L      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Def
  AN     STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Call
  IFT
    L      STG_TC_mag[BL_WZ_mag_index].WL_Tool_Def_pocket_number
    =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  ENDI

  L      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Call
  R      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Call
  IFT
    L      WG_TC_pocket_number
    =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  ENDI

  L      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  X      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_pocket_distance
  =      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position

EM

;----------------------------
LBL Tool_def_quit                      ;01

  L      NP_M4074_strobe_TOOL_DEF
  S      PN_M4094_quit_TOOL_DEF

  LN     NP_M4074_strobe_TOOL_DEF
  AN     STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
  R      STG_TC_mag[BL_WZ_mag_index].MG_TM_started_by_Tool_Def
  PSL

EM

;----------------------------
LBL Pocket_put_in                      ;02

  L      MG_marker_one
  S      STG_TC_mag[BL_WZ_mag_index].MG_O_TC_pocket_put_in
  R      STG_TC_mag[BL_WZ_mag_index].MG_O_TC_pocket_put_out
  PSL

EM

;----------------------------
LBL Pocket_pos_in                      ;03

#if TC_pocket_90_degree_shift = &active
  LN     STG_TC_mag[BL_WZ_mag_index].MG_I_TC_pocket_out
  A      STG_TC_mag[BL_WZ_mag_index].MG_I_TC_pocket_in
  AN     STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
  PSL

  L      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
  =      MG_win_TC_pocket_put_in
#else
  PS     MG_marker_one
#endif

EM

;----------------------------
LBL T_magazine_start                   ;04

  PS     MG_marker_one
  CM     TC_mag_start

EM

;----------------------------
LBL T_mag_in_position                  ;05

  L      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
  IFT
    L      DG_feed_stop_PLC_axes
    BS     BL_TC_mag_axis_index
    =      DG_feed_stop_PLC_axes
  ELSE
    L      DG_feed_stop_PLC_axes
    BC     BL_TC_mag_axis_index
    =      DG_feed_stop_PLC_axes
  ENDI

  L      STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  AN     STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
  CMT    Magazine_start
  S      TS_TC_magazine_timeout
  R      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption

  L      STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  R      STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual

  L      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position
  -      DG_position_axis_X_ref[BL_TC_mag_axis_index]
  PSW
  CM     Absolute
  PLW
  <      DG_MP1030_pos_window_X[BL_TC_mag_axis_index]
  O[
    L      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position
    ==     K+0
    A[
      L      DG_MP810_modulo_value_X[BL_TC_mag_axis_index]
      -      DG_position_axis_X_ref[BL_TC_mag_axis_index]
      <      DG_MP1030_pos_window_X[BL_TC_mag_axis_index]
    ]
  ]
  AN     STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
  AN     PN_error_TC_magazine_timeout
  PSL

  L      KG_axis_brake_active
  A      DG_activate_control_loop_open
  A      NP_D1100_axes_release
  BTX
  IFT
    PLL
    A      MG_marker_zero
    PSL
  ENDI

EM

;----------------------------
LBL Decode_P_no                        ;06

  PS     MG_marker_one

  CM     Decode_P_number

EM

;----------------------------
LBL Standby_T_read                     ;07

  L      MG_marker_one
  S      MG_standby_tool_status_reading
  PSL

EM

;----------------------------
LBL Pocket_in_out                      ;08

  LN     MG_standby_tool_status_reading
  PSL
  EMF

#if TC_pocket_90_degree_shift = &active
  L      MG_manual_mode
  O      MG_SK_TC_mag_load_active
  EMT

  L      BG_standby_tool_status
  BT     K+1
  =      MG_tool_oversized

  LN     MG_tool_oversized
  R      STG_TC_mag[BL_WZ_mag_index].MG_O_TC_pocket_put_in
  S      STG_TC_mag[BL_WZ_mag_index].MG_O_TC_pocket_put_out
#endif

EM

;----------------------------
LBL Pocket_posit                       ;09

  L      MG_tool_oversized
  O      MG_manual_mode
  O      MG_SK_TC_mag_load_active
  CMT    Pocket_pos_in
  CMF    Pocket_pos_out

EM

;----------------------------
LBL Pocket_pos_out

#if TC_pocket_90_degree_shift = &active
  L      STG_TC_mag[BL_WZ_mag_index].MG_I_TC_pocket_out
  AN     STG_TC_mag[BL_WZ_mag_index].MG_I_TC_pocket_in
  AN     STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
  PSL

  L      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
  =      MG_win_TC_pocket_put_out
#else
  PS     MG_marker_one
#endif

EM

;----------------------------
LBL Magazine_end                       ;10

  PS     MG_marker_zero

LBL TC_magazine_reset

  L      MG_marker_one
  R      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_start
  R      STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  R      STG_TC_mag[BL_WZ_mag_index].MG_TC_mag_interruption
  R      STG_TC_mag[BL_WZ_mag_index].MG_WZ_mag_load_unload_start
  R      MG_SK_T_load_unload

  L      K+0
  =      STG_TC_mag[BL_WZ_mag_index].BG_step_TC_magazine

  L      DG_feed_stop_PLC_axes
  BC     BL_TC_mag_axis_index
  =      DG_feed_stop_PLC_axes

  CM     TC_mag_stop

  L      S"            "
  =      SG_diagnostics_line_1
  =      SG_diagnostics_line_2
  =      SG_diagnostics_line_3

EM

;----------------------------
LBL Override

  PS     STG_TC_mag[BL_WZ_mag_index].BG_TC_magazine_axis_index

  L      NP_W496_rapid_traverse_override_NC
  <      K+10000
  IFT
    PS     NP_W496_rapid_traverse_override_NC
  ELSE
    PS     K+10000
  ENDI

  CM     9124
  PL     WL_stack_synchronize
  M_display_module_error

EM

;----------------------------
LBL TC_mag_cw_turn

  L      BG_TC_mag_index_Tool_Call
  =      BL_WZ_mag_index

  L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
  AN     STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  EMT

  LN     DG_keys_inhibit
  BT     BL_TC_mag_axis_index
  S      STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  EMF

  CM     Decode_P_number

  L      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_actual_pocket
  +      K+1
  MOD    STG_TC_mag[BL_WZ_mag_index].WG_MP7261_TC_magazine_max_pocket
  =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  <      K+1
  IFT
    L      STG_TC_mag[BL_WZ_mag_index].WG_MP7261_TC_magazine_max_pocket
    =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  ENDI

EM

;----------------------------
LBL TC_mag_ccw_turn

  L      BG_TC_mag_index_Tool_Call
  =      BL_WZ_mag_index

  L      STG_TC_mag[BL_WZ_mag_index].BG_TC_magazine_axis_index
  =      BL_TC_mag_axis_index

  L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
  AN     STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  EMT

  LN     DG_keys_inhibit
  BT     BL_TC_mag_axis_index
  S      STG_TC_mag[BL_WZ_mag_index].ML_TC_magazine_start_manual
  EMF

  CM     Decode_P_number

  L      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_actual_pocket
  -      K+1
  =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  <      K+1
  IFT
    L      STG_TC_mag[BL_WZ_mag_index].WG_MP7261_TC_magazine_max_pocket
    =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_nominal_pocket
  ENDI

EM

;----------------------------
LBL TC_mag_start

  L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
  ON     MG_power_delayed_on
  ON     MG_servo_drive_release_2nd_axes_group
  ON     I_drives_operational
  EMT

  PS     BL_TC_mag_axis_index

  L      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position
  >=     DG_MP810_modulo_value_X[BL_TC_mag_axis_index]
  IFT
    L      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position
    MOD    DG_MP810_modulo_value_X[BL_TC_mag_axis_index]
    =      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position
  ENDI

  PS     STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_nominal_position
  PS     DG_feed_PLC_X[BL_TC_mag_axis_index]
  PS     KL_absolute
  CM     9120
  PL     WL_stack_synchronize
  M_display_module_error

EM

;----------------------------
LBL TC_mag_stop

  LN     STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
  EMT

  PS     BL_TC_mag_axis_index
  CM     9121
  PL     WL_stack_synchronize
  M_display_module_error

EM

;----------------------------
LBL Status_axis

  PS     BL_TC_mag_axis_index
  CM     9122
  PLW
  B=     STG_TC_mag[BL_WZ_mag_index].MG_status[KG_auxiliary_axis]
  M_display_module_error

  L      STG_TC_mag[BL_WZ_mag_index].MG_status[KG_posit_active]
  S      MG_TC_mag_n_posit_active

EM

;----------------------------
LBL Decode_P_number

  L      STG_TC_mag[BL_WZ_mag_index].BG_TC_magazine_axis_index
  =      BL_TC_mag_axis_index

  L      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_pocket_distance
  /      K+2
  +      DG_position_axis_X_ref[BL_TC_mag_axis_index]
  /      STG_TC_mag[BL_WZ_mag_index].DG_TC_magazine_pocket_distance
  =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_actual_pocket
  ==     K+0
  IFT
    L      STG_TC_mag[BL_WZ_mag_index].WG_MP7261_TC_magazine_max_pocket
    =      STG_TC_mag[BL_WZ_mag_index].WG_TC_magazine_actual_pocket
  ENDI

EM

;----------------------------
LBL TC_mag_n_read_input

#if CFG_Simulation = "inactive"
  L      I_TC_mag_1_pocket_out
  =      STG_TC_mag[KG_TC_magazine_1_index].MG_I_TC_pocket_out

  L      I_TC_mag_1_pocket_in
  =      STG_TC_mag[KG_TC_magazine_1_index].MG_I_TC_pocket_in

  L      I_TC_mag_1_pocket_not_empty
  =      STG_TC_mag[BG_TC_mag_index_Tool_Call].MG_I_TC_pocket_not_empty

  L      I_TC_mag_2_pocket_out
  =      STG_TC_mag[KG_TC_magazine_2_index].MG_I_TC_pocket_out

  L      I_TC_mag_2_pocket_in
  =      STG_TC_mag[KG_TC_magazine_2_index].MG_I_TC_pocket_in

  L      I_TC_mag_2_pocket_not_empty
  =      STG_TC_mag[BG_TC_mag_index_Tool_Call].MG_I_TC_pocket_not_empty
#endif

EM

;----------------------------
LBL TC_mag_n_write_output

  L      STG_TC_mag[KG_TC_magazine_1_index].MG_O_TC_pocket_put_out
  =      O_TC_mag_1_pocket_put_out

  L      STG_TC_mag[KG_TC_magazine_1_index].MG_O_TC_pocket_put_in
  =      O_TC_mag_1_pocket_put_in

  L      STG_TC_mag[KG_TC_magazine_2_index].MG_O_TC_pocket_put_out
  =      O_TC_mag_2_pocket_put_out

  L      STG_TC_mag[KG_TC_magazine_2_index].MG_O_TC_pocket_put_in
  =      O_TC_mag_2_pocket_put_in

EM

KFIELD Diagnostic
  KG_dia_TC_magazine_PLC_axis          ;-1
  KG_dia_magazine_start                ;00
  KG_dia_Tool_Def_quit                 ;01
  KG_dia_pocket_put_in                 ;02
  KG_dia_pocket_pos_in                 ;03
  KG_dia_T_magazine_start              ;04
  KG_dia_T_mag_in_position             ;05
  KG_dia_decode_P_no                   ;06
  KG_dia_standby_T_read                ;07
  KG_dia_pocket_in_out                 ;08
  KG_dia_pocket_posit                  ;09
  KG_dia_magazine_end                  ;10
  KG_dia_empty                         ;11
ENDK

