108 lines
3.6 KiB
Plaintext
108 lines
3.6 KiB
Plaintext
# load realtime modules
|
|
loadrt [KINS]KINEMATICS
|
|
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
|
|
loadrt pid num_chan=2 debug=1
|
|
loadrt ddt count=4
|
|
loadrt pluto_servo
|
|
loadrt scale
|
|
loadrt mux2
|
|
loadrt debounce cfg=1
|
|
loadrt limit1 count=1
|
|
|
|
# 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 ddt.0 servo-thread
|
|
addf ddt.1 servo-thread
|
|
addf ddt.2 servo-thread
|
|
addf ddt.3 servo-thread
|
|
addf scale.0 servo-thread
|
|
addf mux2.0 servo-thread
|
|
addf debounce.0 servo-thread
|
|
addf limit1.0 servo-thread
|
|
|
|
setp pluto-servo.encoder.0.scale [JOINT_0]INPUT_SCALE
|
|
setp pluto-servo.encoder.1.scale [JOINT_2]INPUT_SCALE
|
|
setp pluto-servo.encoder.2.scale -4096
|
|
|
|
setp pluto-servo.pwm.0.scale 26 # volts supply
|
|
setp pluto-servo.pwm.1.scale 26
|
|
|
|
setp pid.0.maxoutput 26 # volts at motor
|
|
setp pid.1.maxoutput 26
|
|
|
|
# the values below come from the ini
|
|
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.1.Pgain [JOINT_2]P
|
|
setp pid.1.Igain [JOINT_2]I
|
|
setp pid.1.Dgain [JOINT_2]D
|
|
setp pid.1.bias [JOINT_2]BIAS
|
|
setp pid.1.FF0 [JOINT_2]FF0
|
|
setp pid.1.FF1 [JOINT_2]FF1
|
|
setp pid.1.FF2 [JOINT_2]FF2
|
|
setp pid.1.deadband [JOINT_2]DEADBAND
|
|
|
|
net Xpos-fb pluto-servo.encoder.0.position => pid.0.feedback joint.0.motor-pos-fb
|
|
net Zpos-fb pluto-servo.encoder.1.position => pid.1.feedback joint.2.motor-pos-fb
|
|
|
|
net Xpos-cmd joint.0.motor-pos-cmd => pid.0.command ddt.0.in
|
|
net Zpos-cmd joint.2.motor-pos-cmd => pid.1.command ddt.2.in
|
|
|
|
net Xvel-cmd pid.0.output => pluto-servo.pwm.0.value
|
|
net Zvel-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
|
|
|
|
# machine ON
|
|
net Xenable joint.0.amp-enable-out => pid.0.enable pluto-servo.pwm.0.enable pluto-servo.dout.09
|
|
net Zenable joint.2.amp-enable-out => pid.1.enable pluto-servo.pwm.1.enable
|
|
|
|
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
|
|
|
|
# spindle SSR
|
|
# net spindle-fwd spindle.0.forward pluto-servo.dout.00
|
|
|
|
setp debounce.0.delay 3
|
|
net Xswitch pluto-servo.din.00-not => debounce.0.0.in
|
|
net Xswitch-debounced debounce.0.0.out => joint.0.home-sw-in joint.0.pos-lim-sw-in
|
|
|
|
# tool loading loopback
|
|
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
|
|
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
|
|
|
|
# spindle speed control
|
|
setp pluto-servo.pwm.3.enable 1
|
|
|
|
# this needs scale/offset to give the right speeds, but 0 should be 0
|
|
net spindle-speed-cmd spindle.0.speed-out => limit1.0.in
|
|
net spindle-speed-cmd-limited limit1.0.out => scale.0.in
|
|
setp scale.0.gain 0.00026315789 # 1/3800
|
|
setp scale.0.offset 0.105
|
|
setp mux2.0.in0 0
|
|
setp limit1.0.max 1300
|
|
net spindle-speed-scaled scale.0.out => mux2.0.in1
|
|
net spindle-fwd spindle.0.forward => mux2.0.sel pluto-servo.dout.00
|
|
net spindle-pwm mux2.0.out => pluto-servo.pwm.3.value
|
|
|
|
# spindle position, for threading
|
|
net spindle-index spindle.0.index-enable <=> pluto-servo.encoder.2.index-enable
|
|
net spindle-pos pluto-servo.encoder.2.position => spindle.0.revs
|
|
net spindle-speed-fb pluto-servo.encoder.2.velocity => spindle.0.speed-in
|