###################### Torch Height Control ########################## 

# Load realtime hal components for THC
#3 extra ands for simulator
loadrt and2 count=13
loadrt comp count=2
loadrt flipflop
loadrt hypot
loadrt logic personality=0x203
loadrt minmax
loadrt mult2 count=2
loadrt mux2 count=7
#1 extra not for simulator
loadrt not count=3
loadrt oneshot count=2
#1 extra or for simulator
loadrt or2 count=8
loadrt scale
loadrt sum2 count=5
loadrt xor2 count=3

# Attach realtime hal components to a realtime thread
addf and2.0 servo-thread
addf and2.1 servo-thread
addf and2.2 servo-thread
addf and2.3 servo-thread
addf and2.4 servo-thread
addf and2.5 servo-thread
addf and2.6 servo-thread
addf and2.7 servo-thread
addf and2.8 servo-thread
addf and2.9 servo-thread
addf comp.0 servo-thread
addf comp.1 servo-thread
addf flipflop.0 servo-thread
addf hypot.0 servo-thread
addf logic.0 servo-thread
addf minmax.0 servo-thread
addf mult2.0 servo-thread
addf mult2.1 servo-thread
addf mux2.0 servo-thread
addf mux2.1 servo-thread
addf mux2.2 servo-thread
addf mux2.3 servo-thread
addf mux2.4 servo-thread
addf mux2.5 servo-thread
addf mux2.6 servo-thread
addf not.0 servo-thread
addf not.1 servo-thread
addf oneshot.0 servo-thread
addf oneshot.1 servo-thread
addf or2.0 servo-thread
addf or2.1 servo-thread
addf or2.2 servo-thread
addf or2.3 servo-thread
addf or2.4 servo-thread
addf or2.5 servo-thread
addf or2.6 servo-thread
addf scale.0 servo-thread
addf sum2.0 servo-thread
addf sum2.1 servo-thread
addf sum2.2 servo-thread
addf sum2.3 servo-thread
addf sum2.4 servo-thread
addf xor2.0 servo-thread
addf xor2.1 servo-thread
addf xor2.2 servo-thread

# Set static values and default values
setp oneshot.1.width [PLASMA]IGNITION_TIMEOUT
setp mux2.1.in0 -1
setp mux2.1.in1 1
setp mux2.3.in0 0

# Set the lowest value the THC will try to move the torch
setp sum2.4.in0 [AXIS_2]MIN_LIMIT

# Set default gains for sum, positive for addition and negative for subtraction
# Setting positive gains will become implicit and those can be removed safely in a later version
setp sum2.0.gain0 1
setp sum2.0.gain1 1
setp sum2.1.gain0 1
setp sum2.1.gain1 1
setp sum2.2.gain0 1
setp sum2.2.gain1 -1
setp sum2.3.gain0 1
setp sum2.3.gain1 -1
setp sum2.4.gain0 1
setp sum2.4.gain1 -1

# Set Hysterisis for float comp
setp comp.1.hyst 0.001

# Set percentage multiplier for scale
setp scale.0.gain 0.01

# Hook up parallel port pins
net MoveDown parport.1.pin-11-in-not => or2.0.in0
net MoveUp parport.1.pin-12-in-not => or2.0.in1 mux2.1.sel
# Floatswitch and ArcOK signals handled by simulator, see simulator.hal
#net FloatSwitch parport.1.pin-13-in => or2.1.in1 and2.4.in0 and2.5.in1 and2.6.in1
#net ArcOK parport.1.pin-15-in-not => or2.4.in1 oneshot.0.in or2.1.in0 or2.6.in0 or2.3.in0 mux2.0.sel or2.5.in0 and2.2.in0 not.1.in and2.4.in1

