Linuxcnc Orientacja wrzeciona do wymiany narzędzia.

Kod: Zaznacz cały

o<toolchange> sub
;(debug,toolchange start)
;(debug,stop1)
;(debug,z_max_limit = #<_hal[ini.2.max_limit]>)



M73 (auto-restore modal settings on return)
G17 G21 G40 G61 G80 G90 G94 G97 M9

(check whether this code is run as preview. If so skip)
o100 if [#<_task> EQ 0]
	o<toolchange> return [1]
o100 endif

(reset iocontrol.0.tool-changed)
m65 p0


(check if spindle is initially indexed, otherwise indexing is unsafe)
m66 P1 L0 Q1
o169 if [#5399 NE 0]
	(abort, spindle not indexed)
	o<toolchange> return [0]
o169 endif

;-------------------------- Orient Spindle ------------

G28.1 (copy absolute coordinates into #5161-#5169)
#<z_current>=#5163 (current depth)


G53 G0 Z0

(orient spindle NOT HGHER THEN Z>0 !!!!)
o130 if [#<z_current> LE 0.05]
	;(debug,Z position <= 0.05, OK to m19 , Z=#<z_current>)
	m19 r0.3 p0 q7
	g04 p1
	
o130 else
	(abort,z position >0 FAIL to m19, Z = #<z_current>)
	o<toolchange> return [0]
o130 endif

g04 p0.1		(dwell 0.1s)

;-------------------------- Calculate pocket task to move ------------
m66 P11 L0 Q1
#1=#5399
m66 P12 L0 Q1
#2=#5399
m66 P13 L0 Q1
#3=#5399
m66 P14 L0 Q1
#4=#5399

#<current_pocket>=[[#1*1]+[#2*2]+[#3*4]+[#4*8]]

#<counter>=0

#<pocket> = #<selected_pocket>

;(debug,current_pocket = #<current_pocket>)



o170 if [#<selected_tool> LT 10]
 ;(debug,Selected tool: #<selected_tool>)
o170 endif

o180 if [#<current_pocket> EQ 0]
	#<pocket>=#<current_pocket>
	;(abort, ATC: current ATC pocket number invalid)
	o<toolchange> return [0]
o180 else
	;(debug,Current Pocket >< 0 OK #<current_pocket>)
o180 endif

o120 if [#<pocket> GT 0]
	#<newpocket>=#<pocket>
o120 else
	;(abort, ATC: pocket number not positive)
	#<newpocket>=#<current_pocket>
	o<toolchange> return [0]
o120 endif

o121 if [#<newpocket> EQ #<current_pocket>]
	;(print, ATC: Pocket already selected.)
	o<toolchange> return [1]
o121 endif

#<movepos>=[#<newpocket>-#<current_pocket>]
#<absmovepos>=ABS[#<movepos>]

o111 if [#<absmovepos> GT 5]
	#<shortmovepos>=[ABS[10-#<absmovepos>]]
o111 endif

#<finalmovepos>=#<absmovepos>
;(debug,movepos #<movepos>)
;(debug, selected_pocket #<selected_pocket>)




;-------------------------Work with optical probe ------------------
;(if current tool is the optical probe, deactivate it before moving)
;o161 if [#<tool_in_spindle> EQ 99]
;	(print, switching off probe..)
;	m64 p2
;	g04 p0.3
;	m65 p2
;	g04 p2
;	(if probe was already off, this might have switched it back on)
;	m66 p19 l0 q1
;	#<probe_error>=#5399
;	o162 if [#<probe_error> EQ 0]
;		m64 p2
;		g04 p0.3
;		m65 p2
;		g04 p2
;	o162 endif
;	m66 p19 l0 q1
;	#<probe_error>=#5399
;	o163 if [#<probe_error> EQ 0]
;		(print, could not switch off probe, try manually by removing battery)
;	o163 endif
;o161 endif



;--------------------------------- MOVE to ATC HOME ---------------------

(unlock z-travel for ATC range. send true to z-limit-mux.sel--> z-limit-max is set to z-limit-maz-atc)
m64 p1
;(debug,Extend z_max_limit = #<_hal[ini.z.max_limit]>)

m66 P16 L0 Q1
;(debug,ATC-home state  #5399)

(move to ATC home position)
;(debug,move to Z=147)
G53 G0 Z147

(check whether at ATC home)
m66 P16 L0 Q1
#<atcready>=#5399
;(debug,ATC-home state  #5399)


;--------------------------Set ATC Direction RELAY -----------------
o106 if [#<movepos> LT 0]
	M64 p4
	#<ccw>=1
	o119 if [#<absmovepos> GT 5]
		M65 p4
		#<ccw>=0
	#<finalmovepos>=#<shortmovepos>
	o119 endif
o106 else
	M65 p4
	#<ccw>=0
	o129 if [#<absmovepos> GT 5]
		M64 p4
		#<ccw>=1
	#<finalmovepos>=#<shortmovepos>
	o129 endif
o106 endif



;(debug,finalmovepos  #<finalmovepos>)


;--------------------------------- MOVE ATC ---------------------

o112 if [#<atcready> EQ 1]
	;(debug,ATC-home OK)
	o104 if [#<current_pocket> NE #<newpocket>]
		m64 p3
		
		o101 while [#<counter> LT [#<finalmovepos>]]
			m66 P10 L1 Q1
			#<counter>=[#<counter>+1]
			;(debug,counter #<counter>)
		o101 endwhile
		
		m65 p3
		m65 p4
		
	o104 else
		(abort, ATC: failed because already on that pocket)
	o104 endif
	
	(jesli przejechalo zero-position cofnij do impulsu)
	g4 p0.01
	m66 P10 L0 Q1
	#<notaligned>=#5399
	
	;(debug,aligned status = #<notaligned>)
	;(debug,ccw = #<ccw>)
	
	o103 if [#<notaligned> NE 1]
		#<reverse_counter>=0
		;(debug,Bede cofal)
		o107 if [#<ccw> EQ 1]
				;(debug,Kierunek cw)
				m64 p3
		o107 else
				;(debug,Kierunek ccw)
				m64 p3
				m64 p4
		o107 endif
		

		
		m66 P10 L1 Q1
		
		m65 p3
		m65 p4
		;(debug,Cofanie finito)
	
	o103 endif
	
	
o112 else
	(abort, ATC: not at ATC HOME abort)
	o<toolchange> return [0]
o112 endif

(read atc-zero-position pin to check alignment)
m66 P10 L0 Q1
#<notaligned>=#5399

(read current pocket number from encoders)
m66 P11 L0 Q1
#1=#5399
m66 P12 L0 Q1
#2=#5399
m66 P13 L0 Q1
#3=#5399
m66 P14 L0 Q1
#4=#5399
#<current_pocket>=[[#1*1]+[#2*2]+[#3*4]+[#4*8]]



o150 if [#<notaligned> NE 1]
	(abort, ATC: ATC not aligned or between stations.)
	o<toolchange> return [0]
o150 elseif [#<newpocket> NE #<current_pocket>]
	(abort, ATC: ATC on the wrong pocket.)
	return
o150 else
	;(debug,SEEMS OK!!! STOP :)
	#<tool_in_spindle>=#<selected_pocket>
	
	(przedmuch wrzeciona)
	m64 p2
	(Z down)
	G53 G0 Z0
	(lock z-travel max limit again)
	m65 p1
	(confirm tool change to iocontrol.0.tool-changed via hal wiring)
	m64 p0
	(write tool number to iocontrol.0.tool-number)
	m68 e0 q#<current_pocket>
	
;	(if tool is the optical probe, activate it)
;	o151 if [#<selected_tool> EQ 99]
;		(print, switching on probe..)
;		m64 p2
;		g04 p0.3
;		m65 p2
;		g04 p2
;		(if probe was already on, this might have switched it back off)
;		m66 p19 l0 q1
;		#<probe_error>=#5399
;		o152 if [#<probe_error> EQ 1]
;			m64 p2
;			g04 p0.3
;			m65 p2
;			g04 p2
;		o152 endif
;		m66 p19 l0 q1
;		#<probe_error>=#5399
;		o153 if [#<probe_error> EQ 1]
;			(abort, could not switch on probe)
;		o153 endif
;	o151 endif

	(koniec przedmuchu wrzeciona)
	m65 p2
	
		
o150 endif	

;(debug,Spindle Off)
m5

o<toolchange> endsub [1]	
m5


	
;--------------------------STOP-----------------------
(debug,STOP :)
o<toolchange> return [1]
m5
m2		
;-----------------------------------------------------