# configuration for 3-axis gantry router with tandem motors on the y-axis
# also has Logitech F310 Gamepad as a jog pendant


loadrt trivkins
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]SERVO_PERIOD key=[EMCMOT]SHMEM_KEY num_joints=[TRAJ]AXES
loadrt probe_parport
loadrt hal_parport cfg=0x378
setp parport.0.reset-time 5000
loadrt stepgen step_type=0,0,0
# loadrt threads fp1=0 name1=fast period1=50000 name2=slow period2=1000000

# load the Gamepad Pro module
loadusr -W hal_input -KRAL X-Box

# load the module for the tandem motor logic
loadrt and2 count=5
loadrt or2 count=5
loadrt not count=6
loadrt mux4 count=1
loadrt debounce cfg=2,3

# add the module to the necessary threads
addf and2.0 base-thread
addf and2.1 base-thread
addf and2.2 base-thread
addf and2.3 base-thread
addf and2.4 base-thread
addf or2.0 base-thread
addf or2.1 base-thread
addf or2.2 base-thread
addf not.0 base-thread
addf not.1 base-thread
addf not.2 base-thread
addf not.3 base-thread
addf not.4 base-thread
addf debounce.0 base-thread
addf debounce.1 base-thread

addf parport.0.read base-thread
addf stepgen.make-pulses base-thread
addf parport.0.write base-thread
addf parport.0.reset base-thread

addf stepgen.capture-position servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf stepgen.update-freq servo-thread


# set up the axis parameters and create the axis signals

# x-axis
setp stepgen.0.position-scale [AXIS_0]SCALE
setp stepgen.0.steplen 1
setp stepgen.0.stepspace 1
setp stepgen.0.dirhold 35000
setp stepgen.0.dirsetup 35000
setp stepgen.0.maxaccel [AXIS_0]STEPGEN_MAXACCEL
net xpos-cmd axis.0.motor-pos-cmd => stepgen.0.position-cmd
net xpos-fb stepgen.0.position-fb => axis.0.motor-pos-fb
net xstep <= stepgen.0.step
net xdir <= stepgen.0.dir
net xenable axis.0.amp-enable-out => stepgen.0.enable

# y-axis
setp stepgen.1.position-scale [AXIS_1]SCALE
setp stepgen.1.steplen 1
setp stepgen.1.stepspace 1
setp stepgen.1.dirhold 35000
setp stepgen.1.dirsetup 35000
setp stepgen.1.maxaccel [AXIS_1]STEPGEN_MAXACCEL
net ypos-cmd axis.1.motor-pos-cmd => stepgen.1.position-cmd
net ypos-fb stepgen.1.position-fb => axis.1.motor-pos-fb
net ystep <= stepgen.1.step
net ydir <= stepgen.1.dir
net yenable axis.1.amp-enable-out => stepgen.1.enable

# z-axis
setp stepgen.2.position-scale [AXIS_2]SCALE
setp stepgen.2.steplen 1
setp stepgen.2.stepspace 0
setp stepgen.2.dirhold 35000
setp stepgen.2.dirsetup 35000
setp stepgen.2.maxaccel [AXIS_2]STEPGEN_MAXACCEL
net zpos-cmd axis.2.motor-pos-cmd => stepgen.2.position-cmd
net zpos-fb stepgen.2.position-fb => axis.2.motor-pos-fb
net zstep <= stepgen.2.step
net zdir <= stepgen.2.dir
net zenable axis.2.amp-enable-out => stepgen.2.enable


# set all of the step signals to reset
#setp parport.0.pin-02-out-reset 1
#setp parport.0.pin-04-out-reset 1
#setp parport.0.pin-06-out-reset 1
#setp parport.0.pin-08-out-reset 1

# invert a couple of direction signals
setp parport.0.pin-03-out-invert 1
setp parport.0.pin-05-out-invert 1
setp parport.0.pin-07-out-invert 1
setp parport.0.pin-09-out-invert 1

# debounce the y-axis switches and connect them to signals
net switches-y1-raw	<= parport.0.pin-11-in
net switches-y1-raw 	=> debounce.0.0.in
net switches-y1 	<= debounce.0.0.out
net switches-y2-raw   	<= parport.0.pin-12-in
net switches-y2-raw 	=> debounce.0.1.in
net switches-y2 	<= debounce.0.1.out
setp debounce.0.delay 10

