*-----------------------------------------------------------------------------
*  Gear shift spindle
*-----------------------------------------------------------------------------
*  Module description
*
*  - Preselection of the last active spindle speed after main switch on
*
*  - conventional machanical shift of 2 gear speeds (expandable)
*
*  - serveral labels also used by module Gear_YD.src
*
*  - Spindle switchover S1/S2 integrated
*
*  - Input-/Output interface
*      I_S1_gear_range_1
*      I_S1_gear_range_2
*      I_S2_gear_range_1
*      I_S2_gear_range_2
*      O_S1_gear_range_1
*      O_S1_gear_range_2
*      O_S1_wye_delta_switchover
*      O_S2_gear_range_1
*      O_S2_gear_range_2
*      O_S2_wye_delta_switchover
*
*-----------------------------------------------------------------------------

#define /s    KL_gear_range_1                          K +0
#define /s    KL_gear_range_2                          K +1

#define /s    KL_end_step_gear_rot                     K -1

#define /s    ML_gear_shift_ok                         M
#define /s    ML_S_0_volt_status_inverse               M

#define /s /c TS_timer_0                               T0

#define /s    BL_step_gear                             B
#define /s    BL_step_timer_gear_rot                   B
#define /s    BL_index_timer_gear_rot                  B

#define /s /r BL_G_code                                B [2]
#define /c    BL_S1_G_code                             B &BL_G_code + 0
#define /c    BL_S2_G_code                             B &BL_G_code + 1

#define /s /r DL_S_programmed_rpm                      D [2]
#define /c    DL_S1_N_programmed_speed                 D &DL_S_programmed_rpm + 0
#define /c    DL_S2_N_programmed_speed                 D &DL_S_programmed_rpm + 4

#include Mac_Lib.inc

GLOBAL Gear_initialize
GLOBAL Backup_G_S
GLOBAL Sn_Input_read
GLOBAL Sn_Output_write
GLOBAL Gear_on
GLOBAL Test_gear_sensors
GLOBAL Spindle_off
GLOBAL Quit

GLOBAL Spindle_gear

;----------------------------
LBL Spindle_gear
;----------------------------

  L      NP_M4172_1st_PLC_run_after_power_on
  O      NP_M4173_1st_PLC_run_after_compile
  S      MG_gear_initialize

  LN     MG_spindle_active
  EMT

  CM     Sn_Input_read

  L      MG_gear_initialize
  A      MG_machine_parameter_are_read
  A      MG_power_delayed_on
  A[
    LN     NP_M4176_control_in_operation_on_or_flashes
    O      MG_M41_spindle_1
    O      MG_M42_spindle_2
    O      MG_M52_AB_head_activate
    O      MG_M53_AB_head_C_table_activate
    O      MG_M54_B_head_C_table_activate
    O      MG_M55_AC_table_1_activate
    O      MG_M56_AC_table_2_activate
    O      MG_M57_AC_fork_head_activate
    O      MG_M58_kinematic_inactive
  ]
  CMT    Gear_initialize

  L      PN_D756_S_nominal_rpm_PLC
  <>     DG_S_nominal_rpm_PLC
  CMT    Backup_G_S

  L      PN_D756_S_nominal_rpm_PLC
  =      DG_S_nominal_rpm_PLC

  LN     NP_M4070_strobe_G_code
  R      PN_M4090_quit_G_code

  CASE   BL_step_gear
    CM     Gear_on                     ;00
    CM     Spindle_off                 ;01
    CM     Coupling                    ;02
    CM     Reference_test              ;03
    CM     Quit                        ;04
    CM     Gear_end                    ;05
  ENDC

  PLL
  IFT
    INC    BL_step_gear
  ENDI

  LN     MG_gear_change_active
  AN     MG_YD_switch_rpm_dependend
  =      TS_gear_timeout

  L      PN_error_gear_change
  ON     MG_control_operational
  CMT    Gear_reset

  CM     Sn_Output_write

  LN     MG_spindle_with_gear
  S      MG_gear_range_ok
  EMT

  CM     Test_gear_sensors

EM

;----------------------------
LBL Gear_on                            ;00

  L      NP_M4070_strobe_G_code
;  O      PN_M4134_strobe_G_range_S_speed
  AN     MG_3D_head_active
  AN     MG_spindle_C_axis_mode_active
  O      MG_gear_range_1_from_3D_head
  O      MG_gear_range_from_3D_head
  AN     NP_M4158_mode_block_scan
  PSL
  S      MG_gear_change_active
  EMF

  L      PN_M4008_S_nominal_off_status_inverse
  =      ML_S_0_volt_status_inverse

  L      MG_spindle_reference_using_sensor
  S      PN_M4015_S_reference_reset

EM

;----------------------------
LBL Spindle_off                        ;01

  LN     TR_gear_timeout
  S      PN_error_S_actual_lower_nominal_rpm

  PS     MG_spindle_stand_still

EM

;----------------------------
LBL Coupling                           ;02

  CM     gear_range

  L      ML_gear_shift_ok
  A      NP_M4002_S_nominal_0
  R      PN_M4015_S_reference_reset
  PSL
  EMF

  L      MG_spindle_reference_using_sensor
  S      PN_M4134_strobe_G_range_S_speed
  S      MG_spindle_reference_active

EM

;----------------------------
LBL Reference_test                     ;03

  LN     MG_spindle_reference_active
  PSL

EM

;----------------------------
LBL Quit                               ;04

  L      NP_M4070_strobe_G_code
  S      PN_M4090_quit_G_code

  LN     NP_M4070_strobe_G_code
  PSL
  EMF

  LN     ML_S_0_volt_status_inverse
  S      MG_spindle_on_after_gear_change

EM

;----------------------------
LBL Gear_end                           ;05

  PS     MG_marker_zero

LBL Gear_reset

  L      MG_marker_one
  R      MG_gear_change_active
  R      MG_spindle_on_after_gear_change
  R      MG_gear_range_1_from_3D_head
  R      MG_gear_range_from_3D_head
  R      ML_gear_shift_ok
  R      PN_M4010_S_rotation_right
  R      PN_M4009_S_rotation_left
  R      PN_M4015_S_reference_reset
  R      MG_YD_switch_rpm_dependend
  R      MG_Sn_O_YD_wye_delta_switchover
  R      MG_Sn_O_gear_range_1
  R      MG_Sn_O_gear_range_2
  R      TS_gear_rotation_pause
  R      TR_gear_rotation_pause
  R      TS_gear_rotation_right
  R      TR_gear_rotation_right
  R      TS_gear_rotation_left
  R      TR_gear_rotation_left

  L      K+0
  =      BL_step_gear
  =      BL_step_timer_gear_rot

EM

;----------------------------
LBL gear_range

  LN     MG_spindle_with_gear
  O      ML_gear_shift_ok
  EMT

  L      BL_step_timer_gear_rot
  =X

  L      KF Timer_gear_rotation[X]
  =      BL_index_timer_gear_rot
  ==     KL_end_step_gear_rot
  IFT
    L      K+0
    =      BL_step_timer_gear_rot
    =X

    L      KF Timer_gear_rotation[X]
    =      BL_index_timer_gear_rot
  ENDI

  L      TS_timer_0[BL_index_timer_gear_rot]
  IFT
    R      TS_timer_0[BL_index_timer_gear_rot]

    INC    BL_step_timer_gear_rot
  ENDI

  LN     TR_gear_rotation_pause
  AN     TR_gear_rotation_right
  AN     TR_gear_rotation_left
  =      TS_timer_0[BL_index_timer_gear_rot]

  L      TR_gear_rotation_right
  =      PN_M4010_S_rotation_right

  L      TR_gear_rotation_left
  =      PN_M4009_S_rotation_left

  L      MG_gear_range_1_from_3D_head
  IFT
    CM     Gear_range_1                ;+00
  ELSE
    CASE   NP_W256_G_code_spindle
      CM     Gear_range_1              ;+00
      CM     Gear_range_2              ;+01
    ENDC
  ENDI

  L      ML_gear_shift_ok
  R      PN_M4010_S_rotation_right
  R      PN_M4009_S_rotation_left

EM

;----------------------------
LBL Gear_range_1                       ;+00

  LN     MG_Sn_I_gear_range_1
  O      MG_Sn_I_gear_range_2
  =      TS_gear_delayed_stop

  L      TS_gear_delayed_stop
  O      TR_gear_delayed_stop
  =      MG_Sn_O_gear_range_1
  SN     ML_gear_shift_ok

EM

