def setupGyro(thread='base_thread'):
name = 'balance'
sigReq = hal.newsig('%s-req' % name, hal.HAL_BIT)
sigAck = hal.newsig('%s-ack' % name, hal.HAL_BIT)
sigDt = hal.newsig('%s-dt' % name, hal.HAL_FLOAT)
sigNewAngle = hal.newsig('%s-new-angle' % name, hal.HAL_FLOAT)
sigNewRate = hal.newsig('%s-new-rate' % name, hal.HAL_FLOAT)
gyroaccel = hal.loadusr('./hal_gyroaccel', name='gyroaccel',
bus_id=1, interval=0.05,
wait_name='gyroaccel')
gyroaccel.pin('req').link(sigReq)
gyroaccel.pin('ack').link(sigAck)
gyroaccel.pin('dt').link(sigDt)
gyroaccel.pin('angle').link(sigNewAngle)
gyroaccel.pin('rate').link(sigNewRate)
gyroaccel.pin('invert').set(True) # invert the output since we mounted the gyro upside down
kalman = rt.loadrt('kalman', 'names=kalman')
hal.addf(kalman.name, thread)
kalman.pin('req').link(sigReq)
kalman.pin('ack').link(sigAck)
kalman.pin('dt').link(sigDt)
kalman.pin('new-angle').link(sigNewAngle)
kalman.pin('new-rate').link(sigNewRate)
def setup_fan(name, thread):
setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT)
pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT)
enable = hal.newsig('%s-enable' % name, hal.HAL_BIT)
# reset fan when estop is cleared
reset = rt.newinst('reset', 'reset.%s-set' % name)
hal.addf(reset.name, thread)
reset.pin('reset-float').set(0.0)
reset.pin('out-float').link(setSig)
reset.pin('rising').set(True)
reset.pin('falling').set(False)
reset.pin('trigger').link('estop-reset')
scale = rt.newinst('scale', 'scale.%s' % name)
hal.addf(scale.name, thread)
scale.pin('in').link(setSig)
scale.pin('out').link(pwmSig)
scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion
setSig.set(0.0)
enable.set(True)
rcomps.create_fan_rcomp(name)
motion.setup_fan_io(name)
def usrcomp_watchdog(comps,
enableSignal,
thread,
okSignal=None,
errorSignal=None):
count = len(comps)
watchdog = rt.loadrt('watchdog', num_inputs=count)
hal.addf('watchdog.set-timeouts', thread)
hal.addf('watchdog.process', thread)
for n, comp in enumerate(comps):
compname = comp[0]
comptime = comp[1]
sigIn = hal.newsig('%s-watchdog' % compname, hal.HAL_BIT)
hal.Pin('%s.watchdog' % compname).link(sigIn)
watchdog.pin('input-%i' % n).link(sigIn)
watchdog.pin('timeout-%i' % n).set(comptime)
watchdog.pin('enable-in').link(enableSignal)
if not okSignal:
okSignal = hal.newsig('watchdog-ok', hal.HAL_BIT)
watchdog.pin('ok-out').link(okSignal)
if errorSignal:
notComp = rt.newinst('not', 'not.watchdog-error')
hal.addf(notComp.name, thread)
notComp.pin('in').link(okSignal)
notComp.pin('out').link(errorSignal)
def setupGyro(thread='base_thread'):
name = 'balance'
sigReq = hal.newsig('%s-req' % name, hal.HAL_BIT)
sigAck = hal.newsig('%s-ack' % name, hal.HAL_BIT)
sigDt = hal.newsig('%s-dt' % name, hal.HAL_FLOAT)
sigNewAngle = hal.newsig('%s-new-angle' % name, hal.HAL_FLOAT)
sigNewRate = hal.newsig('%s-new-rate' % name, hal.HAL_FLOAT)
gyroaccel = hal.loadusr('./hal_gyroaccel',
name='gyroaccel',
bus_id=1,
interval=0.05,
wait_name='gyroaccel')
gyroaccel.pin('req').link(sigReq)
gyroaccel.pin('ack').link(sigAck)
gyroaccel.pin('dt').link(sigDt)
gyroaccel.pin('angle').link(sigNewAngle)
gyroaccel.pin('rate').link(sigNewRate)
gyroaccel.pin('invert').set(
True) # invert the output since we mounted the gyro upside down
kalman = rt.loadrt('kalman', 'names=kalman')
hal.addf(kalman.name, thread)
kalman.pin('req').link(sigReq)
kalman.pin('ack').link(sigAck)
kalman.pin('dt').link(sigDt)
kalman.pin('new-angle').link(sigNewAngle)
kalman.pin('new-rate').link(sigNewRate)
def setup_fans(replicape):
if NUM_FANS == 0:
return
en = config.find('FDM','SYSTEM_FAN', 0)
fan_out = [None] * NUM_FANS
fan_scale = [None] * NUM_FANS
fan_in = [None] * NUM_FANS
#on-board pwm input is 0.0 to 1.0, M106 sends 0 to 255
for i in xrange(NUM_FANS):
fan_out[i] = hal.newsig('fan-output-%d' % i, hal.HAL_FLOAT)
replicape.get_fan_pwm_pin(i).link(fan_out[i])
# the system fan is connected to the last fan pwm output
if (int(en) > 0) and (i == NUM_FANS-1):
fan_out[i].set(1.0)
else:
fan_in[i] = hal.newsig('fan-input-%d' % i, hal.HAL_FLOAT)
fan_in[i].link('motion.analog-out-io-%d' % (FAN_IO_START + i))
fan_scale[i] = rtapi.newinst('div2', 'fan%d.div2.scale-pwm' % i)
hal.addf(fan_scale[i].name, SERVO_THREAD)
fan_scale[i].pin('in0').link(fan_in[i])
fan_scale[i].pin('in1').set(255.0)
fan_scale[i].pin('out').link(fan_out[i])
comp = hal.RemoteComponent('fdm-f%d' % i, timer=100)
comp.newpin('set',hal.HAL_FLOAT, hal.HAL_IO)
comp.ready()
comp.pin('set').link(fan_in[i])
def test_newsig():
l.log()
hal.newsig("floatsig1", hal.HAL_FLOAT)
try:
hal.newsig("floatsig1", hal.HAL_FLOAT)
# RuntimeError: Failed to create signal floatsig1: HAL: ERROR: duplicate signal 'floatsig1'
raise "should not happen"
except RuntimeError:
pass
try:
hal.newsig(32423 * 32432, hal.HAL_FLOAT)
raise "should not happen"
except TypeError:
pass
try:
hal.newsig(None, hal.HAL_FLOAT)
raise "should not happen"
except TypeError:
pass
try:
hal.newsig("badtype", 1234)
raise "should not happen"
except TypeError:
pass
def setup_fan(name, thread):
setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT)
pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT)
enable = hal.newsig('%s-enable' % name, hal.HAL_BIT)
# reset fan when estop is cleared
reset = rt.newinst('reset', 'reset.%s-set' % name)
hal.addf(reset.name, thread)
reset.pin('reset-float').set(0.0)
reset.pin('out-float').link(setSig)
reset.pin('rising').set(True)
reset.pin('falling').set(False)
reset.pin('trigger').link('estop-reset')
scale = rt.newinst('scale', 'scale.%s' % name)
hal.addf(scale.name, thread)
scale.pin('in').link(setSig)
scale.pin('out').link(pwmSig)
scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion
setSig.set(0.0)
enable.set(True)
rcomps.create_fan_rcomp(name)
motion.setup_fan_io(name)
def test_newsig():
l.log()
hal.newsig("floatsig1", hal.HAL_FLOAT)
try:
hal.newsig("floatsig1", hal.HAL_FLOAT)
# RuntimeError: Failed to create signal floatsig1: HAL: ERROR: duplicate signal 'floatsig1'
raise "should not happen"
except RuntimeError:
pass
try:
hal.newsig(32423 * 32432, hal.HAL_FLOAT)
raise "should not happen"
except TypeError:
pass
try:
hal.newsig(None, hal.HAL_FLOAT)
raise "should not happen"
except TypeError:
pass
try:
hal.newsig("badtype", 1234)
raise "should not happen"
except TypeError:
pass
def insert_jplanners():
mod_success = hal.newsig("mod_success", hal.HAL_BIT)
mod_success.set(0)
# check if 'pbmsgs' component exists
c = hal.components
if 'pbmsgs' not in c:
rt.loadrt('pbmsgs')
# create 'series' signal
series = hal.newsig("series", hal.HAL_U32)
series.set(0)
rt.newinst('ornv2', 'jplanners_active', pincount=8)
# start changing the HAL configuration
for i in range(1,7):
# create jplanner
rt.newinst('jplan', 'jp%s' %i)
hal.addf('jp%s.update' %i, 'robot_hw_thread', 68+i)
hal.Pin('jp%s.0.active' %i).link('jplanners_active.in%s' %(i-1))
time.sleep(0.005)
# copy current position
hal.Pin('jp%s.0.pos-cmd' %i).set(hal.Pin('hal_hw_interface.joint_%s.pos-fb' %i).get())
# set values for jplanner
hal.Pin('jp%s.0.enable' %i).set(1)
hal.Pin('jp%s.0.max-acc' %i).set(0.1)
hal.Pin('jp%s.0.max-vel' %i).set(0.1)
# get component to insert _after_
source = 'hal_hw_interface.joint_%s.pos-cmd' %i
rt.newinst('mux2v2', 'joint%s_mux' %i)
hal.addf('joint%s_mux.funct' %i, 'robot_hw_thread', 68+2*i)
time.sleep(0.005)
# insert the mux component _after_ source component
# the target1 is the new pin to be connected to the source component
# the source1 is the pin the existing signals are to be connected to
insert_component(source, 'joint%s_mux.in0' %i, 'joint%s_mux.out' %i)
# connect to the inserted mux component
hal.Pin('jp%s.0.curr-pos' %i).link('joint%s_mux.in1' %i)
# create sample_channel
rt.newinst('sample_channel_pb', 'sampler%s' %i, '--', 'samples=bfu','names=sensor,rotation,series','cycles=2000')
hal.addf('sampler%s.record_sample' %i, 'robot_hw_thread')
hal.Signal('joint%s_ros_pos_fb' %i).link('sampler%s.in-flt.1' %i)
hal.Pin('lcec.0.6.din-7').link('sampler%s.in-bit.1' %i)
# link series signal to sample_channel series pin
series.link('sampler%s.in-u32.1' %i)
# select the jplanner channel
hal.Pin('joint%s_mux.sel' %i).set(1)
# leave info in HAL that this script was succesful
mod_success.set(1)
hal.addf('jplanners_active.funct', 'robot_hw_thread', 75)
def test_net_existing_signal_with_bad_type():
hal.newsig("f", hal.HAL_FLOAT)
try:
hal.net("f", "c1.s32out")
raise "should not happen"
except TypeError:
pass
del hal.signals["f"]
def test_net_existing_signal_with_bad_type():
hal.newsig("f", hal.HAL_FLOAT)
try:
hal.net("f", "c1.s32out")
raise "should not happen"
except TypeError:
pass
del hal.signals["f"]
def setup_light(name, thread):
for color in ('r', 'g', 'b', 'w'):
inSig = hal.newsig('%s-%s' % (name, color), hal.HAL_FLOAT)
outSig = hal.newsig('%s-%s-out' % (name, color), hal.HAL_FLOAT)
ledDim = rt.newinst('led_dim', 'led-dim.%s-%s' % (name, color))
hal.addf(ledDim.name, thread)
ledDim.pin('in').link(inSig)
ledDim.pin('out').link(outSig)
rcomps.create_light_rcomp(name)
storage.setup_light_storage(name)
motion.setup_light_io(name)
def setup_light(name, thread):
for color in ('r', 'g', 'b', 'w'):
inSig = hal.newsig('%s-%s' % (name, color), hal.HAL_FLOAT)
outSig = hal.newsig('%s-%s-out' % (name, color), hal.HAL_FLOAT)
ledDim = rt.newinst('led_dim', 'led-dim.%s-%s' % (name, color))
hal.addf(ledDim.name, thread)
ledDim.pin('in').link(inSig)
ledDim.pin('out').link(outSig)
rcomps.create_light_rcomp(name)
storage.setup_light_storage(name)
motion.setup_light_io(name)
def setup_probe(thread):
probeEnable = hal.newsig('probe-enable', hal.HAL_BIT)
probeInput = hal.newsig('probe-input', hal.HAL_BIT)
probeSignal = hal.newsig('probe-signal', hal.HAL_BIT)
and2 = rt.newinst('and2', 'and2.probe-input')
hal.addf(and2.name, thread)
and2.pin('in0').link(probeSignal)
and2.pin('in1').link(probeEnable)
and2.pin('out').link(probeInput)
probeInput += 'motion.probe-input'
motion.setup_probe_io()
def setup_probe(thread):
probeEnable = hal.newsig('probe-enable', hal.HAL_BIT)
probeInput = hal.newsig('probe-input', hal.HAL_BIT)
probeSignal = hal.newsig('probe-signal', hal.HAL_BIT)
and2 = rt.newinst('and2', 'and2.probe-input')
hal.addf(and2.name, thread)
and2.pin('in0').link(probeSignal)
and2.pin('in1').link(probeEnable)
and2.pin('out').link(probeInput)
probeInput += 'motion.probe-input'
motion.setup_probe_io()
def test_net_existing_signal():
hal.newsig("s32", hal.HAL_S32)
assert hal.pins["c1.s32out"].linked == False
hal.net("s32", "c1.s32out")
assert hal.pins["c1.s32out"].linked == True
hal.newsig("s32too", hal.HAL_S32)
try:
hal.net("s32too", "c1.s32out")
raise "should not happen"
except RuntimeError:
pass
del hal.signals["s32"]
def test_net_existing_signal():
hal.newsig("s32", hal.HAL_S32)
assert hal.pins["c1.s32out"].linked == False
hal.net("s32", "c1.s32out")
assert hal.pins["c1.s32out"].linked == True
hal.newsig("s32too", hal.HAL_S32)
try:
hal.net("s32too", "c1.s32out")
raise "should not happen"
except RuntimeError:
pass
del hal.signals["s32"]
def _setup_usrcomp_watchdog(comps, thread):
power_on = hal.Signal('power-on', hal.HAL_BIT)
watchdog_ok = hal.Signal('watchdog-ok', hal.HAL_BIT)
watchdog_error_raw = hal.Signal('watchdog-error-raw', hal.HAL_BIT)
watchdog_error = hal.Signal('watchdog-error', hal.HAL_BIT)
watchdog = rt.newinst('watchdog',
'watchdog.usrcomp',
pincount=len(comps))
hal.addf('{}.set-timeouts'.format(watchdog.name), thread.name)
hal.addf('{}.process'.format(watchdog.name), thread.name)
for n, comp in enumerate(comps):
sig_in = hal.newsig('{}-watchdog'.format(comp.name), hal.HAL_BIT)
hal.Pin('{}.watchdog'.format(comp.name)).link(sig_in)
watchdog.pin('input-{:02}'.format(n)).link(sig_in)
watchdog.pin('timeout-{:02}'.format(n)).set(comp.timeout)
watchdog.pin('enable-in').link(power_on)
watchdog.pin('ok-out').link(watchdog_ok)
not_comp = rt.newinst('not', 'not.watchdog-error')
hal.addf(not_comp.name, thread.name)
not_comp.pin('in').link(watchdog_ok)
not_comp.pin('out').link(watchdog_error_raw)
and2 = rt.newinst('and2', 'and2.watchdog-error')
hal.addf(and2.name, thread.name)
and2.pin('in0').link(watchdog_error_raw)
and2.pin('in1').link(power_on)
and2.pin('out').link(watchdog_error)
def setup_fan(name, thread):
setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT)
pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT)
enable = hal.newsig('%s-enable' % name, hal.HAL_BIT)
scale = rt.newinst('scale', 'scale.%s' % name)
hal.addf(scale.name, thread)
scale.pin('in').link(setSig)
scale.pin('out').link(pwmSig)
scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion
setSig.set(0.0)
enable.set(True)
rcomps.create_fan_rcomp(name)
motion.setup_fan_io(name)
def setup_fan(name, thread):
setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT)
pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT)
enable = hal.newsig('%s-enable' % name, hal.HAL_BIT)
scale = rt.newinst('scale', 'scale.%s' % name)
hal.addf(scale.name, thread)
scale.pin('in').link(setSig)
scale.pin('out').link(pwmSig)
scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion
setSig.set(0.0)
enable.set(True)
rcomps.create_fan_rcomp(name)
motion.setup_fan_io(name)
def __init__(self):
self.pru = rtapi.loadrt('hal_pru_generic',
pru=0, num_stepgens=5, num_pwmgens=0, halname='hpg',
prucode='%s/xenomai/pru_generic.bin' % (config.Config().EMC2_RTLIB_DIR))
hal.addf('hpg.capture-position', SERVO_THREAD)
hal.addf('hpg.update', SERVO_THREAD)
hal.addf('bb_gpio.read', SERVO_THREAD)
hal.addf('bb_gpio.write', SERVO_THREAD)
for i in xrange(5):
self.get_pru_pin('stepgen.%02i.dirsetup' % i).set(200)
self.get_pru_pin('stepgen.%02i.dirhold' % i).set(200)
self.get_pru_pin('stepgen.%02i.steplen' % i).set(1000)
self.get_pru_pin('stepgen.%02i.stepspace' % i).set(1000)
self.get_pru_pin('stepgen.%02i.dirpin' % i).set(self.pru_dir_pin(i))
self.get_pru_pin('stepgen.%02i.steppin' % i).set(self.pru_step_pin(i))
self.get_pru_pin('stepgen.%02i.maxvel' % i).set(0)
self.get_pru_pin('stepgen.%02i.maxaccel' % i).set(0)
self.pwm = hal.loadusr(USR_HAL_PATH + 'hal_replicape_pwm',
name='replicape_pwm',
wait_name='replicape_pwm')
self.watchdog_sigs = []
for pin in self.get_watchdog_pins():
s = hal.newsig('replicape.watchdog.%d' % len(self.watchdog_sigs), hal.HAL_BIT)
pin.link(s)
self.watchdog_sigs.append(s)
def test_net_existing_signal(self):
l.log()
hal.newsig("s32", hal.HAL_S32)
assert hal.pins["c1.s32out"].linked is False
hal.net("s32", "c1.s32out")
assert hal.pins["c1.s32out"].linked is True
hal.newsig("s32too", hal.HAL_S32)
try:
hal.net("s32too", "c1.s32out")
raise "should not happen"
except RuntimeError:
pass
del hal.signals["s32"]
assert 's32' not in hal.signals
def setup_servo_axis(servoIndex, section, axisIndex, pwm, thread=None):
servo = '%s.%02i' % ('rc_servo', servoIndex)
scale = rt.newinst('scale', '%s-scale' % servo)
hal.addf(scale.name, thread)
limit1 = rt.newinst('limit1', '%s-limit' % servo)
hal.addf(limit1.name, thread)
enable = hal.newsig('emcmot-%i-enable' % axisIndex, hal.HAL_BIT)
enable.set(False)
hal.Pin('%s.enable' % pwm).link(enable)
scale.pin('out').link(limit1.pin('in'))
pwmout = hal.newsig('%s-pwm-out' % servo, hal.HAL_FLOAT)
limit1.pin('out').link(pwmout)
hal.Pin('%s.value' % pwm).link(pwmout)
axis = 'axis.%i' % axisIndex
enable = hal.Signal('emcmot-%i-enable' % axisIndex, hal.HAL_BIT)
enable.link('%s.amp-enable-out' % axis)
# expose timing parameters so we can multiplex them later
sigBase = 'rc-servo-%i' % servoIndex
scale = hal.newsig('%s-scale' % sigBase, hal.HAL_FLOAT)
offset = hal.newsig('%s-offset' % sigBase, hal.HAL_FLOAT)
smin = hal.newsig('%s-min' % sigBase, hal.HAL_FLOAT)
smax = hal.newsig('%s-max' % sigBase, hal.HAL_FLOAT)
hal.Pin('%s-scale.gain' % servo).link(scale)
hal.Pin('%s-scale.offset' % servo).link(offset)
hal.Pin('%s-limit.min' % servo).link(smin)
hal.Pin('%s-limit.max' % servo).link(smax)
scale.set(c.find(section, 'SCALE', 0.000055556))
offset.set(c.find(section, 'SERVO_OFFSET', .003))
smin.set(c.find(section, 'SERVO_MIN', 0.02))
smax.set(c.find(section, 'SERVO_MAX', 0.04))
posCmd = hal.newsig('emcmot-%i-pos-cmd' % axisIndex, hal.HAL_FLOAT)
posCmd.link('%s.motor-pos-cmd' % axis)
posCmd.link('%s-scale.in' % servo)
posCmd.link('%s.motor-pos-fb' % axis)
limitHome = hal.newsig('limit-%i-home' % axisIndex, hal.HAL_BIT)
limitMin = hal.newsig('limit-%i-min' % axisIndex, hal.HAL_BIT)
limitMax = hal.newsig('limit-%i-max' % axisIndex, hal.HAL_BIT)
limitHome.link('%s.home-sw-in' % axis)
limitMin.link('%s.neg-lim-sw-in' % axis)
limitMax.link('%s.pos-lim-sw-in' % axis)
def setup_system_fan(replicape):
en = config.find('FDM','SYSTEM_FAN', 0)
if int(en) == 0:
return
fan_sig = hal.newsig('fan-output', hal.HAL_FLOAT)
replicape.get_fan_pwm_pin(3).link(fan_sig)
fan_sig.set(1.0)
def setup_system_fan(replicape):
en = config.find('FDM','SYSTEM_FAN', 0)
if int(en) == 0:
return
fan_sig = hal.newsig('fan-output', hal.HAL_FLOAT)
replicape.get_fan_pwm_pin(3).link(fan_sig)
fan_sig.set(1.0)
def setup_estop(error_sigs, watchdog_sigs, estop_reset, thread):
# Create estop signal chain
estop_user = hal.Signal('estop-user', hal.HAL_BIT)
estop_user.link('iocontrol.0.user-enable-out')
estop_reset.link('iocontrol.0.user-request-enable')
estop_out = hal.Signal('estop-clear', hal.HAL_BIT)
estop_out.link('iocontrol.0.emc-enable-in')
estop_latch = rtapi.newinst('estop_latch', 'estop.estop-latch')
hal.addf(estop_latch.name, thread)
estop_latch.pin('ok-in').link(estop_user)
estop_latch.pin('reset').link(estop_reset)
estop_latch.pin('ok-out').link(estop_out)
watchdog_sigs = [] # TODO: Fix watchdog code
if len(watchdog_sigs) > 0:
watchdog_ok_sig = hal.newsig('estop.watchdog-ok', hal.HAL_BIT)
watchdog_error_sig = hal.newsig('estop.watchdog-error', hal.HAL_BIT)
watchdog = rtapi.newinst('watchdog',
'estop.watchdog',
pincount=len(watchdog_sigs))
hal.addf(watchdog.name, thread)
for n, sig in watchdog_sigs:
watchdog.pin('input-%02d' % n).link(sig)
watchdog.pin('enable').set(True)
watchdog.pin('ok-out').link(watchdog_ok_sig)
watchdog_not = rtapi.newinst('not', 'estop.watchdog.not')
hal.addf(watchdog_not.name, thread)
watchdog_not.pin('in').link(watchdog_ok_sig)
watchdog_not.pin('out').link(watchdog_error_sig)
error_sigs.append(watchdog_error_sig)
num = len(error_sigs)
if num > 0:
estop_fault = hal.Signal('estop-fault', hal.HAL_BIT)
orn = rtapi.newinst('orn', 'estop.or%i.error' % num, pincount=num)
hal.addf(orn.name, thread)
for n, sig in enumerate(error_sigs):
orn.pin('in%i' % n).link(sig)
orn.pin('out').link(estop_fault)
estop_latch.pin('fault-in').link(estop_fault)
def usrcomp_status(compname, signame, thread, resetSignal='estop-reset'):
sigIn = hal.newsig('%s-error-in' % signame, hal.HAL_BIT)
sigOut = hal.newsig('%s-error' % signame, hal.HAL_BIT)
sigOk = hal.newsig('%s-ok' % signame, hal.HAL_BIT)
sigIn.link('%s.error' % compname)
safetyLatch = rt.newinst('safety_latch', 'safety-latch.%s-error' % signame)
hal.addf(safetyLatch.name, thread)
safetyLatch.pin('error-in').link(sigIn)
safetyLatch.pin('error-out').link(sigOut)
safetyLatch.pin('reset').link(resetSignal)
safetyLatch.pin('threshold').set(500) # 500ms error
safetyLatch.pin('latching').set(True)
notComp = rt.newinst('not', 'not.%s-no-error' % signame)
hal.addf(notComp.name, thread)
notComp.pin('in').link(sigOut)
notComp.pin('out').link(sigOk)
def setup_servo_toggle(servoIndex, section, pwm, selectSignal, thread=None):
servo = '%s.%02i' % ('rc_servo', servoIndex)
mux2 = rt.newinst('mux2', '%s-mux2' % servo)
hal.addf(mux2.name, thread)
pwmout = hal.newsig('%s-pwm-out' % servo, hal.HAL_FLOAT)
mux2.pin('out').link(pwmout)
hal.Pin('%s.value' % pwm).link(pwmout)
enable = hal.Signal('emcmot-0-enable', hal.HAL_BIT)
hal.Pin('%s.enable' % pwm).link(enable)
sigBase = 'rc-servo-%i' % servoIndex
smin = hal.newsig('%s-min' % sigBase, hal.HAL_FLOAT)
smax = hal.newsig('%s-max' % sigBase, hal.HAL_FLOAT)
hal.Pin('%s-mux2.in1' % servo).link(smin)
hal.Pin('%s-mux2.in0' % servo).link(smax)
smin.set(c.find(section, 'SERVO_MIN', 0.1))
smax.set(c.find(section, 'SERVO_MAX', 0.2))
mux2.pin('sel').link('%s' % selectSignal)
def setup_estop(error_sigs, watchdog_sigs, estop_reset, thread):
# Create estop signal chain
estop_user = hal.Signal('estop-user', hal.HAL_BIT)
estop_user.link('iocontrol.0.user-enable-out')
estop_reset.link('iocontrol.0.user-request-enable')
estop_out = hal.Signal('estop-clear', hal.HAL_BIT)
estop_out.link('iocontrol.0.emc-enable-in')
estop_latch = rtapi.newinst('estop_latch', 'estop.estop-latch')
hal.addf(estop_latch.name, thread)
estop_latch.pin('ok-in').link(estop_user)
estop_latch.pin('reset').link(estop_reset)
estop_latch.pin('ok-out').link(estop_out)
watchdog_sigs = [] # TODO: Fix watchdog code
if len(watchdog_sigs) > 0:
watchdog_ok_sig = hal.newsig('estop.watchdog-ok', hal.HAL_BIT)
watchdog_error_sig = hal.newsig('estop.watchdog-error', hal.HAL_BIT)
watchdog = rtapi.newinst('watchdog', 'estop.watchdog', pincount=len(watchdog_sigs))
hal.addf(watchdog.name, thread)
for n, sig in watchdog_sigs:
watchdog.pin('input-%02d' % n).link(sig)
watchdog.pin('enable').set(True)
watchdog.pin('ok-out').link(watchdog_ok_sig)
watchdog_not = rtapi.newinst('not', 'estop.watchdog.not')
hal.addf(watchdog_not.name, thread)
watchdog_not.pin('in').link(watchdog_ok_sig)
watchdog_not.pin('out').link(watchdog_error_sig)
error_sigs.append(watchdog_error_sig)
num = len(error_sigs)
if num > 0:
estop_fault = hal.Signal('estop-fault', hal.HAL_BIT)
orn = rtapi.newinst('orn', 'estop.or%i.error' % num, pincount=num)
hal.addf(orn.name, thread)
for n, sig in enumerate(error_sigs):
orn.pin('in%i' % n).link(sig)
orn.pin('out').link(estop_fault)
estop_latch.pin('fault-in').link(estop_fault)
def usrcomp_status(compname, signame, thread, resetSignal='estop-reset'):
sigIn = hal.newsig('%s-error-in' % signame, hal.HAL_BIT)
sigOut = hal.newsig('%s-error' % signame, hal.HAL_BIT)
sigOk = hal.newsig('%s-ok' % signame, hal.HAL_BIT)
sigIn.link('%s.error' % compname)
safetyLatch = rt.newinst('safety_latch', 'safety-latch.%s-error' % signame)
hal.addf(safetyLatch.name, thread)
safetyLatch.pin('error-in').link(sigIn)
safetyLatch.pin('error-out').link(sigOut)
safetyLatch.pin('reset').link(resetSignal)
safetyLatch.pin('threshold').set(500) # 500ms error
safetyLatch.pin('latching').set(True)
notComp = rt.newinst('not', 'not.%s-no-error' % signame)
hal.addf(notComp.name, thread)
notComp.pin('in').link(sigOut)
notComp.pin('out').link(sigOk)
def setup_hbp_led(thread):
tempMeas = hal.Signal('hbp-temp-meas')
ledHbpHot = hal.newsig('led-hbp-hot', hal.HAL_BIT)
ledHbpInfo = hal.newsig('led-hbp-info', hal.HAL_BIT)
# low temp
comp = rt.newinst('comp', 'comp.hbp-info')
hal.addf(comp.name, thread)
comp.pin('in0').link(tempMeas)
comp.pin('in1').set(50.0)
comp.pin('hyst').set(2.0)
comp.pin('out').link(ledHbpInfo)
# high temp
comp = rt.newinst('comp', 'comp.hbp-hot')
hal.addf(comp.name, thread)
comp.pin('in0').set(50.0)
comp.pin('in1').link(tempMeas)
comp.pin('hyst').set(2.0)
comp.pin('out').link(ledHbpHot)
def setup_stepper(stepgenIndex,
section,
axisIndex=None,
stepgenType='hpg.stepgen',
gantry=False,
gantryJoint=0,
velocitySignal=None,
thread=None):
stepgen = '%s.%01i' % (stepgenType, stepgenIndex)
if axisIndex is not None:
axis = 'axis.%i' % axisIndex
hasMotionAxis = (axisIndex is not None) and (not gantry
or gantryJoint == 0)
velocityControlled = velocitySignal is not None
# axis enable chain
enableIndex = axisIndex
if axisIndex is None:
enableIndex = 0 # use motor enable signal
enable = hal.Signal('emcmot-%i-enable' % enableIndex, hal.HAL_BIT)
if hasMotionAxis:
enable.link('%s.amp-enable-out' % axis)
enable.link('%s.enable' % stepgen)
# expose timing parameters so we can multiplex them later
sigBase = 'stepgen.%i' % stepgenIndex
assign_param(sigBase, "position-scale", c.find(section, 'SCALE'))
assign_param(sigBase, "maxaccel", c.find(section, 'STEPGEN_MAXACC'))
#assign_param(sigBase,"maxvel",c.find("ABP", 'STEPGEN_MAXVEL'))
# position command and feedback
limitHome = hal.newsig('limit-%i-home' % axisIndex, hal.HAL_BIT)
limitMin = hal.newsig('limit-%i-min' % axisIndex, hal.HAL_BIT)
limitMax = hal.newsig('limit-%i-max' % axisIndex, hal.HAL_BIT)
limitHome.link('%s.home-sw-in' % axis)
limitMin.link('%s.neg-lim-sw-in' % axis)
limitMax.link('%s.pos-lim-sw-in' % axis)
if velocityControlled:
hal.net(velocitySignal, '%s.velocity-cmd' % stepgen)
def setup_fans(replicape):
if NUM_FANS == 0:
return
en = config.find('FDM','SYSTEM_FAN', 0)
fan_out = [None] * NUM_FANS
fan_scale = [None] * NUM_FANS
fan_in = [None] * NUM_FANS
#on-board pwm input is 0.0 to 1.0, M106 sends 0 to 255
for i in xrange(NUM_FANS):
fan_out[i] = hal.newsig('fan-out-%d' % i, hal.HAL_FLOAT)
replicape.get_fan_pwm_pin(i).link(fan_out[i])
# the system fan is connected to the last fan pwm output
if (int(en) > 0) and (i == NUM_FANS-1):
fan_out[i].set(1.0)
else:
fan_in[i] = hal.newsig('fan-in-%d' % i, hal.HAL_FLOAT)
fan_in[i].link('motion.analog-out-io-%d' % (FAN_IO_START + i))
fan_scale[i] = rtapi.newinst('div2', 'fan%d.div2.scale-pwm' % i)
hal.addf(fan_scale[i].name, SERVO_THREAD)
fan_scale[i].pin('in0').link(fan_in[i])
fan_scale[i].pin('in1').set(255.0)
fan_scale[i].pin('out').link(fan_out[i])
def setupPosPid(name='pos', thread='base_thread'):
sigPgain = hal.newsig('%s-pgain' % name, hal.HAL_FLOAT)
sigIgain = hal.newsig('%s-igain' % name, hal.HAL_FLOAT)
sigDgain = hal.newsig('%s-dgain' % name, hal.HAL_FLOAT)
sigVel = hal.newsig('%s-vel' % name, hal.HAL_FLOAT)
sigFeedback = hal.newsig('%s-feedback' % name, hal.HAL_FLOAT)
sigOutput = hal.newsig('%s-output' % name, hal.HAL_FLOAT)
sigCmd = hal.newsig('%s-cmd' % name, hal.HAL_FLOAT)
sigEnable = hal.newsig('%s-enable' % name, hal.HAL_BIT)
pid = rt.newinst('at_pid', 'pid.%s' % name)
hal.addf('%s.do-pid-calcs' % pid.name, thread)
pid.pin('maxoutput').set(1.0) # set maxout to prevent windup effect
pid.pin('Pgain').link(sigPgain)
pid.pin('Igain').link(sigIgain)
pid.pin('Dgain').link(sigDgain)
pid.pin('feedback-deriv').link(sigVel)
pid.pin('feedback').link(sigFeedback)
pid.pin('output').link(sigOutput)
pid.pin('command').link(sigCmd)
pid.pin('enable').link(sigEnable)
kalman = hal.components['kalman']
kalman.pin('rate').link(sigVel)
kalman.pin('angle').link(sigFeedback)
# use a sum component to forward the output to the vel PIDs
sum2 = rt.newinst('sum2', 'sum2.mr')
hal.addf(sum2.name, thread)
sum2.pin('in0').link(sigOutput)
sum2.pin('in1').set(0)
sum2.pin('out').link('mr-cmd-vel')
sum2 = rt.newinst('sum2', 'sum2.ml')
hal.addf(sum2.name, thread)
sum2.pin('in0').link(sigOutput)
sum2.pin('in1').set(0)
sum2.pin('out').link('ml-cmd-vel')
# TODO use cmd
sigCmd.set(0.0)
# storage
hal.Pin('storage.%s.pgain' % name).link(sigPgain)
hal.Pin('storage.%s.igain' % name).link(sigIgain)
hal.Pin('storage.%s.dgain' % name).link(sigDgain)
sigEnable.set(True)
def setupPosPid(name='pos', thread='base_thread'):
sigPgain = hal.newsig('%s-pgain' % name, hal.HAL_FLOAT)
sigIgain = hal.newsig('%s-igain' % name, hal.HAL_FLOAT)
sigDgain = hal.newsig('%s-dgain' % name, hal.HAL_FLOAT)
sigVel = hal.newsig('%s-vel' % name, hal.HAL_FLOAT)
sigFeedback = hal.newsig('%s-feedback' % name, hal.HAL_FLOAT)
sigOutput = hal.newsig('%s-output' % name, hal.HAL_FLOAT)
sigCmd = hal.newsig('%s-cmd' % name, hal.HAL_FLOAT)
sigEnable = hal.newsig('%s-enable' % name, hal.HAL_BIT)
pid = rt.newinst('at_pid', 'pid.%s' % name)
hal.addf('%s.do-pid-calcs' % pid.name, thread)
pid.pin('maxoutput').set(1.0) # set maxout to prevent windup effect
pid.pin('Pgain').link(sigPgain)
pid.pin('Igain').link(sigIgain)
pid.pin('Dgain').link(sigDgain)
pid.pin('feedback-deriv').link(sigVel)
pid.pin('feedback').link(sigFeedback)
pid.pin('output').link(sigOutput)
pid.pin('command').link(sigCmd)
pid.pin('enable').link(sigEnable)
kalman = hal.components['kalman']
kalman.pin('rate').link(sigVel)
kalman.pin('angle').link(sigFeedback)
# use a sum component to forward the output to the vel PIDs
sum2 = rt.newinst('sum2', 'sum2.mr')
hal.addf(sum2.name, thread)
sum2.pin('in0').link(sigOutput)
sum2.pin('in1').set(0)
sum2.pin('out').link('mr-cmd-vel')
sum2 = rt.newinst('sum2', 'sum2.ml')
hal.addf(sum2.name, thread)
sum2.pin('in0').link(sigOutput)
sum2.pin('in1').set(0)
sum2.pin('out').link('ml-cmd-vel')
# TODO use cmd
sigCmd.set(0.0)
# storage
hal.Pin('storage.%s.pgain' % name).link(sigPgain)
hal.Pin('storage.%s.igain' % name).link(sigIgain)
hal.Pin('storage.%s.dgain' % name).link(sigDgain)
sigEnable.set(True)
def setup_enable_chain():
"""
Create enable and enable_inv signal for the system
"""
main_enable = hal.net('main-enable', 'axis.0.amp-enable-out')
n = rtapi.newinst('not', 'main-enable.not')
hal.addf(n.name, SERVO_THREAD)
main_enable_inv = hal.newsig('main-enable-inv', hal.HAL_BIT)
n.pin('in').link('main-enable')
n.pin('out').link('main-enable-inv')
return (main_enable, main_enable_inv)
def setup_enable_chain():
"""
Create enable and enable_inv signal for the system
"""
main_enable = hal.net('main-enable', 'axis.0.amp-enable-out')
n = rtapi.newinst('not', 'main-enable.not')
hal.addf(n.name, SERVO_THREAD)
main_enable_inv = hal.newsig('main-enable-inv', hal.HAL_BIT)
n.pin('in').link('main-enable')
n.pin('out').link('main-enable-inv')
return (main_enable, main_enable_inv)
def setup_limit_switches(replicape):
limit_x_sig = hal.newsig('limit-x', hal.HAL_BIT)
limit_x_sig.link('axis.0.home-sw-in')
limit_x_pos = config.find('AXIS_0', 'HOME_SEARCH_VEL', 0)
if limit_x_pos < 0:
replicape.get_limit_pin('X', False).link(limit_x_sig)
limit_x_sig.link('axis.0.neg-lim-sw-in')
if limit_x_pos > 0:
replicape.get_limit_pin('X', True).link(limit_x_sig)
limit_x_sig.link('axis.0.pos-lim-sw-in')
limit_y_sig = hal.newsig('limit-y', hal.HAL_BIT)
limit_y_sig.link('axis.1.home-sw-in')
limit_y_pos = config.find('AXIS_1', 'HOME_SEARCH_VEL', 0)
if limit_y_pos < 0:
replicape.get_limit_pin('Y', False).link(limit_y_sig)
limit_y_sig.link('axis.1.neg-lim-sw-in')
if limit_y_pos > 0:
replicape.get_limit_pin('Y', True).link(limit_y_sig)
limit_y_sig.link('axis.1.pos-lim-sw-in')
limit_z_sig = hal.newsig('limit-z', hal.HAL_BIT)
limit_z_sig.link('axis.2.home-sw-in')
limit_z_pos = config.find('AXIS_2', 'HOME_SEARCH_VEL', 0)
if limit_z_pos < 0:
replicape.get_limit_pin('Z', False).link(limit_z_sig)
limit_z_sig.link('axis.2.neg-lim-sw-in')
if limit_z_pos > 0:
replicape.get_limit_pin('Z', True).link(limit_z_sig)
limit_z_sig.link('axis.2.pos-lim-sw-in')
probe_pin = replicape.get_probe_pin();
if probe_pin is not None:
probe_sig = hal.newsig('limit-probe', hal.HAL_BIT)
probe_sig.link(probe_pin)
probe_sig.link('motion.probe-input')
def usrcomp_watchdog(comps, enableSignal, thread,
okSignal=None, errorSignal=None):
count = len(comps)
watchdog = rt.loadrt('watchdog', num_inputs=count)
hal.addf('watchdog.set-timeouts', thread)
hal.addf('watchdog.process', thread)
for n, comp in enumerate(comps):
compname = comp[0]
comptime = comp[1]
sigIn = hal.newsig('%s-watchdog' % compname, hal.HAL_BIT)
hal.Pin('%s.watchdog' % compname).link(sigIn)
watchdog.pin('input-%i' % n).link(sigIn)
watchdog.pin('timeout-%i' % n).set(comptime)
watchdog.pin('enable-in').link(enableSignal)
if not okSignal:
okSignal = hal.newsig('watchdog-ok', hal.HAL_BIT)
watchdog.pin('ok-out').link(okSignal)
if errorSignal:
notComp = rt.newinst('not', 'not.watchdog-error')
hal.addf(notComp.name, thread)
notComp.pin('in').link(okSignal)
notComp.pin('out').link(errorSignal)
def setup_limit_switches(replicape):
limit_x_sig = hal.newsig('limit-x', hal.HAL_BIT)
limit_x_sig.link('axis.0.home-sw-in')
limit_x_pos = config.find('AXIS_0', 'HOME_SEARCH_VEL', 0)
if limit_x_pos < 0:
replicape.get_limit_pin('X', False).link(limit_x_sig)
limit_x_sig.link('axis.0.neg-lim-sw-in')
if limit_x_pos > 0:
replicape.get_limit_pin('X', True).link(limit_x_sig)
limit_x_sig.link('axis.0.pos-lim-sw-in')
limit_y_sig = hal.newsig('limit-y', hal.HAL_BIT)
limit_y_sig.link('axis.1.home-sw-in')
limit_y_pos = config.find('AXIS_1', 'HOME_SEARCH_VEL', 0)
if limit_y_pos < 0:
replicape.get_limit_pin('Y', False).link(limit_y_sig)
limit_y_sig.link('axis.1.neg-lim-sw-in')
if limit_y_pos > 0:
replicape.get_limit_pin('Y', True).link(limit_y_sig)
limit_y_sig.link('axis.1.pos-lim-sw-in')
limit_z_sig = hal.newsig('limit-z', hal.HAL_BIT)
limit_z_sig.link('axis.2.home-sw-in')
limit_z_pos = config.find('AXIS_2', 'HOME_SEARCH_VEL', 0)
if limit_z_pos < 0:
replicape.get_limit_pin('Z', False).link(limit_z_sig)
limit_z_sig.link('axis.2.neg-lim-sw-in')
if limit_z_pos > 0:
replicape.get_limit_pin('Z', True).link(limit_z_sig)
limit_z_sig.link('axis.2.pos-lim-sw-in')
probe_pin = replicape.get_probe_pin();
if probe_pin is not None:
probe_sig = hal.newsig('limit-probe', hal.HAL_BIT)
probe_sig.link(probe_pin)
probe_sig.link('motion.probe-input')
def create_joint_plumbing(st, joint_nr):
# offset
rt.newinst('offsetv2', 'joint%i_offset' % joint_nr)
hal.addf('joint%i_offset.update-output' % joint_nr, "%s" % st)
hal.addf('joint%i_offset.update-feedback' % joint_nr, st)
# scaling
rt.newinst('mult2v2', 'joint%i_scale_pos_cmd' % joint_nr)
hal.addf('joint%i_scale_pos_cmd.funct' % joint_nr, st)
rt.newinst('div2v2', 'joint%i_scale_pos_fb' % joint_nr)
hal.addf('joint%i_scale_pos_fb.funct' % joint_nr, st)
jointscale = hal.newsig('joint%i_scale' % joint_nr, hal.HAL_FLOAT)
hal.Pin('joint%i_scale_pos_fb.deadband' % joint_nr).set(0.00000001)
# converting components
rt.newinst('conv_float_s32', 'joint%i_cmd_s32' % joint_nr)
rt.newinst('conv_s32_float', 'joint%i_fb_flt' % joint_nr)
hal.addf('joint%i_cmd_s32.funct' % joint_nr, st)
hal.addf('joint%i_fb_flt.funct' % joint_nr, st)
# now connect the components, scale to mult and div
jointscale.link('joint%i_scale_pos_cmd.in1' % joint_nr)
jointscale.link('joint%i_scale_pos_fb.in1' % joint_nr)
# offset joint (ROS) position to scale mult
hal.Pin('joint%i_offset.out' % joint_nr).link('joint%i_scale_pos_cmd.in0' %
joint_nr)
# (de)offset scale div to joint (ROS) position
hal.Pin('joint%i_scale_pos_fb.out' % joint_nr).link(
'joint%i_offset.fb-in' % joint_nr)
# connect converting components
hal.Pin('joint%i_scale_pos_cmd.out' % joint_nr).link('joint%i_cmd_s32.in' %
joint_nr)
hal.Pin('joint%i_fb_flt.out' % joint_nr).link('joint%i_scale_pos_fb.in0' %
joint_nr)
# create jplan component but do nothing for now
# because
# - the current position must first be retrieved from the drive
# - then the joint*_jplan.0.pos-cmd must be set
# - acc and vel must be high
# - wait on joint*_jplan.0.active falling edge (target reached)
# - then set correct values for acc and vel values
# - then connect with a signal
rt.newinst('jplan', 'joint%i_jplan' % joint_nr)
hal.addf('joint%i_jplan.update' % joint_nr, st)
def demo_subscriber(compname, prefix, topic, count=6):
'''
retrieve msg type of <topic>
subscribe to <topic>
create pins <compname>.<prefix>.fieldname of appropriate type
update pin values accordingly
'''
global pins
msgtype = get_topic_type(topic)
pins = gen_halcomp(compname, prefix, msgtype, hal.HAL_OUT, count)
# create signal from joint_states.robot.position.0 to hm2_5i20.0.stepgen.00.position-cmd
# net joint0 joint_states.robot.position.0 hm2_5i20.0.stepgen.00.position-cmd
for i in range(0, 2):
sig_joint = hal.newsig('sig-joint%s' % i, hal.HAL_FLOAT)
sig_joint.link('joint_states.robot.position.%s' % i)
sig_joint.link('hm2_5i20.0.stepgen.0%s.position-cmd' % i)
rospy.init_node(compname, anonymous=True)
rospy.Subscriber(topic, msgtype, callback=demo_callback, callback_args=pins)
rospy.spin()
def __init__(self):
self.pru = rtapi.loadrt('hal_pru_generic',
pru=0, num_stepgens=5, num_pwmgens=0, halname='hpg',
prucode='%s/rt-preempt/pru_generic.bin' % (config.Config().EMC2_RTLIB_DIR))
# 4.14.18-ti-rt-r33 kernel and rt-preempt
hal.addf('hpg.capture-position', SERVO_THREAD)
hal.addf('hpg.update', SERVO_THREAD)
hal.addf('bb_gpio.read', SERVO_THREAD)
hal.addf('bb_gpio.write', SERVO_THREAD)
minvel = config.find('TRAJ','MIN_VELOCITY', 0.001)
for i in xrange(5):
self.get_pru_pin('stepgen.%02i.dirsetup' % i).set(200)
self.get_pru_pin('stepgen.%02i.dirhold' % i).set(200)
self.get_pru_pin('stepgen.%02i.steplen' % i).set(1000)
self.get_pru_pin('stepgen.%02i.stepspace' % i).set(1000)
self.get_pru_pin('stepgen.%02i.dirpin' % i).set(self.pru_dir_pin(i))
self.get_pru_pin('stepgen.%02i.steppin' % i).set(self.pru_step_pin(i))
# setting to zero: pru_generic adapts to maximum velocity and acceleration
# see http://www.machinekit.io/docs/man/man9/hal_pru_generic/
self.get_pru_pin('stepgen.%02i.maxvel' % i).set(0)
self.get_pru_pin('stepgen.%02i.maxaccel' % i).set(0)
# use new pru stepgen minvel pin to avoid pru hunting problem (without PID loop)
# see this discussion https://groups.google.com/forum/#!topic/machinekit/ATEwvfgoIb4
# except for extruder(s)
if i < 3 :
self.get_pru_pin('stepgen.%02i.minvel' % i).set(minvel)
self.pwm = hal.loadusr(USR_HAL_PATH + 'hal_replicape_pwm',
name='replicape_pwm',
wait_name='replicape_pwm')
self.watchdog_sigs = []
for pin in self.get_watchdog_pins():
s = hal.newsig('replicape.watchdog.%d' % len(self.watchdog_sigs), hal.HAL_BIT)
pin.link(s)
self.watchdog_sigs.append(s)
def __init__(self):
self.pru = rtapi.loadrt('hal_pru_generic',
pru=0,
num_stepgens=5,
num_pwmgens=0,
halname='hpg',
prucode='%s/xenomai/pru_generic.bin' %
(config.Config().EMC2_RTLIB_DIR))
hal.addf('hpg.capture-position', SERVO_THREAD)
hal.addf('hpg.update', SERVO_THREAD)
hal.addf('bb_gpio.read', SERVO_THREAD)
hal.addf('bb_gpio.write', SERVO_THREAD)
for i in xrange(5):
self.get_pru_pin('stepgen.%02i.dirsetup' % i).set(200)
self.get_pru_pin('stepgen.%02i.dirhold' % i).set(200)
self.get_pru_pin('stepgen.%02i.steplen' % i).set(1000)
self.get_pru_pin('stepgen.%02i.stepspace' % i).set(1000)
self.get_pru_pin('stepgen.%02i.dirpin' % i).set(
self.pru_dir_pin(i))
self.get_pru_pin('stepgen.%02i.steppin' % i).set(
self.pru_step_pin(i))
self.get_pru_pin('stepgen.%02i.maxvel' % i).set(0)
self.get_pru_pin('stepgen.%02i.maxaccel' % i).set(0)
self.pwm = hal.loadusr(USR_HAL_PATH + 'hal_replicape_pwm',
name='replicape_pwm',
wait_name='replicape_pwm')
self.watchdog_sigs = []
for pin in self.get_watchdog_pins():
s = hal.newsig('replicape.watchdog.%d' % len(self.watchdog_sigs),
hal.HAL_BIT)
pin.link(s)
self.watchdog_sigs.append(s)
def _init_signals(self):
# Input signal
self.enable_sig = hal.newsig('ve%d.enable' % self.nr, hal.HAL_BIT)
self.cross_section_sig = hal.newsig('ve%d.cross-section' % self.nr, hal.HAL_FLOAT)
self.jog_vel_sig = hal.newsig('ve%d.jog-vel' % self.nr, hal.HAL_FLOAT)
self.jog_direction_sig = hal.newsig('ve%d.jog-direction' % self.nr, hal.HAL_BIT)
self.jog_distance_sig = hal.newsig('ve%d.jog-distance' % self.nr, hal.HAL_FLOAT)
self.jog_trigger_sig = hal.newsig('ve%d.jog-trigger' % self.nr, hal.HAL_BIT)
self.jog_continuous_sig = hal.newsig('ve%d.jog-continuous' % self.nr, hal.HAL_BIT)
self.jog_dtg_sig = hal.newsig('ve%d.jog-dtg' % self.nr, hal.HAL_FLOAT)
# Intermediate signal
self.jog_vel_output_sig = hal.newsig('ve%d.jog-vel-output' % self.nr, hal.HAL_FLOAT)
# Parameter signal
self.filament_dia_sig = hal.newsig('ve%d.filament-dia' % self.nr, hal.HAL_FLOAT)
self.extrude_scale_sig = hal.newsig('ve%d.extrude-scale' % self.nr, hal.HAL_FLOAT)
self.accel_gain_sig = hal.newsig('ve%d.accel-gain' % self.nr, hal.HAL_FLOAT)
self.max_jog_vel_sig = hal.newsig('ve%d.max-jog-vel' % self.nr, hal.HAL_FLOAT)
self.retract_vel_sig = hal.newsig('ve%d.retract-vel' % self.nr, hal.HAL_FLOAT)
self.retract_len_sig = hal.newsig('ve%d.retract-len' % self.nr, hal.HAL_FLOAT)
# Output signal
self.extrude_vel_sig = hal.newsig('ve%d.extrude-vel' % self.nr, hal.HAL_FLOAT)
self.retracting_sig = hal.newsig('ve%d.retracting' % self.nr, hal.HAL_BIT)
def setup_stepper(stepgenIndex, section, axisIndex=None,
stepgenType='hpg.stepgen', gantry=False,
gantryJoint=0, velocitySignal=None, thread=None):
stepgen = '%s.%02i' % (stepgenType, stepgenIndex)
if axisIndex is not None:
axis = 'axis.%i' % axisIndex
hasMotionAxis = (axisIndex is not None) and (not gantry or gantryJoint == 0)
velocityControlled = velocitySignal is not None
# axis enable chain
enableIndex = axisIndex
if axisIndex is None:
enableIndex = 0 # use motor enable signal
enable = hal.Signal('emcmot-%i-enable' % enableIndex, hal.HAL_BIT)
if hasMotionAxis:
enable.link('%s.amp-enable-out' % axis)
enable.link('%s.enable' % stepgen)
# expose timing parameters so we can multiplex them later
sigBase = 'stepgen-%i' % stepgenIndex
dirsetup = hal.newsig('%s-dirsetup' % sigBase, hal.HAL_U32)
dirhold = hal.newsig('%s-dirhold' % sigBase, hal.HAL_U32)
steplen = hal.newsig('%s-steplen' % sigBase, hal.HAL_U32)
stepspace = hal.newsig('%s-stepspace' % sigBase, hal.HAL_U32)
scale = hal.newsig('%s-scale' % sigBase, hal.HAL_FLOAT)
maxVel = hal.newsig('%s-max-vel' % sigBase, hal.HAL_FLOAT)
maxAcc = hal.newsig('%s-max-acc' % sigBase, hal.HAL_FLOAT)
controlType = hal.newsig('%s-control-type' % sigBase, hal.HAL_BIT)
hal.Pin('%s.dirsetup' % stepgen).link(dirsetup)
hal.Pin('%s.dirhold' % stepgen).link(dirhold)
dirsetup.set(c.find(section, 'DIRSETUP'))
dirhold.set(c.find(section, 'DIRHOLD'))
hal.Pin('%s.steplen' % stepgen).link(steplen)
hal.Pin('%s.stepspace' % stepgen).link(stepspace)
steplen.set(c.find(section, 'STEPLEN'))
stepspace.set(c.find(section, 'STEPSPACE'))
hal.Pin('%s.position-scale' % stepgen).link(scale)
scale.set(c.find(section, 'SCALE'))
hal.Pin('%s.maxvel' % stepgen).link(maxVel)
hal.Pin('%s.maxaccel' % stepgen).link(maxAcc)
maxVel.set(c.find(section, 'STEPGEN_MAX_VEL'))
maxAcc.set(c.find(section, 'STEPGEN_MAX_ACC'))
hal.Pin('%s.control-type' % stepgen).link(controlType)
# position command and feedback
if not velocityControlled:
if hasMotionAxis: # per axis fb and cmd
posCmd = hal.newsig('emcmot-%i-pos-cmd' % axisIndex, hal.HAL_FLOAT)
posCmd.link('%s.motor-pos-cmd' % axis)
if not gantry:
posCmd.link('%s.position-cmd' % stepgen)
else:
posCmd.link('gantry.%i.position-cmd' % axisIndex)
posFb = hal.newsig('emcmot-%i-pos-fb' % axisIndex, hal.HAL_FLOAT)
posFb.link('%s.motor-pos-fb' % axis)
if not gantry:
posFb.link('%s.position-fb' % stepgen)
else:
posFb.link('gantry.%i.position-fb' % axisIndex)
if gantry: # per joint fb and cmd
posCmd = hal.newsig('emcmot-%i-%i-pos-cmd' % (axisIndex, gantryJoint), hal.HAL_FLOAT)
posCmd.link('gantry.%i.joint.%02i.pos-cmd' % (axisIndex, gantryJoint))
posCmd.link('%s.position-cmd' % stepgen)
posFb = hal.newsig('emcmot-%i-%i-pos-fb' % (axisIndex, gantryJoint), hal.HAL_FLOAT)
posFb.link('%s.position-fb' % stepgen)
posFb.link('gantry.%i.joint.%02i.pos-fb' % (axisIndex, gantryJoint))
else: # velocity control
hal.net(velocitySignal, '%s.velocity-cmd' % stepgen)
controlType.set(1) # enable velocity control
# limits
if hasMotionAxis:
limitHome = hal.newsig('limit-%i-home' % axisIndex, hal.HAL_BIT)
limitMin = hal.newsig('limit-%i-min' % axisIndex, hal.HAL_BIT)
limitMax = hal.newsig('limit-%i-max' % axisIndex, hal.HAL_BIT)
limitHome.link('%s.home-sw-in' % axis)
limitMin.link('%s.neg-lim-sw-in' % axis)
limitMax.link('%s.pos-lim-sw-in' % axis)
if gantry:
if gantryJoint == 0:
axisHoming = hal.newsig('emcmot-%i-homing' % axisIndex, hal.HAL_BIT)
axisHoming.link('%s.homing' % axis)
hal.Pin('gantry.%i.search-vel' % axisIndex).set(c.find(section, 'HOME_SEARCH_VEL'))
hal.Pin('gantry.%i.homing' % axisIndex).link(axisHoming)
hal.Pin('gantry.%i.home' % axisIndex).link(limitHome)
or2 = rt.newinst('or2', 'or2.limit-%i-min' % axisIndex)
hal.addf(or2.name, thread)
or2.pin('out').link(limitMin)
or2 = rt.newinst('or2', 'or2.limit-%i-max' % axisIndex)
hal.addf(or2.name, thread)
or2.pin('out').link(limitMax)
limitHome = hal.newsig('limit-%i-%i-home' % (axisIndex, gantryJoint),
hal.HAL_BIT)
limitMin = hal.newsig('limit-%i-%i-min' % (axisIndex, gantryJoint),
hal.HAL_BIT)
limitMax = hal.newsig('limit-%i-%i-max' % (axisIndex, gantryJoint),
hal.HAL_BIT)
homeOffset = hal.Signal('home-offset-%i-%i' % (axisIndex, gantryJoint),
hal.HAL_FLOAT)
limitHome.link('gantry.%i.joint.%02i.home' % (axisIndex, gantryJoint))
limitMin.link('or2.limit-%i-min.in%i' % (axisIndex, gantryJoint))
limitMax.link('or2.limit-%i-max.in%i' % (axisIndex, gantryJoint))
homeOffset.link('gantry.%i.joint.%02i.home-offset' % (axisIndex, gantryJoint))
storage.setup_gantry_storage(axisIndex, gantryJoint)
def _init_extrude(self, motion_vel_sig):
material_flowrate_sig = hal.newsig('ve%d.material-flowrate' % self.nr, hal.HAL_FLOAT)
material_flowrate = rtapi.newinst('mult2', 've%d.mult2.material-flowrate' % self.nr)
hal.addf(material_flowrate.name, SERVO_THREAD)
material_flowrate.pin('in0').link(motion_vel_sig)
material_flowrate.pin('in1').link(self.cross_section_sig)
material_flowrate.pin('out').link(material_flowrate_sig)
filament_dia_sq_sig = hal.newsig('ve%d.filament-dia-sq' % self.nr, hal.HAL_FLOAT)
filament_dia_sq = rtapi.newinst('mult2', 've%d.mult2.filament-dia-sq' % self.nr)
hal.addf(filament_dia_sq.name, SERVO_THREAD)
filament_dia_sq.pin('in0').link(self.filament_dia_sig)
filament_dia_sq.pin('in1').link(self.filament_dia_sig)
filament_dia_sq.pin('out').link(filament_dia_sq_sig)
filament_area_sig = hal.newsig('ve%d.filament-area' % self.nr, hal.HAL_FLOAT)
filament_area = rtapi.newinst('mult2', 've%d.mult2.filament-area' % self.nr)
hal.addf(filament_area.name, SERVO_THREAD)
filament_area.pin('in0').link(filament_dia_sq_sig)
filament_area.pin('in1').set(math.pi / 4)
filament_area.pin('out').link(filament_area_sig)
extrude_rate_sig = hal.newsig('ve%d.extrude-rate' % self.nr, hal.HAL_FLOAT)
extrude_rate = rtapi.newinst('div2', 've%d.div2.extrude-rate' % self.nr)
hal.addf(extrude_rate.name, SERVO_THREAD)
extrude_rate.pin('in0').link(material_flowrate_sig)
extrude_rate.pin('in1').link(filament_area_sig)
extrude_rate.pin('out').link(extrude_rate_sig)
extrude_rate_scaled_sig = hal.newsig('ve%d.extrude-rate-scaled' % self.nr, hal.HAL_FLOAT)
extrude_rate_scaled = rtapi.newinst('mult2', 've%d.mult2.extrude-rate-scaled' % self.nr)
hal.addf(extrude_rate_scaled.name, SERVO_THREAD)
extrude_rate_scaled.pin('in0').link(extrude_rate_sig)
extrude_rate_scaled.pin('in1').link(self.extrude_scale_sig)
extrude_rate_scaled.pin('out').link(extrude_rate_scaled_sig)
extrude_accel_sig = hal.newsig('ve%d.extrude-accel' % self.nr, hal.HAL_FLOAT)
extrude_accel = rtapi.newinst('ddt', 've%d.ddt.extrude-accel' % self.nr)
hal.addf(extrude_accel.name, SERVO_THREAD)
extrude_accel.pin('in').link(extrude_rate_scaled_sig)
extrude_accel.pin('out').link(extrude_accel_sig)
extrude_accel_adj_sig = hal.newsig('ve%d.extrude-accel-adj' % self.nr, hal.HAL_FLOAT)
extrude_accel_adj = rtapi.newinst('mult2', 've%d.mult2.extrude-accel-adj' % self.nr)
hal.addf(extrude_accel_adj.name, SERVO_THREAD)
extrude_accel_adj.pin('in0').link(extrude_accel_sig)
extrude_accel_adj.pin('in1').link(self.accel_gain_sig)
extrude_accel_adj.pin('out').link(extrude_accel_adj_sig)
extrude_rate_comp_sig = hal.newsig('ve%d.extrude-rate-comp' % self.nr, hal.HAL_FLOAT)
extrude_rate_comp = rtapi.newinst('sum2', 've%d.sum2.extrude-rate-comp' % self.nr)
hal.addf(extrude_rate_comp.name, SERVO_THREAD)
extrude_rate_comp.pin('in0').link(extrude_rate_scaled_sig)
extrude_rate_comp.pin('in1').link(extrude_accel_adj_sig)
extrude_rate_comp.pin('out').link(extrude_rate_comp_sig)
retract_vel_neg_sig = hal.newsig('ve%d.retract-vel-neg' % self.nr, hal.HAL_FLOAT)
retract_time_sig = hal.newsig('ve%d.retract-time' % self.nr, hal.HAL_FLOAT)
retract_vel_neg = rtapi.newinst('neg', 've%d.neg.retract-vel-neg' % self.nr)
hal.addf(retract_vel_neg.name, SERVO_THREAD)
retract_vel_neg.pin('in').link(self.retract_vel_sig)
retract_vel_neg.pin('out').link(retract_vel_neg_sig)
retract_time = rtapi.newinst('div2', 've%d.div2.retract-time' % self.nr)
hal.addf(retract_time.name, SERVO_THREAD)
retract_time.pin('in0').link(self.retract_len_sig)
retract_time.pin('in1').link(self.retract_vel_sig)
retract_time.pin('out').link(retract_time_sig)
retracting_delay = rtapi.newinst('oneshot', 've%d.oneshot.retracting' % self.nr)
hal.addf(retracting_delay.name, SERVO_THREAD)
retracting_delay.pin('rising').set(True)
retracting_delay.pin('falling').set(True)
retracting_delay.pin('retriggerable').set(True)
retracting_delay.pin('width').link(retract_time_sig)
retracting_delay.pin('in').link(self.enable_sig)
retracting_delay.pin('out').link(self.retracting_sig)
extrude_vel = rtapi.newinst('mux4', 've%d.mux4.extrude-vel' % self.nr)
hal.addf(extrude_vel.name, SERVO_THREAD)
extrude_vel.pin('sel0').link(self.retracting_sig)
extrude_vel.pin('sel1').link(self.enable_sig)
extrude_vel.pin('in0').link(self.jog_vel_output_sig)
extrude_vel.pin('in1').link(retract_vel_neg_sig)
extrude_vel.pin('in2').link(extrude_rate_comp_sig)
extrude_vel.pin('in3').link(self.retract_vel_sig)
extrude_vel.pin('out').link(self.extrude_vel_sig)
def setup_stepper(stepgenIndex,
section,
axisIndex=None,
stepgenType='hpg.stepgen',
gantry=False,
gantryJoint=0,
velocitySignal=None,
thread=None):
stepgen = '%s.%02i' % (stepgenType, stepgenIndex)
if axisIndex is not None:
axis = 'axis.%i' % axisIndex
hasMotionAxis = (axisIndex is not None) and (not gantry
or gantryJoint == 0)
velocityControlled = velocitySignal is not None
# axis enable chain
enableIndex = axisIndex
if axisIndex is None:
enableIndex = 0 # use motor enable signal
enable = hal.Signal('emcmot-%i-enable' % enableIndex, hal.HAL_BIT)
if hasMotionAxis:
enable.link('%s.amp-enable-out' % axis)
enable.link('%s.enable' % stepgen)
# expose timing parameters so we can multiplex them later
sigBase = 'stepgen-%i' % stepgenIndex
dirsetup = hal.newsig('%s-dirsetup' % sigBase, hal.HAL_U32)
dirhold = hal.newsig('%s-dirhold' % sigBase, hal.HAL_U32)
steplen = hal.newsig('%s-steplen' % sigBase, hal.HAL_U32)
stepspace = hal.newsig('%s-stepspace' % sigBase, hal.HAL_U32)
scale = hal.newsig('%s-scale' % sigBase, hal.HAL_FLOAT)
maxVel = hal.newsig('%s-max-vel' % sigBase, hal.HAL_FLOAT)
maxAcc = hal.newsig('%s-max-acc' % sigBase, hal.HAL_FLOAT)
controlType = hal.newsig('%s-control-type' % sigBase, hal.HAL_BIT)
hal.Pin('%s.dirsetup' % stepgen).link(dirsetup)
hal.Pin('%s.dirhold' % stepgen).link(dirhold)
dirsetup.set(c.find(section, 'DIRSETUP'))
dirhold.set(c.find(section, 'DIRHOLD'))
hal.Pin('%s.steplen' % stepgen).link(steplen)
hal.Pin('%s.stepspace' % stepgen).link(stepspace)
steplen.set(c.find(section, 'STEPLEN'))
stepspace.set(c.find(section, 'STEPSPACE'))
hal.Pin('%s.position-scale' % stepgen).link(scale)
scale.set(c.find(section, 'SCALE'))
hal.Pin('%s.maxvel' % stepgen).link(maxVel)
hal.Pin('%s.maxaccel' % stepgen).link(maxAcc)
maxVel.set(c.find(section, 'STEPGEN_MAX_VEL'))
maxAcc.set(c.find(section, 'STEPGEN_MAX_ACC'))
hal.Pin('%s.control-type' % stepgen).link(controlType)
# position command and feedback
if not velocityControlled:
if hasMotionAxis: # per axis fb and cmd
posCmd = hal.newsig('emcmot-%i-pos-cmd' % axisIndex, hal.HAL_FLOAT)
posCmd.link('%s.motor-pos-cmd' % axis)
if not gantry:
posCmd.link('%s.position-cmd' % stepgen)
else:
posCmd.link('gantry.%i.position-cmd' % axisIndex)
posFb = hal.newsig('emcmot-%i-pos-fb' % axisIndex, hal.HAL_FLOAT)
posFb.link('%s.motor-pos-fb' % axis)
if not gantry:
posFb.link('%s.position-fb' % stepgen)
else:
posFb.link('gantry.%i.position-fb' % axisIndex)
if gantry: # per joint fb and cmd
posCmd = hal.newsig(
'emcmot-%i-%i-pos-cmd' % (axisIndex, gantryJoint),
hal.HAL_FLOAT)
posCmd.link('gantry.%i.joint.%02i.pos-cmd' %
(axisIndex, gantryJoint))
posCmd.link('%s.position-cmd' % stepgen)
posFb = hal.newsig(
'emcmot-%i-%i-pos-fb' % (axisIndex, gantryJoint),
hal.HAL_FLOAT)
posFb.link('%s.position-fb' % stepgen)
posFb.link('gantry.%i.joint.%02i.pos-fb' %
(axisIndex, gantryJoint))
else: # velocity control
hal.net(velocitySignal, '%s.velocity-cmd' % stepgen)
controlType.set(1) # enable velocity control
# limits
if hasMotionAxis:
limitHome = hal.newsig('limit-%i-home' % axisIndex, hal.HAL_BIT)
limitMin = hal.newsig('limit-%i-min' % axisIndex, hal.HAL_BIT)
limitMax = hal.newsig('limit-%i-max' % axisIndex, hal.HAL_BIT)
limitHome.link('%s.home-sw-in' % axis)
limitMin.link('%s.neg-lim-sw-in' % axis)
limitMax.link('%s.pos-lim-sw-in' % axis)
if gantry:
if gantryJoint == 0:
axisHoming = hal.newsig('emcmot-%i-homing' % axisIndex,
hal.HAL_BIT)
axisHoming.link('%s.homing' % axis)
hal.Pin('gantry.%i.search-vel' % axisIndex).set(
c.find(section, 'HOME_SEARCH_VEL'))
hal.Pin('gantry.%i.homing' % axisIndex).link(axisHoming)
hal.Pin('gantry.%i.home' % axisIndex).link(limitHome)
or2 = rt.newinst('or2', 'or2.limit-%i-min' % axisIndex)
hal.addf(or2.name, thread)
or2.pin('out').link(limitMin)
or2 = rt.newinst('or2', 'or2.limit-%i-max' % axisIndex)
hal.addf(or2.name, thread)
or2.pin('out').link(limitMax)
limitHome = hal.newsig('limit-%i-%i-home' % (axisIndex, gantryJoint),
hal.HAL_BIT)
limitMin = hal.newsig('limit-%i-%i-min' % (axisIndex, gantryJoint),
hal.HAL_BIT)
limitMax = hal.newsig('limit-%i-%i-max' % (axisIndex, gantryJoint),
hal.HAL_BIT)
homeOffset = hal.Signal('home-offset-%i-%i' % (axisIndex, gantryJoint),
hal.HAL_FLOAT)
limitHome.link('gantry.%i.joint.%02i.home' % (axisIndex, gantryJoint))
limitMin.link('or2.limit-%i-min.in%i' % (axisIndex, gantryJoint))
limitMax.link('or2.limit-%i-max.in%i' % (axisIndex, gantryJoint))
homeOffset.link('gantry.%i.joint.%02i.home-offset' %
(axisIndex, gantryJoint))
storage.setup_gantry_storage(axisIndex, gantryJoint)
def create_temperature_control(name, section, thread, hardwareOkSignal=None,
coolingFan=None, hotendFan=None):
tempSet = hal.newsig('%s-temp-set' % name, hal.HAL_FLOAT)
tempMeas = hal.newsig('%s-temp-meas' % name, hal.HAL_FLOAT)
tempInRange = hal.newsig('%s-temp-in-range' % name, hal.HAL_BIT)
tempPwm = hal.newsig('%s-temp-pwm' % name, hal.HAL_FLOAT)
tempPwmMax = hal.newsig('%s-temp-pwm-max' % name, hal.HAL_FLOAT)
tempLimitMin = hal.newsig('%s-temp-limit-min' % name, hal.HAL_FLOAT)
tempLimitMax = hal.newsig('%s-temp-limit-max' % name, hal.HAL_FLOAT)
tempStandby = hal.newsig('%s-temp-standby' % name, hal.HAL_FLOAT)
tempInLimit = hal.newsig('%s-temp-in-limit' % name, hal.HAL_BIT)
tempThermOk = hal.newsig('%s-temp-therm-ok' % name, hal.HAL_BIT)
error = hal.newsig('%s-error' % name, hal.HAL_BIT)
active = hal.newsig('%s-active' % name, hal.HAL_BIT)
tempPidPgain = hal.newsig('%s-temp-pid-Pgain' % name, hal.HAL_FLOAT)
tempPidIgain = hal.newsig('%s-temp-pid-Igain' % name, hal.HAL_FLOAT)
tempPidDgain = hal.newsig('%s-temp-pid-Dgain' % name, hal.HAL_FLOAT)
tempPidMaxerrorI = hal.newsig('%s-temp-pid-maxerrorI' % name, hal.HAL_FLOAT)
tempPidOut = hal.newsig('%s-temp-pid-out' % name, hal.HAL_FLOAT)
tempPidBias = hal.newsig('%s-temp-pid-bias' % name, hal.HAL_FLOAT)
tempRangeMin = hal.newsig('%s-temp-range-min' % name, hal.HAL_FLOAT)
tempRangeMax = hal.newsig('%s-temp-range-max' % name, hal.HAL_FLOAT)
noErrorIn = hal.newsig('%s-no-error-in' % name, hal.HAL_BIT)
errorIn = hal.newsig('%s-error-in' % name, hal.HAL_BIT)
# reset set temperature when estop is cleared
reset = rt.newinst('reset', 'reset.%s-temp-set' % name)
hal.addf(reset.name, thread)
reset.pin('reset-float').set(0.0)
reset.pin('out-float').link(tempSet)
reset.pin('rising').set(True)
reset.pin('falling').set(False)
reset.pin('trigger').link('estop-reset')
tempPidBiasOut = tempPidBias
# coolingFan compensation
if coolingFan:
tempPidFanBias = hal.newsig('%s-temp-pid-fan-bias' % name, hal.HAL_FLOAT)
tempPidBiasOut = hal.newsig('%s-temp-pid-bias-out' % name, hal.HAL_FLOAT)
scale = rt.newinst('scale', 'scale.%s-temp-pid-fan-bias' % name)
hal.addf(scale.name, thread)
scale.pin('in').link('%s.pwm' % coolingFan)
scale.pin('out').link(tempPidFanBias)
scale.pin('gain').set(c.find(section, 'FAN_BIAS'))
sum2 = rt.newinst('sum2', 'sum2.%s-temp-pid-bias' % name)
hal.addf(sum2.name, thread)
sum2.pin('in0').link(tempPidBias)
sum2.pin('in1').link(tempPidFanBias)
sum2.pin('out').link(tempPidBiasOut)
# PID
pid = rt.newinst('pid', 'pid.%s' % name)
hal.addf('%s.do-pid-calcs' % pid.name, thread)
pid.pin('enable').link('emcmot-0-enable') # motor enable
pid.pin('feedback').link(tempMeas)
pid.pin('command').link(tempSet)
pid.pin('output').link(tempPidOut)
pid.pin('maxoutput').link(tempPwmMax)
pid.pin('bias').link(tempPidBias)
pid.pin('Pgain').link(tempPidPgain)
pid.pin('Igain').link(tempPidIgain)
pid.pin('Dgain').link(tempPidDgain)
pid.pin('maxerrorI').link(tempPidMaxerrorI)
# Limit heater PWM to positive values
# PWM mimics hm2 implementation, which generates output for negative values
limit1 = rt.newinst('limit1', 'limit.%s-temp-heaterl' % name)
hal.addf(limit1.name, thread)
limit1.pin('in').link(tempPidOut)
limit1.pin('out').link(tempPwm)
limit1.pin('min').set(0.0)
limit1.pin('max').link(tempPwmMax)
# Temperature checking
sum2 = rt.newinst('sum2', 'sum2.%s-temp-range-pos' % name)
hal.addf(sum2.name, thread)
sum2.pin('in0').link(tempSet)
sum2.pin('in1').set(c.find(section, 'TEMP_RANGE_POS_ERROR'))
sum2.pin('out').link(tempRangeMax)
sum2 = rt.newinst('sum2', 'sum2.%s-temp-range-neg' % name)
hal.addf(sum2.name, thread)
sum2.pin('in0').link(tempSet)
sum2.pin('in1').set(c.find(section, 'TEMP_RANGE_NEG_ERROR'))
sum2.pin('out').link(tempRangeMin)
#the output of this component will say if measured temperature is in range of set value
wcomp = rt.newinst('wcomp', 'wcomp.%s-temp-in-range' % name)
hal.addf(wcomp.name, thread)
wcomp.pin('min').link(tempRangeMin)
wcomp.pin('max').link(tempRangeMax)
wcomp.pin('in').link(tempMeas)
wcomp.pin('out').link(tempInRange)
# limit the output temperature to prevent damage when thermistor is broken/removed
wcomp = rt.newinst('wcomp', 'wcomp.%s-temp-in-limit' % name)
hal.addf(wcomp.name, thread)
wcomp.pin('min').link(tempLimitMin)
wcomp.pin('max').link(tempLimitMax)
wcomp.pin('in').link(tempMeas)
wcomp.pin('out').link(tempInLimit)
# check the thermistor
# net e0.temp.meas => thermistor-check.e0.temp
# net e0.temp.in-range => not.e0-temp-range.in
# net e0.temp.in-range_n <= not.e0-temp-range.out
# net e0.temp.in-range_n => thermistor-check.e0.enable
# net e0.heaterl => thermistor-check.e0.pid
# net e0.therm-ok <= thermistor-check.e0.no-error
# no error chain
and3 = rt.newinst('andn', 'and3.%s-no-error-in' % name, pincount=3)
hal.addf(and3.name, thread)
and3.pin('in0').link(tempThermOk)
and3.pin('in1').link(tempInLimit)
if hardwareOkSignal:
and3.pin('in2').link(hardwareOkSignal)
else:
and3.pin('in2').set(True)
and3.pin('out').link(noErrorIn)
tempThermOk.set(True) # thermistor checking for now disabled
notComp = rt.newinst('not', 'not.%s-error-in' % name)
hal.addf(notComp.name, thread)
notComp.pin('in').link(noErrorIn)
notComp.pin('out').link(errorIn)
safetyLatch = rt.newinst('safety_latch', 'safety-latch.%s-error' % name)
hal.addf(safetyLatch.name, thread)
safetyLatch.pin('error-in').link(errorIn)
safetyLatch.pin('error-out').link(error)
safetyLatch.pin('reset').link('estop-reset')
safetyLatch.pin('threshold').set(500) # 500ms error
safetyLatch.pin('latching').set(True)
# active chain
comp = rt.newinst('comp', 'comp.%s-active' % name)
hal.addf(comp.name, thread)
comp.pin('in0').set(0.0001)
comp.pin('hyst').set(0.0)
comp.pin('in1').link(tempPwm)
comp.pin('out').link(active)
# Thermistor checking
# setp thermistor-check.e0.wait 9.0
# setp thermistor-check.e0.min-pid 1.5 # disable0.25
# setp thermistor-check.e0.min-temp 1.5
# net e0.pid.bias => thermistor-check.e0.bias
# Hotend fan
if hotendFan:
comp = rt.newinst('comp', 'comp.%s-pwm-enable' % hotendFan)
hal.addf(comp.name, thread)
comp.pin('in0').set(c.find(section, 'HOTEND_FAN_THRESHOLD', 50.0))
comp.pin('in1').link(tempMeas)
comp.pin('hyst').set(c.find(section, 'HOTEND_FAN_HYST', 2.0))
comp.pin('out').link('%s-pwm-enable' % hotendFan)
hal.Signal('%s-pwm' % hotendFan).set(1.0)
rcomps.create_temperature_rcomp(name)
motion.setup_temperature_io(name)
# init parameter signals
tempLimitMin.set(c.find(section, 'TEMP_LIMIT_MIN'))
tempLimitMax.set(c.find(section, 'TEMP_LIMIT_MAX'))
tempStandby.set(c.find(section, 'TEMP_STANDBY'))
tempPwmMax.set(c.find(section, 'PWM_MAX'))
tempPidPgain.set(c.find(section, 'PID_PGAIN'))
tempPidIgain.set(c.find(section, 'PID_IGAIN'))
tempPidDgain.set(c.find(section, 'PID_DGAIN'))
tempPidMaxerrorI.set(c.find(section, 'PID_MAXERRORI'))
tempPidBias.set(c.find(section, 'PID_BIAS'))
def setup_exp(name):
hal.newsig('%s-pwm' % name, hal.HAL_FLOAT, init=0.0)
hal.newsig('%s-enable' % name, hal.HAL_BIT, init=False)
def setup_exp(name):
hal.newsig('%s-pwm' % name, hal.HAL_FLOAT, init=0.0)
hal.newsig('%s-enable' % name, hal.HAL_BIT, init=False)
def velocity_jog(extruders, thread):
''' Velocity extruding jog support '''
# from ui
jogVelocity = hal.newsig('ve-jog-velocity', hal.HAL_FLOAT)
jogVelocityLimited = hal.newsig('ve-jog-velocity-limited', hal.HAL_FLOAT)
jogDirection = hal.newsig('ve-jog-direction', hal.HAL_BIT)
jogDistance = hal.newsig('ve-jog-distance', hal.HAL_FLOAT)
jogTrigger = hal.newsig('ve-jog-trigger', hal.HAL_BIT)
jogDtg = hal.newsig('ve-jog-dtg', hal.HAL_FLOAT)
jogContinuous = hal.newsig('ve-jog-continuous', hal.HAL_BIT)
# helper signals
jogEnable = hal.newsig('ve-jog-enable', hal.HAL_BIT)
jogVelocityNeg = hal.newsig('ve-jog-velocity-neg', hal.HAL_FLOAT)
jogVelocitySigned = hal.newsig('ve-velocity-signed', hal.HAL_FLOAT)
jogTime = hal.newsig('ve-jog-time', hal.HAL_FLOAT)
jogTimeLeft = hal.newsig('ve-jog-time-left', hal.HAL_FLOAT)
jogActive = hal.newsig('ve-jog-active', hal.HAL_BIT)
maxVelocity = hal.Signal('ve-max-velocity', hal.HAL_FLOAT)
baseVel = hal.Signal('ve-base-vel')
extruderEn = hal.Signal('ve-extruder-en')
# multiplexing jog velocity for multiple extruders
ioMux = rt.newinst('io_muxn',
'io-mux%i.ve-jog-velocity' % extruders,
pincount=extruders)
hal.addf(ioMux.name, thread)
ioMux.pin('out').link(jogVelocity)
ioMux.pin('sel').link('extruder-sel')
for n in range(0, extruders):
signal = hal.newsig('ve-jog-velocity-e%i' % n, hal.HAL_FLOAT)
ioMux.pin('in%i' % n).link(signal)
limit1 = rt.newinst('limit1', 'limit1.ve-jog-velocity-limited')
hal.addf(limit1.name, thread)
limit1.pin('in').link(jogVelocity)
limit1.pin('out').link(jogVelocityLimited)
limit1.pin('min').set(0.01) # prevent users from setting 0 as jog velocity
limit1.pin('max').link(maxVelocity)
neg = rt.newinst('neg', 'neg.ve-jog-velocity-neg')
hal.addf(neg.name, thread)
neg.pin('in').link(jogVelocityLimited)
neg.pin('out').link(jogVelocityNeg)
mux2 = rt.newinst('mux2', 'mux2.ve-jog-velocity-signed')
hal.addf(mux2.name, thread)
mux2.pin('in0').link(jogVelocityLimited)
mux2.pin('in1').link(jogVelocityNeg)
mux2.pin('sel').link(jogDirection)
mux2.pin('out').link(jogVelocitySigned)
div2 = rt.newinst('div2', 'div2.ve-jog-time')
hal.addf(div2.name, thread)
div2.pin('in0').link(jogDistance)
div2.pin('in1').link(jogVelocityLimited)
div2.pin('out').link(jogTime)
oneshot = rt.newinst('oneshot', 'oneshot.ve-jog-active')
hal.addf(oneshot.name, thread)
oneshot.pin('in').link(jogTrigger)
oneshot.pin('width').link(jogTime)
oneshot.pin('time-left').link(jogTimeLeft)
oneshot.pin('rising').set(True)
oneshot.pin('falling').set(False)
oneshot.pin('retriggerable').set(1)
oneshot.pin('out').link(jogActive)
reset = rt.newinst('reset', 'reset.ve-jog-trigger')
hal.addf(reset.name, thread)
reset.pin('reset-bit').set(False)
reset.pin('out-bit').link(jogTrigger)
reset.pin('rising').set(False)
reset.pin('falling').set(True)
reset.pin('trigger').link(jogActive)
or2 = rt.newinst('or2', 'or2.ve-jog-enable')
hal.addf(or2.name, thread)
or2.pin('in0').link(jogContinuous)
or2.pin('in1').link(jogActive)
or2.pin('out').link(jogEnable)
mux2 = rt.newinst('mux2', 'mux2.ve-base-vel')
hal.addf(mux2.name, thread)
mux2.pin('in0').set(0.0)
mux2.pin('in1').link(jogVelocitySigned)
mux2.pin('sel').link(jogEnable)
mux2.pin('out').link(baseVel)
mult2 = rt.newinst('mult2', 'mult2.ve-jog-dtg')
hal.addf(mult2.name, thread)
mult2.pin('in0').link(jogVelocityLimited)
mult2.pin('in1').link(jogTimeLeft)
mult2.pin('out').link(jogDtg)
# disable extruder on jog
reset = rt.newinst('reset', 'reset.extruder-en1')
hal.addf(reset.name, thread)
reset.pin('rising').set(True)
reset.pin('falling').set(False)
reset.pin('retriggerable').set(True)
reset.pin('reset-bit').set(False)
reset.pin('trigger').link(jogTrigger)
reset.pin('out-bit').link(extruderEn)
reset = rt.newinst('reset', 'reset.extruder-en2')
hal.addf(reset.name, thread)
reset.pin('rising').set(True)
reset.pin('falling').set(False)
reset.pin('retriggerable').set(True)
reset.pin('reset-bit').set(False)
reset.pin('trigger').link(jogContinuous)
reset.pin('out-bit').link(extruderEn)
rcomps.create_ve_jog_rcomp(extruders=extruders)
def velocity_extrusion(extruders, thread):
''' Velocity extrusion support '''
# from motion/ui
crossSection = hal.newsig('ve-cross-section', hal.HAL_FLOAT)
crossSectionIn = hal.newsig('ve-cross-section-in', hal.HAL_FLOAT)
lineWidth = hal.newsig('ve-line-width', hal.HAL_FLOAT)
lineHeight = hal.newsig('ve-line-height', hal.HAL_FLOAT)
filamentDia = hal.newsig('ve-filament-dia', hal.HAL_FLOAT)
extrudeScale = hal.newsig('ve-extrude-scale', hal.HAL_FLOAT)
extrudeAccelAdjGain = hal.newsig('ve-extrude-accel-adj-gain', hal.HAL_FLOAT)
retractVel = hal.newsig('ve-retract-vel', hal.HAL_FLOAT)
retractLen = hal.newsig('ve-retract-len', hal.HAL_FLOAT)
maxVelocity = hal.newsig('ve-max-velocity', hal.HAL_FLOAT)
# helper signals
nozzleVel = hal.newsig('ve-nozzle-vel', hal.HAL_FLOAT)
nozzleDischarge = hal.newsig('ve-nozzle-discharge', hal.HAL_FLOAT)
filamentDiaSquared = hal.newsig('ve-filament-dia-squared', hal.HAL_FLOAT)
filamentArea = hal.newsig('ve-filament-area', hal.HAL_FLOAT)
extrudeRate = hal.newsig('ve-extrude-rate', hal.HAL_FLOAT)
extrudeRateScaled = hal.newsig('ve-extrude-rate-scaled', hal.HAL_FLOAT)
extrudeAccel = hal.newsig('ve-extrude-accel', hal.HAL_FLOAT)
extrudeAccelAdj = hal.newsig('ve-extrude-accel-adj', hal.HAL_FLOAT)
extrudeRateAdj = hal.newsig('ve-extrude-rate-adj', hal.HAL_FLOAT)
extruderEn = hal.newsig('ve-extruder-en', hal.HAL_BIT)
retractVelNeg = hal.newsig('ve-retract-vel-neg', hal.HAL_FLOAT)
retractTime = hal.newsig('ve-retract-time', hal.HAL_FLOAT)
retract = hal.newsig('ve-retract', hal.HAL_BIT)
extrudeVel = hal.newsig('ve-extrude-vel', hal.HAL_FLOAT)
baseVel = hal.newsig('ve-base-vel', hal.HAL_FLOAT)
nozzleVel.link('motion.current-vel')
mult2 = rt.newinst('mult2', 'mult2.ve-cross-section')
hal.addf(mult2.name, thread)
mult2.pin('in0').link(lineWidth)
mult2.pin('in1').link(lineHeight)
mult2.pin('out').link(crossSectionIn)
outToIo = rt.newinst('out_to_io', 'out-to-io.ve-cross-section')
hal.addf(outToIo.name, thread)
outToIo.pin('in-float').link(crossSectionIn)
outToIo.pin('out-float').link(crossSection)
# multiply area with speed and we get discharge (mm^3 per second)
mult2 = rt.newinst('mult2', 'mult2.ve-nozzle-discharge')
hal.addf(mult2.name, thread)
mult2.pin('in0').link(crossSection)
mult2.pin('in1').link(nozzleVel)
mult2.pin('out').link(nozzleDischarge)
# calculate filament cross section area
# PI divided by 4
mult2 = rt.newinst('mult2', 'mult2.ve-filament-dia')
hal.addf(mult2.name, thread)
mult2.pin('in0').link(filamentDia)
mult2.pin('in1').link(filamentDia)
mult2.pin('out').link(filamentDiaSquared)
mult2 = rt.newinst('mult2', 'mult2.ve-filament-area')
hal.addf(mult2.name, thread)
mult2.pin('in0').set(math.pi / 4)
mult2.pin('in1').link(filamentDiaSquared)
mult2.pin('out').link(filamentArea)
# calculate extrude rate
div2 = rt.newinst('div2', 'div2.ve-extrude-rate')
hal.addf(div2.name, thread)
div2.pin('in0').link(nozzleDischarge)
div2.pin('in1').link(filamentArea)
div2.pin('out').link(extrudeRate)
# scale extrude rate
mult2 = rt.newinst('mult2', 'mult2.ve-extrude-rate-scaled')
hal.addf(mult2.name, thread)
mult2.pin('in0').link(extrudeRate)
mult2.pin('in1').link(extrudeScale)
mult2.pin('out').link(extrudeRateScaled)
# these are used for a small offset in velocity during acceleration (buildup pressure inside
# the nozzle because of the current speed. Take the maximum accel you've specified in .ini
# get acceleration into lincurve
ddt = rt.newinst('ddt', 'ddt.ve-extruder-accel')
hal.addf(ddt.name, thread)
ddt.pin('in').link(extrudeRateScaled)
ddt.pin('out').link(extrudeAccel)
mult2 = rt.newinst('mult2', 'mult2.ve-extrude-accel-adj')
hal.addf(mult2.name, thread)
mult2.pin('in0').link(extrudeAccel)
mult2.pin('in1').link(extrudeAccelAdjGain)
mult2.pin('out').link(extrudeAccelAdj)
# get adjusted speed for adding to current speed during acceleration
sum2 = rt.newinst('sum2', 'sum2.ve-extrude-rate-adj')
hal.addf(sum2.name, thread)
sum2.pin('in0').link(extrudeRateScaled)
sum2.pin('in1').link(extrudeAccelAdj)
sum2.pin('out').link(extrudeRateAdj)
# negative retract velocity
neg = rt.newinst('neg', 'neg.ve-rectract-vel-neg')
hal.addf(neg.name, thread)
neg.pin('in').link(retractVel)
neg.pin('out').link(retractVelNeg)
# calculate retract time
div2 = rt.newinst('div2', 'div2.ve-retract-time')
hal.addf(div2.name, thread)
div2.pin('in0').link(retractLen)
div2.pin('in1').link(retractVel)
div2.pin('out').link(retractTime)
# We want the retract-charge to run for a fixed time:
# when sel0 set to "1" meaning motion with extrusion" the on the rising edge
# there will temporarily be also sel1 which is high, meaning a pre-charge because the
# sel combination is 11
# when sel1 set to "0" meaning decoupling motion with extrusion" then the falling edge
# will trigger a 01 combination, meaning a retract
# trigger a retract/unretract move when extruder is enable or disabled
oneshot = rt.newinst('oneshot', 'oneshot.ve-retract')
hal.addf(oneshot.name, thread)
oneshot.pin('rising').set(True)
oneshot.pin('falling').set(True)
oneshot.pin('retriggerable').set(True)
oneshot.pin('width').link(retractTime)
oneshot.pin('in').link(extruderEn)
oneshot.pin('out').link(retract)
retract += 'motion.feed-hold' # stop motion until retract/unretract finished
# jogging needs to be inserted here
velocity_jog(extruders, thread)
# now the solution of Andy Pugh for automatically retracting/priming
#00 = motion without extrusion, jog
#01 = retract
#10 = motion with extrusion
#11 = unretract, pre-charge
mux4 = rt.newinst('mux4', 'mux4.ve-extrude-vel')
hal.addf(mux4.name, thread)
mux4.pin('in0').link(baseVel)
mux4.pin('in1').link(retractVelNeg)
mux4.pin('in2').link(extrudeRateAdj)
mux4.pin('in3').link(retractVel)
mux4.pin('out').link(extrudeVel)
mux4.pin('sel0').link(retract)
mux4.pin('sel1').link(extruderEn)
sections = [[retractLen, 'RETRACT_LEN'],
[retractVel, 'RETRACT_VEL'],
[filamentDia, 'FILAMENT_DIA'],
[maxVelocity, 'MAX_VELOCITY'],
[extrudeScale, 'EXTRUDE_SCALE']]
for section in sections:
ioMux = rt.newinst('io_muxn',
'io-mux%i.%s' % (extruders, section[0].name),
pincount=extruders)
hal.addf(ioMux.name, thread)
ioMux.pin('out').link(section[0])
ioMux.pin('sel').link('extruder-sel')
for n in range(0, extruders):
signal = hal.newsig('%s-e%i' % (section[0].name, n), hal.HAL_FLOAT)
ioMux.pin('in%i' % n).link(signal)
signal.set(c.find('EXTRUDER_%i' % n, section[1]))
extrudeAccelAdjGain.set(0.05)
if baseVel.writers < 1: # can only write when jogging not configured
baseVel.set(0.0)
rcomps.create_ve_params_rcomp()
storage.setup_ve_storage(extruders=extruders)
motion.setup_ve_io()
from machinekit import hal
from machinekit import rtapi as rt
# we need a thread to execute the component functions
rt.newthread('main-thread', 1000000, fp=False)
# create the signal for connecting the components
input0 = hal.newsig('input0', hal.HAL_BIT)
input1 = hal.newsig('input1', hal.HAL_BIT)
output = hal.newsig('output', hal.HAL_BIT)
# and2 component
and2 = rt.newinst('and2', 'and2.demo')
and2.pin('in0').link(input0)
and2.pin('in1').link(input1)
and2.pin('out').link(output)
hal.addf(and2.name, 'main-thread')
# create remote component
rcomp = hal.RemoteComponent('anddemo', timer=100)
rcomp.newpin('button0', hal.HAL_BIT, hal.HAL_OUT)
rcomp.newpin('button1', hal.HAL_BIT, hal.HAL_OUT)
rcomp.newpin('led', hal.HAL_BIT, hal.HAL_IN)
rcomp.ready()
# link remote component pins
rcomp.pin('button0').link(input0)
rcomp.pin('button1').link(input1)
rcomp.pin('led').link(output)
# ready to start the threads
def test_check_net_args():
l.log()
try:
hal.net()
except TypeError:
pass
assert 'c1-s32out' not in hal.signals
try:
hal.net(None, "c1.s32out")
except TypeError:
pass
assert 'c1-s32out' not in hal.signals
# single pin argument
assert hal.pins["c1.s32out"].linked is False
try:
hal.net("c1.s32out")
# RuntimeError: net: at least one pin name expected
except RuntimeError:
pass
# XXX die beiden gehen daneben:
# der pin wird trotz runtime error gelinkt und das signal erzeugt:
# offensichtlich hal_net.pyx:39 vor dem test in hal_net.pyx:60
assert hal.pins["c1.s32out"].linked is False
assert 'c1-s32out' not in hal.signals
# single signal argument
try:
hal.net("noexiste")
# RuntimeError: net: at least one pin name expected
except RuntimeError:
pass
# two signals
hal.newsig('sig1', hal.HAL_FLOAT)
hal.newsig('sig2', hal.HAL_FLOAT)
try:
hal.net('sig1', 'sig2')
# NameError: no such pin: sig2
except NameError:
pass
assert "noexiste" not in hal.signals
hal.net("noexiste", "c1.s32out")
assert "noexiste" in hal.signals
ne = hal.signals["noexiste"]
assert ne.writers == 1
assert ne.readers == 0
assert ne.bidirs == 0
try:
hal.net("floatsig1", "c1.s32out")
raise "should not happen"
except RuntimeError:
pass
def __init__(self, name='motor', thread='base_thread', eqep='eQEP0',
eqepScale=2797.0,
pwmDown='hpg.pwmgen.00.out.00',
pwmUp='hpg.pwmgen.00.out.01',
enableDown='bb_gpio.p9.out-15',
enableUp='bb_gpio.p9.out-17'):
sigPgain = hal.newsig('%s-pgain' % name, hal.HAL_FLOAT)
sigIgain = hal.newsig('%s-igain' % name, hal.HAL_FLOAT)
sigDgain = hal.newsig('%s-dgain' % name, hal.HAL_FLOAT)
sigCmdVel = hal.newsig('%s-cmd-vel' % name, hal.HAL_FLOAT)
sigPwmIn = hal.newsig('%s-pwm-in' % name, hal.HAL_FLOAT)
sigPos = hal.newsig('%s-pos' % name, hal.HAL_FLOAT)
sigVel = hal.newsig('%s-vel' % name, hal.HAL_FLOAT)
sigAcc = hal.newsig('%s-acc' % name, hal.HAL_FLOAT)
sigUp = hal.newsig('%s-pwm-up' % name, hal.HAL_FLOAT)
sigDown = hal.newsig('%s-pwm-down' % name, hal.HAL_FLOAT)
sigEnable = hal.newsig('%s-enable' % name, hal.HAL_BIT)
sigPwmEn = hal.newsig('%s-pwm-enable' % name, hal.HAL_BIT)
sigTuneStart = hal.newsig('%s-tune-start' % name, hal.HAL_BIT)
sigTuneMode = hal.newsig('%s-tune-mode' % name, hal.HAL_BIT)
# 43.7:1 gear
# encoder resolution of 64 counts per revolution of the motor shaft,
# 2797 counts per revolution of the gearboxs output shaft.
# for eQEP0.position in shaft revs:
hal.Pin('%s.position-scale' % eqep).set(eqepScale)
hal.Pin('%s.capture-prescaler' % eqep).set(5)
hal.Pin('%s.min-speed-estimate' % eqep).set(0.001)
# feed into PID
sigPos.link('%s.position' % eqep)
# for UI feedback
sigVel.link('%s.velocity' % eqep)
# ddt for accel
ddt = rt.newinst('ddt', 'ddt.%s-acc' % name)
hal.addf(ddt.name, thread)
ddt.pin('in').link(sigVel)
ddt.pin('out').link(sigAcc)
# PID
pid = rt.newinst('at_pid', 'pid.%s-vel' % name)
hal.addf('%s.do-pid-calcs' % pid.name, thread)
pid.pin('maxoutput').set(1.0) # set maxout to prevent windup effect
pid.pin('Pgain').link(sigPgain)
pid.pin('Igain').link(sigIgain)
pid.pin('Dgain').link(sigDgain)
pid.pin('command').link(sigCmdVel)
pid.pin('output').link(sigPwmIn)
pid.pin('feedback').link(sigVel)
pid.pin('enable').link(sigEnable)
# auto tuning
pid.pin('tuneCycles').set(200)
pid.pin('tuneEffort').set(0.15)
pid.pin('tuneMode').link(sigTuneMode)
pid.pin('tuneStart').link(sigTuneStart)
# automatically start auto tuning when switched to tune mode
timedelay = rt.newinst('timedelay', 'timedelay.%s' % sigTuneStart.name)
hal.addf(timedelay.name, thread)
timedelay.pin('in').link(sigTuneMode)
timedelay.pin('on-delay').set(0.1)
timedelay.pin('off-delay').set(0.0)
# convert out singnal to IO
outToIo = rt.newinst('out_to_io', 'out-to-io.%s' % sigTuneStart.name)
hal.addf(outToIo.name, thread)
timedelay.pin('out').link(outToIo.pin('in-bit'))
outToIo.pin('out-bit').link(sigTuneStart)
# reset the tune mode to false once tuning is finished
reset = rt.newinst('reset', 'reset.%s' % sigTuneMode.name)
hal.addf(reset.name, thread)
reset.pin('out-bit').link(sigTuneMode)
reset.pin('reset-bit').set(False)
reset.pin('trigger').link(sigTuneStart)
reset.pin('rising').set(False)
reset.pin('falling').set(True)
# hbridge
hbridge = rt.newinst('hbridge', 'hbridge.%s' % name)
hal.addf(hbridge.name, thread)
hbridge.pin('up').link(sigUp)
hbridge.pin('down').link(sigDown)
hbridge.pin('enable').link(sigEnable)
hbridge.pin('enable-out').link(sigPwmEn)
hbridge.pin('command').link(sigPwmIn)
# PWM signals
hal.Pin('%s.value' % pwmUp).link(sigUp)
hal.Pin('%s.value' % pwmDown).link(sigDown)
# Enable
hal.Pin(enableUp).link(sigPwmEn)
hal.Pin(enableDown).link(sigPwmEn)
hal.Pin('%s.enable' % pwmUp).link(sigPwmEn)
hal.Pin('%s.enable' % pwmDown).link(sigPwmEn)
# prevent pid runup if disabled
sigEnable.set(True)
# storage
hal.Pin('storage.%s.pgain' % name).link(sigPgain)
hal.Pin('storage.%s.igain' % name).link(sigIgain)
hal.Pin('storage.%s.dgain' % name).link(sigDgain)
def _init_jog(self):
jog_vel_limited_sig = hal.newsig('ve%d.jog-vel-limited' % self.nr, hal.HAL_FLOAT)
jog_vel_limit = rtapi.newinst('limit1', 've%d.limit1.jog-vel-limit' % self.nr)
hal.addf(jog_vel_limit.name, SERVO_THREAD)
jog_vel_limit.pin('in').link(self.jog_vel_sig)
jog_vel_limit.pin('out').link(jog_vel_limited_sig)
jog_vel_limit.pin('min').set(0.01)
jog_vel_limit.pin('max').link(self.max_jog_vel_sig)
jog_vel_limited_neg_sig = hal.newsig('ve%d.jog-vel-limited-neg' % self.nr, hal.HAL_FLOAT)
jog_vel_limited_neg = rtapi.newinst('neg', 've%d.neg.jog-vel-limited-neg' % self.nr)
hal.addf(jog_vel_limited_neg.name, SERVO_THREAD)
jog_vel_limited_neg.pin('in').link(jog_vel_limited_sig)
jog_vel_limited_neg.pin('out').link(jog_vel_limited_neg_sig)
jog_vel_signed_sig = hal.newsig('ve%d.jog-vel-signed' % self.nr, hal.HAL_FLOAT)
jog_vel_signed = rtapi.newinst('mux2', 've%d.mux2.jog-vel-signed' % self.nr)
hal.addf(jog_vel_signed.name, SERVO_THREAD)
jog_vel_signed.pin('in0').link(jog_vel_limited_sig)
jog_vel_signed.pin('in1').link(jog_vel_limited_neg_sig)
jog_vel_signed.pin('sel').link(self.jog_direction_sig)
jog_vel_signed.pin('out').link(jog_vel_signed_sig)
jog_time_sig = hal.newsig('ve%d.jog-time' % self.nr, hal.HAL_FLOAT)
jog_time_left_sig = hal.newsig('ve%d.jog-time-left' % self.nr, hal.HAL_FLOAT)
jog_time = rtapi.newinst('div2', 've%d.div2.jog-time' % self.nr)
hal.addf(jog_time.name, SERVO_THREAD)
jog_time.pin('in0').link(self.jog_distance_sig)
jog_time.pin('in1').link(jog_vel_limited_sig)
jog_time.pin('out').link(jog_time_sig)
jog_single_sig = hal.newsig('ve%d.jog-single' % self.nr, hal.HAL_BIT)
jog_single_oneshot = rtapi.newinst('oneshot', 've%d.oneshot.jog-single' % self.nr)
hal.addf(jog_single_oneshot.name, SERVO_THREAD)
jog_single_oneshot.pin('in').link(self.jog_trigger_sig)
jog_single_oneshot.pin('width').link(jog_time_sig)
jog_single_oneshot.pin('time-left').link(jog_time_left_sig)
jog_single_oneshot.pin('rising').set(True)
jog_single_oneshot.pin('falling').set(False)
jog_single_oneshot.pin('retriggerable').set(1)
jog_single_oneshot.pin('out').link(jog_single_sig)
jog_reset_trigger = rtapi.newinst('reset', 've%d.reset.jog-trigger' % self.nr)
hal.addf(jog_reset_trigger.name, SERVO_THREAD)
jog_reset_trigger.pin('trigger').link(jog_single_sig)
jog_reset_trigger.pin('rising').set(False)
jog_reset_trigger.pin('falling').set(True)
jog_reset_trigger.pin('reset-bit').set(False)
jog_reset_trigger.pin('out-bit').link(self.jog_trigger_sig)
jog_enable_sig = hal.newsig('ve%d.jog-enable' % self.nr, hal.HAL_BIT)
jog_enable = rtapi.newinst('or2', 've%d.or2.jog-enable' % self.nr)
hal.addf(jog_enable.name, SERVO_THREAD)
jog_enable.pin('in0').link(self.jog_continuous_sig)
jog_enable.pin('in1').link(jog_single_sig)
jog_enable.pin('out').link(jog_enable_sig)
jog_vel_output = rtapi.newinst('mux2', 've%d.mux2.jog-vel-output' % self.nr)
hal.addf(jog_vel_output.name, SERVO_THREAD)
jog_vel_output.pin('in0').set(0.0)
jog_vel_output.pin('in1').link(jog_vel_signed_sig)
jog_vel_output.pin('sel').link(jog_enable_sig)
jog_vel_output.pin('out').link(self.jog_vel_output_sig)
jog_dtg = rtapi.newinst('mult2', 've%d.mult2.jog-dtg' % self.nr)
hal.addf(jog_dtg.name, SERVO_THREAD)
jog_dtg.pin('in0').link(jog_vel_limited_sig)
jog_dtg.pin('in1').link(jog_time_left_sig)
jog_dtg.pin('out').link(self.jog_dtg_sig)
disable1 = rtapi.newinst('reset', 've%d.reset.on-jog-single' % self.nr)
hal.addf(disable1.name, SERVO_THREAD)
disable1.pin('trigger').link(self.jog_trigger_sig)
disable1.pin('rising').set(True)
disable1.pin('falling').set(False)
disable1.pin('retriggerable').set(True)
disable1.pin('reset-bit').set(False)
disable1.pin('out-bit').link(self.enable_sig)
disable2 = rtapi.newinst('reset', 've%d.reset.on-jog-continuous' % self.nr)
hal.addf(disable2.name, SERVO_THREAD)
disable2.pin('trigger').link(self.jog_continuous_sig)
disable2.pin('rising').set(True)
disable2.pin('falling').set(False)
disable2.pin('retriggerable').set(True)
disable2.pin('reset-bit').set(False)
disable2.pin('out-bit').link(self.enable_sig)