# core HAL config file for simulation - 6 axis

# load RT modules
loadrt pumakins
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD num_joints=[TRAJ]AXES
loadrt ddt count=12

# add motion controller functions to servo thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
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
addf ddt.8 servo-thread

# create HAL signals for position commands from motion module
net J0pos <= axis.0.motor-pos-cmd
net J1pos <= axis.1.motor-pos-cmd
net J2pos <= axis.2.motor-pos-cmd
net J3pos <= axis.3.motor-pos-cmd
net J4pos <= axis.4.motor-pos-cmd
net J5pos <= axis.5.motor-pos-cmd


# loop position commands back to motion module feedback
net J0pos => axis.0.motor-pos-fb
net J1pos => axis.1.motor-pos-fb
net J2pos => axis.2.motor-pos-fb
net J3pos => axis.3.motor-pos-fb
net J4pos => axis.4.motor-pos-fb
net J5pos => axis.5.motor-pos-fb

# send the position commands thru differentiators to
# generate velocity and accel signals
# define the signals, and hook them up
net J0pos => ddt.0.in
net J0vel <= ddt.0.out
net J0vel => ddt.1.in
net J0acc <= ddt.1.out

net J1pos => ddt.2.in
net J1vel <= ddt.2.out
net J1vel => ddt.3.in
net J1acc <= ddt.3.out

net J2pos => ddt.4.in
net J2vel <= ddt.4.out
net J2vel => ddt.5.in
net J2acc <= ddt.5.out

net J3pos => ddt.6.in
net J3vel <= ddt.6.out
net J3vel => ddt.7.in
net J3acc <= ddt.7.out

net J4pos => ddt.8.in
net J4vel <= ddt.8.out
net J4vel => ddt.9.in
net J4acc <= ddt.9.out

net J5pos => ddt.10.in
net J5vel <= ddt.10.out
net J5vel => ddt.11.in
net J5acc <= ddt.11.out

# 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

# amp control
net xena axis.0.amp-enable-out
net yena axis.1.amp-enable-out
net zena axis.2.amp-enable-out
net aena axis.3.amp-enable-out
net bena axis.4.amp-enable-out
net cena axis.5.amp-enable-out

net xflt axis.0.amp-fault-in
net yflt axis.1.amp-fault-in
net zflt axis.2.amp-fault-in
net aflt axis.3.amp-fault-in
net bflt axis.4.amp-fault-in
net cflt axis.5.amp-fault-in

loadusr -W pumagui

loadrt scale count=6

addf scale.0 servo-thread
addf scale.1 servo-thread
addf scale.2 servo-thread
addf scale.3 servo-thread
addf scale.4 servo-thread
addf scale.5 servo-thread

net J0pos scale.0.in
net J1pos scale.1.in
net J2pos scale.2.in
net J3pos scale.3.in
net J4pos scale.4.in
net J5pos scale.5.in

setp scale.0.gain 1
setp scale.1.gain 1
setp scale.2.gain 1
setp scale.3.gain 1
setp scale.4.gain 1
setp scale.5.gain 1

net J0scaled scale.0.out pumagui.joint1
net J1scaled scale.1.out pumagui.joint2
net J2scaled scale.2.out pumagui.joint3
net J3scaled scale.3.out pumagui.joint4
net J4scaled scale.4.out pumagui.joint5
net J5scaled scale.5.out pumagui.joint6

#net j0 axis.0.joint-pos-fb pumagui.joint1
#net j1 axis.1.joint-pos-fb pumagui.joint2
#net j2 axis.2.joint-pos-fb pumagui.joint3
#net j3 axis.3.joint-pos-fb pumagui.joint4
#net j4 axis.4.joint-pos-fb pumagui.joint5
#net j5 axis.5.joint-pos-fb pumagui.joint6
#net j6 axis.6.joint-pos-fb pumagui.grip