# join the home switch signals so that both switches have to be closed to trigger a home position
net switches-y1		=> and2.0.in0
net switches-y2		=> and2.0.in1
net home-y 		<= and2.0.out
net home-y 		=> axis.1.home-sw-in

# we will need the inverse of the home switch signals for our logic, so run them through not gates
net switches-y1		=> not.0.in
net switches-y1-inv 	<= not.0.out
net switches-y2		=> not.1.in
net switches-y2-inv 	<= not.1.out

# we will need the inverse of the direction signals for our logic, so run it through a not gate
net ydir	=> not.2.in
net ydir-inv	=> not.2.out

# we need to determine if the home switch for each motor is tripped while the direction line is
# moving the motor towards the switch

# for both y-axis motors, we will prevent the step signal from passing to the motor
# if the motor is moving towards the switch and the switch is tripped
# so "or" the inverted direction signals and the inverted home switches
net ydir	=> and2.3.in0
net switches-y1 => and2.3.in1
net doh1	<= and2.3.out
net doh1	=> not.3.in
net doh1-inv	=> not.3.out

net ydir	=> and2.4.in0
net switches-y2 => and2.4.in1
net doh2	<= and2.4.out
net doh2	=> not.4.in
net doh2-inv	=> not.4.out

# if the step generator is trying to send a step (ystep signal) and either the motor is moving
# away from the switch or the switch is not tripped, then pass the step signal to the motor
net ystep 	and2.1.in0
net doh1-inv	and2.1.in1
net ystep1 	and2.1.out

net ystep 	and2.2.in0
net doh2-inv	and2.2.in1
net ystep2	and2.2.out

# connect our step and direction signals to the output pins
net xstep 	=> parport.0.pin-02-out
net xdir 	=> parport.0.pin-03-out
net ystep1 	=> parport.0.pin-04-out
net ystep2 	=> parport.0.pin-08-out
net ydir 	=> parport.0.pin-05-out
net ydir 	=> parport.0.pin-09-out
net zstep 	=> parport.0.pin-06-out
net zdir 	=> parport.0.pin-07-out

# we are using combined home and limit switches
# if either switch trips while the homing sequence is not running, then triggered a negative
# limit switch fault
net switches-y1 => or2.2.in0
net switches-y2	=> or2.2.in1
net limit-y 	<= or2.2.out
net limit-y 	=> axis.1.neg-lim-sw-in
net limit-y 	=> axis.1.pos-lim-sw-in


# set up the other home and limit switches
net switches-x-raw   	<= parport.0.pin-10-in
net switches-x-raw 	=> debounce.1.0.in
net switches-x 		<= debounce.1.0.out
net switches-x 		=> axis.0.home-sw-in
net switches-x		=> axis.0.neg-lim-sw-in
net switches-x		=> axis.0.pos-lim-sw-in

net switches-z-raw 	<= parport.0.pin-13-in
net switches-z-raw 	=> debounce.1.1.in
net switches-z 		<= debounce.1.1.out
net switches-z 		=> axis.2.home-sw-in
net switches-z		=> axis.2.pos-lim-sw-in

setp debounce.1.delay 10

# connect the spindle control to the relay pin
net spindle-cw => parport.0.pin-14-out
net spindle-cw <= motion.spindle-forward

# connect the shop vac to the flood coolant signal
net flood => iocontrol.0.coolant-flood
net flood => parport.0.pin-16-out

# set up the e-stop switch
net estop-raw <= parport.0.pin-15-in-not
net estop-raw => debounce.1.2.in
net estop-ext <= debounce.1.2.out
net estop-ext => iocontrol.0.emc-enable-in



#net estop-out <= iocontrol.0.user-enable-out
#net estop-out => iocontrol.0.emc-enable-in

net estop-out <= iocontrol.0.user-enable-out


loadusr -W hal_manualtoolchange
net tool-change iocontrol.0.tool-change => hal_manualtoolchange.change
net tool-changed iocontrol.0.tool-changed <= hal_manualtoolchange.changed
net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number
net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared