o<toolchange> sub
;Main toolchange macro
(print, o<toolchange>)

;toolchange             Main toolchange macro
;M10                    Rotate to correct pocket, home if not homed
;M11                    Jog one pocket FWD
;M12                    Jog one pocket REV
;M13                    Home carousel
;M21                    Move tool from ATC to spindle (Orient, Lower head, extend ATC, activate PDB, raise head)
;M22                    Move tool from spindle to ATC (Orient, activate PDB, Lower head, deactivate PDB, retract ATC)
;M24                    Activate PDB
;M25                    Extend ATC
;load_spindle_safety    Set tool in spindle
;store_tool_in_carousel Remove tool from spindle, store in ATC

;pocket request         motion.analog-out-00
;carousel enable        motion.digital-out-00
;extend solenoid        motion.digital-out-01
;retract solenoid       motion.digital-out-02
;drawbar solenoid       motion.digital-out-03
;spindle lock solenoid  motion.digital-out-04
;carousel JOG FWD       motion.digital-out-05
;carousel JOG REV       motion.digital-out-06
;carousel-ready         motion.digital-in-00
;extend proxy           motion.digital-in-01
;retract proxy          motion.digital-in-02
;drawbar up proxy       motion.digital-in-03 (up is HIGH)
;spindle-enable         motion.digital-in-04
;spindle oriented       motion.digital-in-05
;spindle lock proxy     motion.digital-in-06
;carousel-homed         motion.digital-in-07

O001 if [#<_task> EQ 0]
  (debug, Task is null)
  o<toolchange> return [999]
O001 endif (this code eliminates tool missing error on load for multiple use tools)

;assign the tool numbers held in parameters to the pocket numbers
#1 = #5190
#2 = #5191
#3 = #5192
#4 = #5193
#5 = #5194
#6 = #5195
#7 = #5196
#8 = #5197
#9 = #5198
#10 = #5199
#11 = #5200
#12 = #5201
#13 = #5202
#14 = #5203
#15 = #5204
#16 = #5205
#17 = #5206
#18 = #5207

;assign the variables passed by M6 to some parameters
#100 = #<selected_tool>
#110 = #<tool_in_spindle>
#120 = #<selected_pocket>

O10 if [#100 EQ #110]                               ; Abort if tool in the spindle is same as requested
 o<toolchange> endsub [1]
M2
O10 endif

#<next_pocket> = 0                                  ; assigns 0 to the next pocket for a later check if the tool is found in the carousel
#<open_pocket> = 0

#15 = 18                                            ;assign test parameter the number of pockets in the carousel

O100 do
 	O110 if [#[5189 + #15] EQ #100]                   ; checks all pockets to see if it contains tool number requested as the new tool
		#<next_pocket> = #15                            ; if tool is found in pocket, assigns the next pocket
	O110 endif

	O115 if [#[5189 + #15] EQ 0]                      ; checks if the pocket is empty, last pocket checked will be the lowest empty pocket number, for putting tool in spindle away.
		#<open_pocket> = #15
	O115 endif
  #15= [#15 - 1]
O100 while [#15 GT 0]

O120 if [#<next_pocket> EQ 0]                       ; if tool is not found, aborts and sends a message
    (abort, tool not in carousel)
O120 endif

;now we know which pocket the next tool is sitting in
;we need to know if we need to put a tool away
;or if there is no tool in the spindle

; if tool in spindle is not 0, put it in ATC
O180 IF [#110 GT 0]                                 ; checks if there is a valid tool in the spindle
  O190 if [#<open_pocket> EQ 0]                     ; If there is a tool in the spindle, checks if there is an open pocket
	(abort, carousel is full)
	O190 endif

	M10 P[#<open_pocket>]                             ; move carousel to an open pocket
  M21                                               ; puts the tool in spindle away into the open pocket
  (DEBUG, EVAL[vcp.getWidget{"dynatc"}.store_tool{#<open_pocket>, #110}])
  #30 = #<open_pocket>
	##30 = #110                                       ; save tool number from spindle in pocket
	#5231 = 0                                         ; empty tool in the spindle
O180 ENDIF

G90
G1 G53 Z[#<_ini[ATC]ZHIGH] F[#<_ini[ATC]ZFAST]

; if selected tool is not 0
O200 IF [#100 GT 0]                                 ; if selected tool is not tool 0
  M10 P#<next_pocket>                               ; set the carousel to move to the right pocket for the selected tool
  M65 P2                                            ; turn off retract solenoid
  M64 P1                                            ; turn on extend solenoid
  M66 P1 L3 Q5                                      ; check carousel extended sensor
  O205 if [#5399 LT 0]
	  M65 P1                                          ; turn off extend solenoid
    (abort, ATC not in position)                    ; abort if the 	sensor does not activate in 5 seconds
  O205 endif
  M65 P1                                            ; turn off extend solenoid
  (DEBUG, EVAL[vcp.getWidget{"dynatc"}.store_tool{#<next_pocket>, 0}])
  M22                                               ; 
  #25 = #<next_pocket>
  ##25 = 0                                          ; empty the pocket
  #5231 = #100                                      ; Set persistent variable to remember tool in spindle after power cycle

O200 Else                                           ; if selected tool = 0, deactivate PDB and send ATC home
  M65 P3                                            ; deactive drawbar
  M65 P1                                            ; turn off extend solenoid
  M64 P2                                            ; turn on retract solenoid
  M66 P2 L3 Q4                                      ; check ATC retracted sensor
  O210 if [#5399 LT 0]
	M65 P2                                            ; turn off retract solonoid
  (abort, failed to send carousel home)             ; abort if the sensor does not activate in 5 seconds
  O210 endif
  M65 P2                                            ; turn off retract solonoid
O200 ENDIF

;assign the tool numbers in the pockets to the parameters
#5190= #1
#5191= #2
#5192= #3
#5193= #4
#5194= #5
#5195= #6
#5196= #7
#5197= #8
#5198= #9
#5199= #10
#5200= #11
#5201= #12
#5202= #13
#5203= #14
#5204= #15
#5205= #16
#5206= #17
#5207= #18

M61 Q#100                                           ; set tool in spindle to selected tool

o<010> if [1 EQ 1]                                  ; set tool offset to selected tool
  G43 H#100
o<010> endif

o<toolchange> endsub [1]

M2