# 5AX MPG

loadrt mux4 names=mpg-scale
addf mpg-scale servo-thread

loadrt ilowpass
addf ilowpass.0 servo-thread

loadrt or2 count=2
addf or2.0 servo-thread
addf or2.1 servo-thread

setp ilowpass.0.gain 0.02
setp ilowpass.0.scale 1
setp mpg-scale.in0 0.0
setp mpg-scale.in1 0.001
setp mpg-scale.in2 0.010
setp mpg-scale.in3 0.050

setp axis.x.jog-vel-mode 	1
setp axis.y.jog-vel-mode 	1
setp axis.z.jog-vel-mode 	1
setp axis.a.jog-vel-mode 	1
setp axis.b.jog-vel-mode 	1

setp axis.x.jog-accel-fraction 0.8
setp axis.y.jog-accel-fraction 0.8
setp axis.z.jog-accel-fraction 0.8
setp axis.a.jog-accel-fraction 0.8
setp axis.b.jog-accel-fraction 0.8

net x-jog-select 	axis.x.jog-enable  joint.0.jog-enable <= rio.mpg_x_sel.bit
net y-jog-select 	axis.y.jog-enable  joint.1.jog-enable <= rio.mpg_y_sel.bit
net z-jog-select 	axis.z.jog-enable  joint.2.jog-enable <= rio.mpg_z_sel.bit
net a-jog-select 	axis.a.jog-enable  joint.3.jog-enable <= rio.mpg_a_sel.bit
net b-jog-select 	axis.b.jog-enable  joint.4.jog-enable <= rio.mpg_b_sel.bit

net scale1 or2.0.in0 <= rio.mpg_scale_x1.bit
net scale2 or2.1.in0 <= rio.mpg_scale_x10.bit
net scale3 or2.0.in1 or2.1.in1 <= rio.mpg_scale_x100.bit

#setp [CARD](NAME).encoder.00.x4-mode 0
net jog-smoothing ilowpass.0.in <= rio.mpg_encoder.position-s32
net jog-position <= ilowpass.0.out
net jog-position => axis.x.jog-counts => joint.0.jog-counts
net jog-position => axis.y.jog-counts => joint.1.jog-counts
net jog-position => axis.z.jog-counts => joint.2.jog-counts
net jog-position => axis.a.jog-counts => joint.3.jog-counts
net jog-position => axis.b.jog-counts => joint.4.jog-counts

net pos2 mpg-scale.sel0 <= or2.0.out
net pos3 mpg-scale.sel1 <= or2.1.out

net axis-mpg-jog-scale 	axis.x.jog-scale  joint.0.jog-scale <= mpg-scale.out
net axis-mpg-jog-scale 	axis.y.jog-scale  joint.1.jog-scale <= mpg-scale.out
net axis-mpg-jog-scale 	axis.z.jog-scale  joint.2.jog-scale <= mpg-scale.out
net axis-mpg-jog-scale 	axis.a.jog-scale  joint.3.jog-scale <= mpg-scale.out
net axis-mpg-jog-scale 	axis.b.jog-scale  joint.4.jog-scale <= mpg-scale.out