# HAL config file for MAX NC test machine
#######################################################
# first load all the HAL modules we're going to need
#######################################################
# kinematics
loadrt [KINS]KINEMATICS
# main motion controller module
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
# using steppers....
loadrt stepgen step_type=0,0,0,0
# I/O thru the parport
loadrt hal_parport cfg="0x0378"
# counting the spindle encoder in software
loadrt encoder num_chan=2
# simulate the encoder
loadrt sim_encoder num_chan=1
# misc blocks needed to do fun things
loadrt ddt count=8

#######################################################
# add realtime functions to the threads
# first the high speed thread
#######################################################
# first we read parport inputs
addf parport.0.read		base-thread
addf sim-encoder.make-pulses    base-thread
# update encoder counter
addf encoder.update-counters    base-thread
# generate step and dir signals
addf stepgen.make-pulses	base-thread
# write to outputs
addf parport.0.write		base-thread



# now the servo thread
########################################################
# capture axis and spindle positions
addf stepgen.capture-position	servo-thread
addf encoder.capture-position	servo-thread
# process motion commands coming from user space
addf motion-command-handler	servo-thread
# run main motion controller
addf motion-controller		servo-thread
# differentiators to make vel and acc signals
addf ddt.0			servo-thread
addf ddt.1			servo-thread
addf ddt.2			servo-thread
addf ddt.3			servo-thread
addf ddt.4			servo-thread
addf ddt.5			servo-thread
addf ddt.6			servo-thread
addf ddt.7			servo-thread
# update output info, axis and spindle
addf stepgen.update-freq	servo-thread
addf sim-encoder.update-speed   servo-thread



#######################################################
# interconnections
#
# first, the basic stepper machine connections
#######################################################

# connect position commands from motion module to step generator
net Xpos-cmd <= joint.0.motor-pos-cmd
net Xpos-cmd => stepgen.0.position-cmd
net Ypos-cmd <= joint.1.motor-pos-cmd
net Ypos-cmd => stepgen.1.position-cmd
net Zpos-cmd <= joint.2.motor-pos-cmd
net Zpos-cmd => stepgen.2.position-cmd
net Apos-cmd <= joint.3.motor-pos-cmd
net Apos-cmd => stepgen.3.position-cmd

# connect position feedback from step generators
# to motion module
net Xpos-fb <= stepgen.0.position-fb
net Xpos-fb => joint.0.motor-pos-fb
net Ypos-fb <= stepgen.1.position-fb
net Ypos-fb => joint.1.motor-pos-fb
net Zpos-fb <= stepgen.2.position-fb
net Zpos-fb => joint.2.motor-pos-fb
net Apos-fb <= stepgen.3.position-fb
net Apos-fb => joint.3.motor-pos-fb

# connect enable signals for step generators
net Xen <= joint.0.amp-enable-out
net Xen => stepgen.0.enable
net Yen <= joint.1.amp-enable-out
net Yen => stepgen.1.enable
net Zen <= joint.2.amp-enable-out
net Zen => stepgen.2.enable
net Aen <= joint.3.amp-enable-out
net Aen => stepgen.3.enable

# connect signals to step pulse generator outputs
net Xstep <= stepgen.0.step
net Xdir <= stepgen.0.dir
net Ystep <= stepgen.1.step
net Ydir <= stepgen.1.dir
net Zstep <= stepgen.2.step
net Zdir <= stepgen.2.dir
net Astep <= stepgen.3.step
net Adir <= stepgen.3.dir

# create a signal for the estop loopback
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in

# create signals for tool loading loopback
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed

# create a signal for "spindle on"
net spindle_on <= motion.spindle-on

# connect physical pins to the signals
net Xstep => parport.0.pin-03-out
net Xdir => parport.0.pin-02-out
net Ystep => parport.0.pin-05-out
net Ydir => parport.0.pin-04-out
net Zstep => parport.0.pin-07-out
net Zdir => parport.0.pin-06-out
net Astep => parport.0.pin-09-out
net Adir => parport.0.pin-08-out
# amp enable (active lo)
net Xen => parport.0.pin-01-out
setp parport.0.pin-01-out-invert 1
# spindle enable (active lo)
net spindle_on => parport.0.pin-14-out
setp parport.0.pin-14-out-invert 1

#######################################################
# set scaling and other parameters of the basic machine

# set stepgen module scaling - get values from ini file
setp stepgen.0.position-scale [AXIS_0]SCALE
setp stepgen.1.position-scale [AXIS_1]SCALE
setp stepgen.2.position-scale [AXIS_2]SCALE
setp stepgen.3.position-scale [AXIS_3]SCALE

# set stepgen module accel limits - get values from ini file
# jmk said to set these to ini value + 5% to avoid stepgen bug
setp stepgen.0.maxaccel [AXIS_0]HAL_MAXACCEL
setp stepgen.1.maxaccel [AXIS_1]HAL_MAXACCEL
setp stepgen.2.maxaccel [AXIS_2]HAL_MAXACCEL
setp stepgen.3.maxaccel [AXIS_3]HAL_MAXACCEL

# end of basic machine

#######################################################
# Beginning of threading related stuff
#######################################################

# spindle speed control
net spindle-speed-cmd <= motion.spindle-speed-out
net spindle-speed-cmd => sim-encoder.0.speed

# spindle encoder
# connect encoder signals to encoder counter
net spindle-phase-A => encoder.0.phase-A
net spindle-phase-B => encoder.0.phase-B
net spindle-phase-Z => encoder.0.phase-Z

net spindle-phase-A <= sim-encoder.0.phase-A
net spindle-phase-B <= sim-encoder.0.phase-B
net spindle-phase-Z <= sim-encoder.0.phase-Z

# assume 120 ppr = 480 counts/rev for the spindle
setp sim-encoder.0.ppr 120
# iocontrol output is in rpm, but sim-encoder speed is rps
setp sim-encoder.0.scale 60
# scale encoder output to read in revolutions
# (that way thread pitches can be straightforward,
#  a 20 tpi thread would multiply the encoder output
#  by 1/20, etc)
setp encoder.0.position-scale 480

# encoder reset control
net spindle-index-enable <= motion.spindle-index-enable
net spindle-index-enable <=> encoder.0.index-enable


# report our revolution count to the motion controller
net spindle-pos <= encoder.0.position
net spindle-pos => motion.spindle-revs


#######################################################
# make vel and accel sigs for testing
#######################################################

# send the position commands thru differentiators to
# generate velocity and accel signals (for testing)

# define the signals, and hook them up
net Xpos-cmd => ddt.0.in
net Xvel <= ddt.0.out
net Xvel => ddt.1.in
net Xacc <= ddt.1.out

net Ypos-cmd => ddt.2.in
net Yvel <= ddt.2.out
net Yvel => ddt.3.in
net Yacc <= ddt.3.out

net Zpos-cmd => ddt.4.in
net Zvel <= ddt.4.out
net Zvel => ddt.5.in
net Zacc <= ddt.5.out

net Apos-cmd => ddt.6.in
net Avel <= ddt.6.out
net Avel => ddt.7.in
net Aacc <= ddt.7.out

# for spindle velocity estimate
loadrt lowpass count=1
net spindle-rpm-raw encoder.0.velocity
net spindle-rpm-raw lowpass.0.in
net spindle-rpm-filtered lowpass.0.out
setp lowpass.0.gain .03
addf lowpass.0 servo-thread

