o<m22> sub

; Move Carousel to the home position - IN
; after loading any tool in the current pocket to the spindle
; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
(PRINT, o<m22>)

#<atc_z_tool_change_height> = -3.9000
o100 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
    #<atc_z_tool_change_height> = #<_ini[atc]z_tool_change_height>
o100 endif
#<atc_z_tool_clearance_height> = [#<_ini[AXIS_Z]MAX_LIMIT>-0.01]
o110 if [EXISTS[#<_ini[atc]z_tool_clearance_height>]]
    #<atc_z_tool_clearance_height> = #<_ini[atc]z_tool_clearance_height>
o110 endif

;M19 R0 Q2
M24
;M19 R0 Q1
;G90
G0 G53 Z#<atc_z_tool_change_height> ; rapid move to above the tool change height
M65 P2 ; release the drawbar to clamp the tool
M5
M66 P5 L3 Q1 ; check the tool clamped sensor
o120 if [#5399 LT 0]
    (abort, Failed to reclamp tool)
o120 endif

M65 P0 ; Move Carousel home
M66 P0 L3 Q4 ; check carousel in position sensor
o130 if [#5399 LT 0]
    M65 P1 ; turn off the solenoid to send atc home
    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
o130 endif

;M65 P1 ; Turn off carousel home position solenoid

(PRINT, o<m22> endsub)
o<m22> endsub [1]

M2