rendered paste body# load realtime modules
# kinematics
loadrt trivkins
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD key=[EMCMOT]SHMEM_KEY
loadrt hal_parport cfg="0x378"
loadrt encoder num_chan=2
loadrt pid num_chan=2
#loadrt freqgen step_type=1,1
loadrt pwmgen output_type=2,2
loadrt blocks comp=2 constant=1 ddt=4
# define the order of execution for RT code
addf parport.0.read base-thread
addf encoder.update-counters base-thread
#addf freqgen.make-pulses base-thread
addf pwmgen.make-pulses base-thread
addf parport.0.write base-thread
addf encoder.capture-position servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf pid.0.do-pid-calcs servo-thread
addf pid.1.do-pid-calcs servo-thread
addf constant.0 servo-thread
addf comp.0 servo-thread
addf comp.1 servo-thread
#addf freqgen.update-freq servo-thread
addf pwmgen.update servo-thread
addf ddt.0 servo-thread
addf ddt.1 servo-thread
addf ddt.2 servo-thread
addf ddt.3 servo-thread
# hook stuff together
newsig Xpos-fb float
newsig Ypos-fb float
setp encoder.0.position-scale [AXIS_0]INPUT_SCALE
setp encoder.1.position-scale [AXIS_1]INPUT_SCALE
linkpp encoder.0.phase-A <= parport.0.pin-10-in
linkpp encoder.0.phase-B <= parport.0.pin-11-in
linkpp encoder.1.phase-A <= parport.0.pin-12-in
linkpp encoder.1.phase-B <= parport.0.pin-13-in
linksp Xpos-fb <= encoder.0.position
linksp Ypos-fb <= encoder.1.position
newsig Xvel-cmd float
newsig Yvel-cmd float
linksp Xpos-fb => pid.0.feedback
linksp Ypos-fb => pid.1.feedback
linksp Xpos-fb => axis.0.motor-pos-fb
linksp Ypos-fb => axis.1.motor-pos-fb
linksp Xvel-cmd <= pid.0.output
linksp Yvel-cmd <= pid.1.output
#linksp Xvel-cmd => freqgen.0.velocity
#linksp Yvel-cmd => freqgen.1.velocity
linksp Xvel-cmd => pwmgen.0.value
linksp Yvel-cmd => pwmgen.1.value
#newsig Xpwm bit
#newsig Ypwm bit
#linkps freqgen.0.count Xpwm
#linkps freqgen.1.count Ypwm
#setp constant.0.value 0.0
#linkpp constant.0.out comp.0.in0
#linksp constant.0.out comp.1.in0
#linksp Xvel-cmd comp.0.in1
#linksp Yvel-cmd comp.1.in1
#newsig Xdir bit
#newsig Ydir bit
#linkps comp.0.out => Xdir
#linkps comp.1.out => Ydir
#linksp Xdir => parport.0.pin-02-out
#linksp Xdir => parport.0.pin-04-out
#setp parport.0.pin-02-out-invert TRUE
#linksp Xpwm => parport.0.pin-03-out
#linksp Ydir => parport.0.pin-05-out
#linksp Ydir => parport.0.pin-07-out
#setp parport.0.pin-05-out-invert TRUE
#linksp Ypwm => parport.0.pin-06-out
linkpp pwmgen.0.up => parport.0.pin-03-out
linkpp pwmgen.0.down => parport.0.pin-02-out
linkpp pwmgen.1.up => parport.0.pin-04-out
linkpp pwmgen.1.down => parport.0.pin-05-out
setp pwmgen.0.max-dc 0.99000e+00
setp pwmgen.1.max-dc 0.99000e+00
newsig pwmena bit
linksp pwmena pwmgen.0.enable
linksp pwmena pwmgen.1.enable
sets pwmena true
setp pid.0.maxoutput 1.0
setp pid.1.maxoutput 1.0
# the values below come from the ini
setp pid.0.Pgain [AXIS_0]P
setp pid.0.Igain [AXIS_0]I
setp pid.0.Dgain [AXIS_0]D
setp pid.0.bias [AXIS_0]BIAS
setp pid.0.FF0 [AXIS_0]FF0
setp pid.0.FF1 [AXIS_0]FF1
# deadband should be just over 1 count
setp pid.0.deadband [AXIS_0]DEADBAND
setp pid.1.Pgain [AXIS_1]P
setp pid.1.Igain [AXIS_1]I
setp pid.1.Dgain [AXIS_1]D
setp pid.1.bias [AXIS_1]BIAS
setp pid.1.FF0 [AXIS_1]FF0
setp pid.1.FF1 [AXIS_1]FF1
# deadband should be just over 1 count
setp pid.1.deadband [AXIS_1]DEADBAND
newsig Xpos-cmd float
newsig Ypos-cmd float
linksp Xpos-cmd <= axis.0.motor-pos-cmd
linksp Ypos-cmd <= axis.1.motor-pos-cmd
linkps pid.0.command <= Xpos-cmd
linkps pid.1.command <= Ypos-cmd
linkpp pid.0.enable axis.0.amp-enable-out
linkpp pid.1.enable axis.1.amp-enable-out
linkpp iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
newsig Xvel float
newsig Xacc float
linksp Xpos-cmd => ddt.0.in
linkps ddt.0.out => Xvel
linksp Xvel => ddt.1.in
linkps ddt.1.out => Xacc
newsig Yvel float
newsig Yacc float
linksp Ypos-cmd => ddt.2.in
linkps ddt.2.out => Yvel
linksp Yvel => ddt.3.in
linkps ddt.3.out => Yacc
# These depend on the rounded BASE_PERIOD
#setp freqgen.0.velocity-scale 19885
#setp freqgen.1.velocity-scale 19885
#setp freqgen.0.velocity-scale 49885
#setp freqgen.1.velocity-scale 49885
setp pwmgen.0.scale 8.3
setp pwmgen.1.scale 8.3
linkpp iocontrol.0.tool-prepare iocontrol.0.tool-prepared
linkpp iocontrol.0.tool-change iocontrol.0.tool-changed
loadrt scope_rt
#loadusr halscope