# Hook up component pins using the net command
net LockHeight and2.0.out => xor2.2.in0
net EnabledAdjustHeight and2.1.out => mux2.3.sel
net ReleaseFeedHold and2.2.out => xor2.0.in1
net TorchOn => and2.3.out parport.1.pin-01-out oneshot.1.in xor2.1.in0 and2.8.in0
net FloatSwitchEstop and2.4.out => logic.0.in-02
net FloatAndTorchOn and2.5.out => or2.2.in1
net TorchAndFloat and2.6.out => flipflop.0.set
net PosAndFloat and2.7.out => or2.5.in1
net IgnitionEstopFilter and2.8.out => logic.0.in-01
net StartTorchOn and2.9.out => and2.3.in0
net OriginalPosZ-cmd axis.2.motor-pos-cmd => mux2.5.in0 sum2.2.in1
net JointPoz-cmd axis.2.joint-pos-cmd => sum2.2.in0
net CHLTriggered comp.0.out => and2.0.in0
net AtPosition comp.1.equal => and2.7.in0
net VelX ddt.0.out => hypot.0.in0
net VelY ddt.1.out => hypot.0.in1
net FloatSwitchSet flipflop.0.out => or2.3.in1 and2.7.in1
net ModeIsAuto halui.mode.is-auto => mux2.5.sel
net VelXY hypot.0.out => minmax.0.in comp.0.in0
net TriggerEStop logic.0.or => estop-latch.0.fault-in or2.6.in1
net MaximumVelocity minmax.0.max => mult2.1.in0 
net TurnTorchOn motion.spindle-forward => xor2.0.in0 and2.9.in0 and2.6.in0 mux2.4.sel not.0.in
net HeightAdjustment mult2.0.out => mux2.3.in1
net VelocityThreshold mult2.1.out => comp.0.in1
net HeightOffset mux2.0.out => sum2.1.in1
net AdjustmentDirection mux2.1.out => mult2.0.in1
net PositionSnapshot mux2.2.out => sum2.1.in0 mux2.2.in0
net SelectedAdjustment mux2.3.out => mux2.0.in1
net PosZ-cmd mux2.4.out => stepgen.2.position-cmd comp.1.in1
net PosSelectB mux2.5.out => mux2.4.in0
net PosSelectC mux2.6.out => mux2.4.in1
net ReversedSpindleOn not.0.out => and2.5.in0
net ReversedArcOK not.1.out => minmax.0.reset
net PierceDelayElapsed oneshot.0.out-not => and2.2.in1
net IgnitionTimerElapsed oneshot.1.out => or2.4.in0
net AdjustHeight or2.0.out => xor2.2.in1 and2.1.in0
net TakeSnapshot or2.1.out => mux2.2.sel
net TriggerLimit or2.2.out => axis.2.neg-lim-sw-in axis.2.pos-lim-sw-in axis.2.home-sw-in
net ProbeOrArcOK or2.3.out => mux2.6.sel 
net IgnitionOrArcOK or2.4.out => xor2.1.in1
net PAFOrArcOK or2.5.out => and2.3.in1
net EstopOrArcOK or2.6.out => flipflop.0.reset
# Estop signal handled by simulator, see simulator.hal
#net ExtEStop parport.0.pin-10-in => logic.0.in-00
net ExtEStop  => logic.0.in-00
# Z limit handled by simulator, see simulator.hal
#net LimitZ parport.0.pin-13-in => or2.2.in0
net ThresholdPercentage scale.0.out => mult2.1.in1
net PosZ-fb stepgen.2.position-fb => axis.2.motor-pos-fb mux2.2.in1 comp.1.in0
net PierceOffset sum2.0.out => mux2.0.in0
net DestinationHeight sum2.1.out => mux2.6.in1
net JointAxisDiff sum2.2.out => sum2.3.in1 sum2.4.in1
net StepCordsTH sum2.3.out => mux2.5.in1
net StepCordsML sum2.4.out => mux2.6.in0
net FeedHold xor2.0.out => motion.feed-hold
net IgnitionEStop xor2.1.out => and2.8.in1
net CHLSwitched xor2.2.out => and2.1.in1