
#########################################################################
#      		Loading real-time modules 				#
#########################################################################

loadrt [KINS]KINEMATICS
loadrt hal_gm
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadrt pid num_chan=3
loadrt not

loadusr halmeter
#loadusr halscope -f

#########################################################################
# 			Adding functions				#
#########################################################################

addf motion-command-handler 	servo-thread
addf motion-controller 		servo-thread

addf gm.0.read 			servo-thread -1
addf pid.0.do-pid-calcs         servo-thread
addf pid.1.do-pid-calcs         servo-thread
addf pid.2.do-pid-calcs         servo-thread
addf gm.0.write 		servo-thread -1

addf gm.0.RS485 		servo-thread -1
addf not.0 			servo-thread

#########################################################################
#				Card mgt				#
#########################################################################

setp gm.0.watchdog-enable 1
setp gm.0.watchdog-timeout-ns 3000000

net pwrEnable motion.motion-enabled => gm.0.power-enable

# create power fault loopback
net hw_fault gm.0.power-fault => not.0.in
net estop not.0.out => iocontrol.0.emc-enable-in

# connect enable signals for DAC and PID
net Xen joint.0.amp-enable-out => gm.0.dac.0.enable
net Xen => pid.0.enable
net Yen joint.1.amp-enable-out => gm.0.dac.1.enable
net Yen => pid.1.enable
net Zen joint.2.amp-enable-out => gm.0.dac.2.enable
net Zen => pid.2.enable

#########################################################################
#				Encoder setup				#
#########################################################################

# ################
# X [0] Axis
# ################

# encoder feedback
setp gm.0.encoder.0.counter-mode 0 	#0:quad, 1:stepDir
setp gm.0.encoder.0.index-enable 0 	#0:disable, 1:enable
setp gm.0.encoder.0.index-mode 	1 	#0:reset counter at index, 1:round position at index
setp gm.0.encoder.0.counts-per-rev 2000
setp gm.0.encoder.0.position-scale  [JOINT_0]INPUT_SCALE
setp gm.0.encoder.0.min-speed-estimate 0.1

# ################
# Y [1] Axis
# ################

# encoder feedback
setp gm.0.encoder.1.counter-mode 0 	#0:quad, 1:stepDir
setp gm.0.encoder.1.index-enable 0 	#0:disable, 1:enable
setp gm.0.encoder.1.index-mode 	1 	#0:reset counter at index, 1:round position at index
setp gm.0.encoder.1.counts-per-rev 2000
setp gm.0.encoder.1.position-scale  [JOINT_2]INPUT_SCALE
setp gm.0.encoder.1.min-speed-estimate 0.1

# ################
# Z [2] Axis
# ################

# encoder feedback
setp gm.0.encoder.2.counter-mode 0 	#0:quad, 1:stepDir
setp gm.0.encoder.2.index-enable 0 	#0:disable, 1:enable
setp gm.0.encoder.2.index-mode 	1 	#0:reset counter at index, 1:round position at index
setp gm.0.encoder.2.counts-per-rev 2000
setp gm.0.encoder.2.position-scale  [JOINT_2]INPUT_SCALE
setp gm.0.encoder.2.min-speed-estimate 0.1

#########################################################################
#				PID setup				#
#########################################################################

# ################
# X [0] Axis
# ################

# set PID loop gains from inifile
setp pid.0.Pgain [JOINT_0]P
setp pid.0.Igain [JOINT_0]I
setp pid.0.Dgain [JOINT_0]D
setp pid.0.bias [JOINT_0]BIAS
setp pid.0.FF0 [JOINT_0]FF0
setp pid.0.FF1 [JOINT_0]FF1
setp pid.0.FF2 [JOINT_0]FF2
setp pid.0.deadband [JOINT_0]DEADBAND
setp pid.0.maxoutput [JOINT_0]MAX_OUTPUT

# ################
# Y [1] Axis
# ################

# set PID loop gains from inifile
setp pid.1.Pgain [JOINT_1]P
setp pid.1.Igain [JOINT_1]I
setp pid.1.Dgain [JOINT_1]D
setp pid.1.bias [JOINT_1]BIAS
setp pid.1.FF0 [JOINT_1]FF0
setp pid.1.FF1 [JOINT_1]FF1
setp pid.1.FF2 [JOINT_1]FF2
setp pid.1.deadband [JOINT_1]DEADBAND
setp pid.1.maxoutput [JOINT_1]MAX_OUTPUT

# ################
# Z [2] Axis
# ################

# set PID loop gains from inifile
setp pid.2.Pgain [JOINT_2]P
setp pid.2.Igain [JOINT_2]I
setp pid.2.Dgain [JOINT_2]D
setp pid.2.bias [JOINT_2]BIAS
setp pid.2.FF0 [JOINT_2]FF0
setp pid.2.FF1 [JOINT_2]FF1
setp pid.2.FF2 [JOINT_2]FF2
setp pid.2.deadband [JOINT_2]DEADBAND
setp pid.2.maxoutput [JOINT_2]MAX_OUTPUT

#########################################################################
#        	 position reference and feedback connection		#
#########################################################################

# connect encoder feedback to PID
net Xpos-fb gm.0.encoder.0.position => pid.0.feedback
net Ypos-fb gm.0.encoder.1.position => pid.1.feedback
net Zpos-fb gm.0.encoder.2.position => pid.2.feedback

# connect position commands from motion module to PID
net Xpos-cmd joint.0.motor-pos-cmd => pid.0.command
net Ypos-cmd joint.1.motor-pos-cmd => pid.1.command
net Zpos-cmd joint.2.motor-pos-cmd => pid.2.command

# connect PID output to DAC
net Xdac-cmd pid.0.output  =>  gm.0.dac.0.value
net Ydac-cmd pid.1.output  =>  gm.0.dac.1.value
net Zdac-cmd pid.2.output  =>  gm.0.dac.2.value

# connect position feedback
net Xpos-cmd =>  joint.0.motor-pos-fb
net Ypos-cmd =>  joint.1.motor-pos-fb
net Zpos-cmd =>  joint.2.motor-pos-fb

#########################################################################
#		homing and limit switches				#
#########################################################################

net lim-sw-x-pos gm.0.joint.0.pos-lim-sw-in-not => joint.0.pos-lim-sw-in
net lim-sw-x-neg gm.0.joint.0.neg-lim-sw-in-not => joint.0.neg-lim-sw-in
net lim-sw-x-home gm.0.joint.0.home-sw-in-not => joint.0.home-sw-in

net lim-sw-y-pos gm.0.joint.1.pos-lim-sw-in-not => joint.1.pos-lim-sw-in
net lim-sw-y-neg gm.0.joint.1.neg-lim-sw-in-not => joint.1.neg-lim-sw-in
net lim-sw-y-home gm.0.joint.1.home-sw-in-not => joint.1.home-sw-in

net lim-sw-z-pos gm.0.joint.2.pos-lim-sw-in-not => joint.2.pos-lim-sw-in
net lim-sw-z-neg gm.0.joint.2.neg-lim-sw-in-not => joint.2.neg-lim-sw-in
net lim-sw-z-home gm.0.joint.2.home-sw-in-not => joint.2.home-sw-in
