180 lines
5.9 KiB
Plaintext
180 lines
5.9 KiB
Plaintext
|
|
#########################################################################
|
|
# 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
|