rendered paste body# load realtime modules
loadrt trivkins
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD key=[EMCMOT]SHMEM_KEY num_joints=[TRAJ]AXES
loadrt pid num_chan=2
loadrt ddt count=4
loadrt pluto_servo epp_wide=0
loadrt constant count=1
loadrt comp count=1
loadrt not
# define the order of execution for RT code
addf pluto-servo.read 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 pluto-servo.write servo-thread
addf constant.0 servo-thread
addf comp.0 servo-thread
addf not.0 servo-thread
addf ddt.0 servo-thread
addf ddt.1 servo-thread
addf ddt.2 servo-thread
addf ddt.3 servo-thread
setp constant.0.value 0.0
setp pluto-servo.pwm.is-pdm 0
setp pluto-servo.pwm.0.pwmdir TRUE
setp pluto-servo.pwm.1.pwmdir TRUE
setp pluto-servo.encoder.0.scale [AXIS_0]INPUT_SCALE
setp pluto-servo.encoder.1.scale [AXIS_1]INPUT_SCALE
setp pluto-servo.pwm.0.scale 8.33 # volts supply
setp pluto-servo.pwm.1.scale 8.33
setp pid.0.maxoutput 8.33
setp pid.1.maxoutput 8.33
# 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
setp pid.0.FF2 [AXIS_0]FF2
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
setp pid.1.FF2 [AXIS_1]FF2
setp pid.1.deadband [AXIS_1]DEADBAND
net Xpos-fb pluto-servo.encoder.0.position => pid.0.feedback axis.0.motor-pos-fb
net Ypos-fb pluto-servo.encoder.1.position => pid.1.feedback axis.1.motor-pos-fb
net Xpos-cmd axis.0.motor-pos-cmd => pid.0.command ddt.0.in
net Ypos-cmd axis.1.motor-pos-cmd => pid.1.command ddt.2.in
net Xvel-cmd pid.0.output => pluto-servo.pwm.0.value comp.0.in1
net Yvel-cmd pid.1.output => pluto-servo.pwm.1.value
net Xvel ddt.0.out => ddt.1.in
net Xacc ddt.1.out
net Zvel ddt.2.out => ddt.3.in
net Zacc ddt.3.out
#setp pluto-servo.dout.00-invert 1
#newsig dir bit
#linksp dir pluto-servo.dout.00
#linksp dir pluto-servo.dout.11
net zero-signal constant.0.out comp.0.in0
newsig Xdir bit
linksp Xdir comp.0.out
linksp Xdir pluto-servo.dout.00
linksp Xdir not.0.in
net Xdir-not not.0.out pluto-servo.dout.01
# machine ON
net Xenable axis.0.amp-enable-out => pid.0.enable pluto-servo.pwm.0.enable
net Zenable axis.1.amp-enable-out => pid.1.enable pluto-servo.pwm.1.enable
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in