#######################################################################
# Tool Changer
#######################################################################
# type of tool changer is set by the UI via tormachltc.0.tool-changer-type
# 0 - manual
# 1 - gang
# 3 - 8 position ATC

setp hm2_5i25.0.stepgen.01.control-type 1
setp hm2_5i25.0.stepgen.01.position-scale [AXIS_3]STEP_SCALE

# stepper driver timing parameters
setp hm2_5i25.0.stepgen.01.steplen [AXIS_3]STEPLEN
setp hm2_5i25.0.stepgen.01.stepspace [AXIS_3]STEPSPACE
setp hm2_5i25.0.stepgen.01.dirhold [AXIS_3]DIRHOLD
setp hm2_5i25.0.stepgen.01.dirsetup [AXIS_3]DIRSETUP

setp hm2_5i25.0.stepgen.01.maxvel [AXIS_3]STEPGEN_MAX_VEL
setp hm2_5i25.0.stepgen.01.maxaccel [AXIS_3]STEPGEN_MAXACCEL


loadrt mux4 names=mux.turret-speed
addf mux.turret-speed servo-thread
setp mux.turret-speed.in0 0
setp mux.turret-speed.in1 .1
setp mux.turret-speed.in2 0
setp mux.turret-speed.in3 -5

net turret-on oracchanger.run mux.turret-speed.sel0
net turret-dir oracchanger.forward mux.turret-speed.sel1


net turret-speed mux.turret-speed.out  hm2_5i25.0.stepgen.01.velocity-cmd


# And possibly turn the stepper drive off when not actually changing tools
setp hm2_5i25.0.stepgen.01.enable 1


loadrt timedelay count=1
addf timedelay.0   servo-thread
# will wait 5 secs approx and turn motor off after reversing against stop
setp timedelay.0.on-delay 4 

net tool-change iocontrol.0.tool-change => oracchanger.toolchange

net xhomed axis.0.homed => oracchanger.ishomedX
net zhomed axis.2.homed => oracchanger.ishomedZ

net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared
net tool-changed iocontrol.0.tool-changed <= oracchanger.toolchanged
net tool-number iocontrol.0.tool-prep-number => oracchanger.toolnumber
net tool-oldnumber iocontrol.0.tool-number => oracchanger.currenttoolnumber


net sig1 oracchanger.opto1 <= hm2_5i25.0.7i76.0.0.input-05
net sig2 oracchanger.opto2 <= hm2_5i25.0.7i76.0.0.input-06
net sig3 oracchanger.opto3 <= hm2_5i25.0.7i76.0.0.input-07


net delaystart timedelay.0.in <= oracchanger.delaystart
net delaydone timedelay.0.out => oracchanger.delaydone

setp oracchanger.times 100
setp oracchanger.optodelay 25 #needs to be above 15 to work