# HAL config file for Rugludallur Plasma Cutting Table using classic ladder
# Loads the dallur-advanced.hal file first to set up motion and stepgen
# for a 3 axis stepper system where Axis A is slaved to Axis Y 
# This adds limits and home from limit to the motion module
# Adds ESTOP button  (with classicladder)
# Creates Charge Pump Signals for 2 Parallel ports on pins 17
# Includes unused intermittent lube pump and pressure or level sensing
# Includes unused coolant 
# Spindle forward is used for "Torch ON" signal, reverse is not used

# Load Driver for Two Parallel Ports, one for Breakout Board, other for Plasma Torch Height Control
loadrt hal_parport cfg="0xa400 0xac00"

# Connect both Paralell Ports to Threads for Read / Write
addf parport.0.read base-thread 1
addf parport.0.write base-thread -1
addf parport.1.read base-thread 1
addf parport.1.write base-thread -1

# This configuration uses classicladder for machine logic
# (load the realtime portion)
# these classicladder parameters need to be optimized after ladder completion
loadrt classicladder_rt numRungs=10 numBits=20 numWords=4 numTimers=5 numMonostables=2 numPhysInputs=20 numPhysOutputs=10 numArithmExpr=4 numSections=4
# We kick ladder into a thread at the slower servo rate rather than base rate
addf classicladder.0.refresh        servo-thread 1

# invoke the user part of CL to silently load the program
# later we will need to add the clp file mentioned below
# to the ini file so that we do not need to hardcode it here.
loadusr -w classicladder --nogui dallur-advanced.clp
# load the GUI aswell
loadusr classicladder

#Reverse Direction for A(Auxilary) Axis so both motors drive in same direction
setp parport.0.pin-09-out-invert 1

# begin to connect physical pins to signals
# this is EMC's standard motion pinouts
#Axis for Cart, single stepper motor marked Axis X in breakout
linksp Xstep parport.0.pin-02-out
linksp Xdir  parport.0.pin-06-out
#Axis for Gantry, primary stepper motor (same side as ctrl box), marked Axis Y in breakout
linksp Ystep parport.0.pin-03-out
linksp Ydir  parport.0.pin-07-out
#Axis for Gantry, secondary stepper motor (other side of ctrl box), marked Axis A(Auxilary) in breakout
linksp Ystep parport.0.pin-05-out
linksp Ydir  parport.0.pin-09-out
#Axis for Height Control, single stepper motor marked Axis Z in breakout
linksp Zstep parport.0.pin-04-out
linksp Zdir  parport.0.pin-08-out

#Enable signals not used because of Charge Pump
#linksp Yen   parport.0.pin-06-out
#linksp Yen   parport.0.pin-09-out
#linksp Xen   parport.0.pin-03-out

# Estop signals
# This signal is the internal or gui estop command from EMC
net estop-out iocontrol.0.user-enable-out classicladder.0.in-00
# This is a one-shot on timer when signal above goes high.
net estop-in iocontrol.0.user-request-enable  classicladder.0.in-02
# This bit is an external estop button connected to parport pin 10 
newsig ext-estop bit
linksp ext-estop parport.0.pin-10-in
linkps classicladder.0.in-01 ext-estop
# This bit signal is comand to estop from CL to EMC
net emc-enable-in iocontrol.0.emc-enable-in classicladder.0.out-00 


# Spindle Signal, forward drives Torch ON
# Spindle reverse is not used (might be used for a Running Start later)
newsig turn_torch_on bit
linksp turn_torch_on parport.1.pin-01-out
#net spindle-reverse motion.spindle-reverse  parport.0.pin-17-out 

#Create a limit switch signal for each axis
newsig limit-reached-x bit
newsig limit-reached-y bit
newsig limit-reached-z bit
newsig limit-reached-a bit

#Read limit switches from normally open circuit on parport 0
linkps parport.0.pin-11-in-not  limit-reached-x
linkps parport.0.pin-12-in-not  limit-reached-y
linkps parport.0.pin-13-in-not  limit-reached-z
linkps parport.0.pin-15-in-not  limit-reached-a

#Connect all limit signals to relevant axes
linksp limit-reached-x  axis.0.neg-lim-sw-in
linksp limit-reached-x  axis.0.pos-lim-sw-in
linksp limit-reached-y  axis.1.neg-lim-sw-in
linksp limit-reached-y  axis.1.pos-lim-sw-in
linksp limit-reached-a  axis.1.neg-lim-sw-in
linksp limit-reached-a  axis.1.neg-lim-sw-in
linksp limit-reached-z  axis.2.neg-lim-sw-in
linksp limit-reached-z  axis.2.neg-lim-sw-in


########## Charge Pump #########

# This is for Charge Pump which requires a 15khz signal to activate, maxfreq is the limiter 
loadrt freqgen step_type=0
loadrt siggen
loadrt threads name1=fastcp period1=30000 name2=slowcp period2=1000000

newsig velcp float
newsig stepcp bit

linksp velcp siggen.0.square
linksp velcp freqgen.0.velocity
linksp stepcp freqgen.0.step

addf siggen.0.update slowcp
addf freqgen.update-freq slowcp
addf freqgen.make-pulses fastcp
setp freqgen.0.velocity-scale 1000000
setp freqgen.0.maxfreq 15000

linksp stepcp parport.0.pin-17-out
linksp stepcp parport.1.pin-17-out


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

# Load the neccessary functions
loadrt mux2 count=7
loadrt or2 count=4
loadrt not count=4
loadrt sum2 count=2
loadrt comp count=2
loadrt and2 count=2

# Make a sensor for the ARC OK signal with an active low
newsig senseArcOKInverted bit
linksp senseArcOKInverted <= parport.1.pin-15-in

# Invert ArcOK Signal to so TRUE = Arc ON (active high)
newsig senseArcOK bit
linksp senseArcOKInverted not.0.in
linksp senseArcOK not.0.out

# Make a sensor for the Move Down bit from the THC, active low
newsig senseZDownInverted bit
linksp senseZDownInverted <= parport.1.pin-11-in 

# Invert SenseDown Signal to so TRUE = Sense Down (active high)
newsig senseZDown bit
linksp senseZDownInverted not.1.in
linksp senseZDown not.1.out

# Make a sensor for the Move Up bit from the THC, this signal is active low
newsig senseZUpInverted bit
linksp senseZUpInverted <= parport.1.pin-12-in 

# Invert senseZUp Signal to so TRUE = Sense Up (active high)
newsig senseZUp bit
linksp senseZUpInverted not.2.in
linksp senseZUp not.2.out

# Make a sensor for the Z Float Switch
# Invert because THC has active low
# The Z float switch is a proxmity sensor from torch to work piece
# PNP or NPN Capacative or Inductive switches are recomended, Capacative for non metals but Inductive for metals
newsig senseZFloatSwitchInverted bit
linksp senseZFloatSwitchInverted <= parport.1.pin-13-in

# invert the senseZFloatSwitch so TRUE = At distance (active high)
newsig senseZFloatSwitch bit
linksp senseZFloatSwitchInverted not.3.in
linksp senseZFloatSwitch not.3.out

# Create signals to move Axis Y up or down with destinations, Y should never reach destination because as soon
# as the axis moves the input signal will change and disconnect the signal at the mux
# In the future it would be good to pull estop if Z axis is ever >= ZMaxCord
newsig ZMinCord float
sets ZMinCord 0
newsig ZMaxCord float
sets ZMaxCord 300

# Create signals to join the moveUP from parport and the one from init
newsig moveUp bit
newsig moveDown bit
newsig moveZtoFloat bit
newsig moveZtoSafe bit
newsig moveZDown bit
newsig moveZUp bit
newsig moveZtoOK bit
newsig moveZEnabled bit
newsig LockPierceHeight bit

linksp senseZDown or2.0.in0
linksp moveZtoFloat or2.0.in1
linksp moveZDown or2.0.out

linksp senseZUp or2.1.in0
linksp moveZtoSafe or2.1.in1
linksp moveZUp or2.1.out

linksp moveZtoFloat or2.2.in0
linksp moveZtoSafe or2.2.in1
linksp moveZtoOK or2.2.out

linksp LockPierceHeight or2.3.in0
linksp moveZtoOK or2.3.in1
linksp moveZEnabled or2.3.out

addf or2.0 servo-thread
addf or2.1 servo-thread
addf or2.2 servo-thread
addf or2.3 servo-thread
addf not.0 servo-thread
addf not.1 servo-thread
addf not.2 servo-thread
addf not.3 servo-thread

# Mux the zpos-cmd and UpDest based on the state of the senseZUp signal
newsig ZMinCord-Zpos-cmd-muxed float

linksp Zpos-cmd mux2.0.in0
linksp ZMinCord mux2.0.in1
linksp moveZUp mux2.0.sel
linksp ZMinCord-Zpos-cmd-muxed mux2.0.out

addf mux2.0 servo-thread 

# Mux the zpos-cmd and ZMaxCord based on the state of the senseZDown signal
newsig ZMaxCord-Zpos-cmd-muxed float

linksp Zpos-cmd mux2.1.in0
linksp ZMaxCord mux2.1.in1
linksp moveZDown mux2.1.sel
linksp ZMaxCord-Zpos-cmd-muxed mux2.1.out

addf mux2.1 servo-thread 

# Mux ZMaxCord-Zpos-cmd-muxed and UpDest-Zpos-cmd-muxed
newsig Zpos-cmd-muxed float

linksp ZMinCord-Zpos-cmd-muxed mux2.2.in0
linksp ZMaxCord-Zpos-cmd-muxed mux2.2.in1
linksp moveZDown mux2.2.sel
linksp Zpos-cmd-muxed mux2.2.out

addf mux2.2 servo-thread

#Disconnect Machine/Manual Axis Z Control while ArcOK for safety and to eliminate jitter
newsig Zpos-instructed-cmd float
linksp Zpos-instructed-cmd <= axis.2.motor-pos-cmd

linksp Zpos-instructed-cmd => mux2.3.in0
linksp Zpos-fb => mux2.3.in1
linksp moveZEnabled => mux2.3.sel
linksp Zpos-cmd <= mux2.3.out

