101 lines
3.1 KiB
Plaintext
101 lines
3.1 KiB
Plaintext
# core HAL config file for steppers
|
|
|
|
# first load the core RT modules that will be needed
|
|
# kinematics
|
|
loadrt [KINS]KINEMATICS
|
|
# motion controller, get name and thread periods from ini file
|
|
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
|
|
# stepper module
|
|
loadrt stepgen step_type=0,0,0,0
|
|
|
|
# hook its functions to realtime threads
|
|
addf stepgen.make-pulses 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
|
|
|
|
# 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
|
|
|
|
# send the position commands thru differentiators to
|
|
# generate velocity and accel signals
|
|
# first 8 differentiators
|
|
loadrt ddt count=8
|
|
# link the differentiator functions into the code
|
|
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
|
|
# 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
|
|
|
|
# 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
|
|
|
|
# set stepgen module scaling - get values from ini file
|
|
setp stepgen.0.position-scale [JOINT_0]SCALE
|
|
setp stepgen.1.position-scale [JOINT_1]SCALE
|
|
setp stepgen.2.position-scale [JOINT_2]SCALE
|
|
setp stepgen.3.position-scale [JOINT_3]SCALE
|
|
|
|
# set stepgen module accel limits - get values from ini file
|
|
setp stepgen.0.maxaccel [JOINT_0]STEPGEN_MAXACCEL
|
|
setp stepgen.1.maxaccel [JOINT_1]STEPGEN_MAXACCEL
|
|
setp stepgen.2.maxaccel [JOINT_2]STEPGEN_MAXACCEL
|
|
setp stepgen.3.maxaccel [JOINT_3]STEPGEN_MAXACCEL
|