;----------------------------
LBL Gear_range_2                       ;+01

  L      MG_Sn_I_gear_range_1
  ON     MG_Sn_I_gear_range_2
  =      TS_gear_delayed_stop

  L      TS_gear_delayed_stop
  O      TR_gear_delayed_stop
  =      MG_Sn_O_gear_range_2
  SN     ML_gear_shift_ok

EM

;----------------------------
LBL Test_gear_sensors

  L      MG_marker_one
  R      MG_gear_range_ok

  L      NP_W256_G_code_spindle
  ==     KL_gear_range_1
  O      MG_3D_head_active
  A      MG_Sn_I_gear_range_1
  AN     MG_Sn_I_gear_range_2
  S      MG_gear_range_ok

  L      NP_W256_G_code_spindle
  ==     KL_gear_range_2
  AN     MG_Sn_I_gear_range_1
  A      MG_Sn_I_gear_range_2
  S      MG_gear_range_ok

  L      MG_spindle_1_active
  A      O_servo_drive_release_S1
  O[
    L      MG_spindle_2_active
    A      O_servo_drive_release_S2
  ]
  ON     PN_M4007_S_M05_nominal_0_and_status
  ON     PN_M4012_S_open_control_loop
  AN     MG_gear_range_ok
  AN     NP_M4070_strobe_G_code
  AN     MG_gear_change_active
  AN     MG_gear_initialize
  O[
    AN     TS_gear_timeout
    AN     TR_gear_timeout
  ]
  A      MG_power_delayed_on
  S      PN_error_gear_change

EM

;----------------------------
LBL Gear_initialize

  L      MG_marker_one
  R      MG_gear_initialize
  S      PN_M4134_strobe_G_range_S_speed

  L      BL_G_code[BG_number_active_spindle]
  =      NP_W256_G_code_spindle

  L      DL_S_programmed_rpm[BG_number_active_spindle]
  ==     K+0
  IFT
    L      KL_gear_range_1
    =      NP_W256_G_code_spindle

    L      DG_MP3520_1_rpm_S_orientate[BG_number_active_spindle]
    =      PN_D756_S_nominal_rpm_PLC
  ELSE
    L      DL_S_programmed_rpm[BG_number_active_spindle]
    =      PN_D756_S_nominal_rpm_PLC
    =      DG_S_nominal_rpm_PLC
  ENDI

EM

;----------------------------
LBL Backup_G_S

  L      MG_spindle_with_gear
  IFT
    L      NP_W256_G_code_spindle
    =      BL_G_code[BG_number_active_spindle]
  ELSE
    L      KL_gear_range_1
    =      BL_G_code[BG_number_active_spindle]
  ENDI

  L      PN_D756_S_nominal_rpm_PLC
  =      DL_S_programmed_rpm[BG_number_active_spindle]

EM

;----------------------------
LBL Sn_Input_read

  L      MG_spindle_1_active
  IFT
    L      I_S1_gear_range_1
    =      MG_Sn_I_gear_range_1
    L      I_S1_gear_range_2
    =      MG_Sn_I_gear_range_2
  ENDI

  L      MG_spindle_2_active
  IFT
    L      I_S2_gear_range_1
    =      MG_Sn_I_gear_range_1
    L      I_S2_gear_range_2
    =      MG_Sn_I_gear_range_2
  ENDI

EM

;----------------------------
LBL Sn_Output_write

  L      MG_spindle_1_active
  IFT
    L      MG_Sn_O_YD_wye_delta_switchover
    =      O_S1_wye_delta_switchover
    L      MG_Sn_O_gear_range_1
    =      O_S1_gear_range_1
    L      MG_Sn_O_gear_range_2
    =      O_S1_gear_range_2
  ENDI

  L      MG_spindle_2_active
  IFT
    L      MG_Sn_O_YD_wye_delta_switchover
    =      O_S2_wye_delta_switchover
    L      MG_Sn_O_gear_range_1
    =      O_S2_gear_range_1
    L      MG_Sn_O_gear_range_2
    =      O_S2_gear_range_2
  ENDI

EM

KFIELD Timer_gear_rotation
  K &TS_gear_rotation_pause            ;00
  K &TS_gear_rotation_right            ;01
  K &TS_gear_rotation_pause            ;02
  K &TS_gear_rotation_left             ;03
  KL_end_step_gear_rot                 ;04
ENDK