addf mux2.3 servo-thread

# Feedhold is used to halt gcode execution while torch is placed at correct initial height
# feedhold, uses G50 adaptive feed input
newsig zero float
sets zero 0.0
newsig one float
sets one 1.0
newsig adaptive-feed float
linksp adaptive-feed mux2.4.out
linksp adaptive-feed motion.adaptive-feed
linksp one mux2.4.in0
linksp zero mux2.4.in1
newsig feed-hold bit

# Not only do we want to set temp. to 0 we also want to pause state within GUI App, this way the user has to confirm before cutting starts
# Pause state will be maintained even after feedrate has been reset to 1
# This is can now be shut off/turned on from the VCP UI
linksp feed-hold mux2.4.sel

# To remove the manual pause comment out the following line
#linksp feed-hold halui.program.pause

newsig ManualVerifyBeforePierce bit

linksp feed-hold and2.0.in0
linksp ManualVerifyBeforePierce and2.0.in1
net program.pause halui.program.pause and2.0.out

addf mux2.4 servo-thread

# Create the signals for classicladder to control parts of THC
newsig ui_mode_is_auto bit
linksp senseZFloatSwitch => classicladder.0.in-03
linksp moveZtoFloat classicladder.0.out-03
linksp moveZtoSafe classicladder.0.out-04
linksp senseArcOK classicladder.0.in-04
net spindle-forward classicladder.0.in-05 <= motion.spindle-forward 
linksp feed-hold classicladder.0.out-06
linksp turn_torch_on classicladder.0.out-05
linksp ui_mode_is_auto halui.mode.is-auto 
linksp ui_mode_is_auto classicladder.0.in-06

############## PierceGap & SwitchTravel ############

newsig PierceGap float
newsig SwitchTravel float
newsig PierceHeight float
newsig MoveZtoPierceHeight bit
newsig WorkStandOff float
newsig TorchIsAtPierceHeight bit
newsig PierceDelay float
newsig EnablePierceDelay bit
newsig ArcStartTimeout float
newsig ProgramIsPaused bit
newsig limit-estop bit

# These parameters control the SwitchTravel and PierceGap, later we hope to have piercegap automatically set
# from the tools table as there will be a tool for each amp setting with cutting/gauging consumables
sets SwitchTravel 10
sets PierceHeight 50
sets ArcStartTimeout 5
sets PierceDelay 0.2
sets EnablePierceDelay 1

# Add PierceGap to SwitchTravel, add this to the height at the moment LockPierceHeight is received
# Make a new height signal which is PierceGap+SwitchTravel+LockPierceHeight
# Tell the stepgen to move to the newly calculated position

linksp SwitchTravel sum2.0.in0
linksp PierceGap sum2.0.in1
linksp WorkStandOff sum2.0.out

linksp Zpos-fb => sum2.1.in0
linksp WorkStandOff => sum2.1.in1
#FIXME add a proper name
net mux2-sum2 mux2.6.in0 <= sum2.1.out

linksp PierceHeight mux2.6.in1
linksp PierceHeight mux2.6.out
linksp LockPierceHeight mux2.6.sel

linksp Zpos-cmd-muxed mux2.5.in0
linksp PierceHeight mux2.5.in1
linksp MoveZtoPierceHeight mux2.5.sel
net Z-pos-cmd mux2.5.out stepgen.2.position-cmd

addf mux2.5 servo-thread
addf mux2.6 servo-thread
addf sum2.0 servo-thread
addf sum2.1 servo-thread
addf comp.0 servo-thread
addf comp.1 servo-thread
addf and2.0 servo-thread
addf and2.1 servo-thread

# Also make sure we tell classicladder when we are at pierce height so it can turn on the torch

linksp PierceHeight comp.0.in0
linksp Zpos-fb comp.0.in1
linksp TorchIsAtPierceHeight comp.0.out

# make a signal so we do not turn Arc ON while the program is paused
# put it through OR to make sure pause only affects operations while
# mode is not manual
newsig PauseIsOn bit
linksp PauseIsOn  halui.program.is-paused

linksp PauseIsOn and2.0.in0
linksp ui_mode_is_auto and2.0.in1
linksp ProgramIsPaused and2.0.out

# If for some reason the Z location is further down or equal  than the maximum Z coord we estop!
# if this happens it means that a. there is no work piece, b the torch is stuck, c the Floatswitch is broken

linksp ZMaxCord comp.1.in0
linksp Zpos-fb comp.1.in1
linksp limit-estop comp.1.out 

# This is an estop signal used when THC has detected a limit error
linksp limit-estop classicladder.0.in-11

linksp TorchIsAtPierceHeight classicladder.0.in-07
linksp LockPierceHeight classicladder.0.out-07
linksp MoveZtoPierceHeight classicladder.0.out-08
#linksp PierceDelay classicladder.0.in-08
#linksp ArcStartTimeout classicladder.0.in-09
linksp ProgramIsPaused classicladder.0.in-10
linksp EnablePierceDelay classicladder.0.in-12
sets EnablePierceDelay 1

############ Corner Height Lock ##################
newsig EnableCornerHeightLock bit