o<setspeed> sub



#1 = 83

#2 = 505

#3 = 625

#4 = 3800



;---NOTES---

; Low Gear: Min = 83 RPM, Max = 505 RPM

; High Gear: Min = 625 RPM, Max = 3800 RPM

; Increments = 23, 1 to 23

; In high gear: (Low Limit) 0 = 625 RPM,  Delta = 3175/23
; In low gear: (Low Limit) 0 = 83 RPM,  Delta = 422/23
 
#<hg_count> = ROUND[[#<speed>-625]*[23/3175]]

#<lg_count> = ROUND[[#<speed>-83]*[23/422]]

#<counter_hg> = 0

#<counter_lg> = 0



(High gear routine)

o100 if [[#<speed> GE #3] and [#<speed> LE #4]]



;M0


;(msg,Change to high gear!)



M3; <==== This does not turn on spindle the first time M3S900 is commanded in MDI.

G4 P5


M64 P6 ; M26 RPM Home command

M66 P2 L3 Q10 ; wait for reset = true
	
	o110 if [#5399 LT 0]
		
	(abort, failed to home RPM)
	
	o110 endif

M65 P6 ; Turn off M26 RPM Home command

	

	o120 while [#<counter_hg> LT #<hg_count>]
		
	M64 P4 ; M24 RPM Up command
		
	M66 P2 L3 Q5 ; wait for reset = true, wait for RPM motor to finish
			
		o121 if [#5399 LT 0]

			(Abort, Failed to increase RPM)
			
		o121 endif
		
	M65 P4 ; Turn off M24 RPM Up command
		
	G4 P2
		
	#<counter_hg> = [#<counter_hg>+1]
	
	o120 endwhile
	


;(Low gear routine)

;o100 elseif [[#<speed> GE #1] and [#<speed> LE #2]]



;(msg,Change to low gear!)



;o100 else


;(msg,RPM chosen is out of range!)



o100 endif	



o<setspeed> endsub [1]



m2