90 lines
2.4 KiB
Plaintext
90 lines
2.4 KiB
Plaintext
# removeshort.hal
|
|
# (for use following core_sim.hal and simulated_home.hal)
|
|
# (this halfile should follow core_sim.hal)
|
|
|
|
# Demonstrate that hookup_moveoff.tcl works with
|
|
# non-shortcircuit hal configurations (like a servo)
|
|
|
|
# Also: use sample-hold (mux2) to simulate a system
|
|
# with encoder measuring output when power off too
|
|
|
|
# remove usual sim signals
|
|
delsig Xpos
|
|
delsig Ypos
|
|
delsig Zpos
|
|
|
|
# add pid components, and addf in right sequence
|
|
loadrt pid names=xpid,ypid,zpid
|
|
addf xpid.do-pid-calcs servo-thread 3
|
|
addf ypid.do-pid-calcs servo-thread 3
|
|
addf zpid.do-pid-calcs servo-thread 3
|
|
|
|
# add mux2 for sample-hold on output, and addf in right sequence
|
|
# to simulate system with encoder measuring output
|
|
loadrt mux2 names=xmux,ymux,zmux
|
|
addf xmux servo-thread 6
|
|
addf ymux servo-thread 6
|
|
addf zmux servo-thread 6
|
|
|
|
# note: pid has default PGain=1, Dgain=Igain=FF0=FF1=FF2=0
|
|
setp xpid.Pgain 0
|
|
setp ypid.Pgain 0
|
|
setp zpid.Pgain 0
|
|
|
|
# pass thru input to output by using FF0=1
|
|
setp xpid.FF0 1
|
|
setp ypid.FF0 1
|
|
setp zpid.FF0 1
|
|
|
|
# enable pid in usual manner
|
|
net xenable joint.0.amp-enable-out => xpid.enable
|
|
net yenable joint.1.amp-enable-out => ypid.enable
|
|
net zenable joint.2.amp-enable-out => zpid.enable
|
|
|
|
# connect without a shortcircuit using pass-thru pid
|
|
net Xpos-cmd <= joint.0.motor-pos-cmd
|
|
net Xpos-cmd => xpid.command
|
|
|
|
# sample-hold
|
|
net sample <= motion.motion-enabled
|
|
net sample => xmux.sel
|
|
net sample => ymux.sel
|
|
net sample => zmux.sel
|
|
#---------------------------------------
|
|
net Xonposition <= xpid.output
|
|
net Xonposition => xmux.in1
|
|
|
|
# hold position when power off (in0):
|
|
net Xpos-fb => xmux.in0
|
|
|
|
net Xpos-fb <= xmux.out
|
|
net Xpos-fb => joint.0.motor-pos-fb
|
|
net Xpos-fb => comp_x.in1
|
|
net Xpos-fb => ddt_x.in
|
|
#---------------------------------------
|
|
net Yonposition <= ypid.output
|
|
net Yonposition => ymux.in1
|
|
|
|
# hold position when power off (in0):
|
|
net Ypos-fb => ymux.in0
|
|
|
|
net Ypos-fb <= ymux.out
|
|
net Ypos-cmd <= joint.1.motor-pos-cmd
|
|
net Ypos-cmd => ypid.command
|
|
net Ypos-fb => joint.1.motor-pos-fb
|
|
net Ypos-fb => comp_y.in1
|
|
net Ypos-fb => ddt_y.in
|
|
#---------------------------------------
|
|
net Zonposition <= zpid.output
|
|
net Zonposition => zmux.in1
|
|
|
|
# hold position when power off (in0):
|
|
net Zpos-fb => zmux.in0
|
|
|
|
net Zpos-fb <= zmux.out
|
|
net Zpos-cmd <= joint.2.motor-pos-cmd
|
|
net Zpos-cmd => zpid.command
|
|
net Zpos-fb => joint.2.motor-pos-fb
|
|
net Zpos-fb => comp_z.in1
|
|
net Zpos-fb => ddt_z.in
|