def _setup_user_control(thread):
conv_state_cmd = rt.newinst('conv_u32_bit', 'conv_u32_bit.state-cmd')
hal.addf(conv_state_cmd.name, thread.name)
conv_state_cmd.pin('in').link('state-cmd')
conv_state_cmd.pin('out').link('power-on')
conv_state_fb = rt.newinst('conv_bit_s32', 'conv_bit_s32.state-fb')
hal.addf(conv_state_fb.name, thread.name)
conv_state_fb.pin('in').link('lamp-yellow')
conv_state_fb.pin('out').link('state-fb')
reset = rt.newinst('reset', 'reset.state-cmd')
hal.addf(reset.name, thread.name)
reset.pin('trigger').link('estop-active')
reset.pin('out-u32').link('state-cmd')
reset.pin('rising').set(True)
and2 = rt.newinst('and2', 'and2.reset-state-cmd')
hal.addf(and2.name, thread.name)
and2.pin('in0').link('reset')
and2.pin('in1').link('estop-active')
oneshot = rt.newinst('oneshot', 'oneshot.reset')
hal.addf(oneshot.name, thread.name)
oneshot.pin('in').link('reset-in')
oneshot.pin('out-not').link('reset')
oneshot.pin('width').set(BorunteConfig.RESET_DELAY_S)
oneshot.pin('rising').set(True)
oneshot.pin('falling').set(True)
def init_gantry(axisIndex, joints=2, latching=True):
if latching:
comp = 'lgantry'
else:
comp = 'gantry'
rt.newinst(comp, 'gantry.%i' % axisIndex, pincount=joints)
rcomps.create_gantry_rcomp(axisIndex=axisIndex)
def setup_stepper_multiplexer(stepgenIndex, sections, selSignal, thread):
num = len(sections)
sigBase = 'stepgen-%i' % stepgenIndex
unsignedSignals = [['dirsetup', 'DIRSETUP'], ['dirhold', 'DIRHOLD'],
['steplen', 'STEPLEN'], ['stepspace', 'STEPSPACE']]
floatSignals = [['scale', 'SCALE'], ['max-vel', 'STEPGEN_MAX_VEL'],
['max-acc', 'STEPGEN_MAX_ACC']]
for item in unsignedSignals:
signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_U32)
mux = rt.newinst('muxn_u32',
'mux%i.%s' % (num, signal.name),
pincount=num)
hal.addf(mux.name, thread)
for n, section in enumerate(sections):
mux.pin('in%i' % n).set(c.find(section, item[1]))
mux.pin('sel').link(selSignal)
mux.pin('out').link(signal)
for item in floatSignals:
signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_FLOAT)
mux = rt.newinst('muxn', 'mux%i.%s' % (num, signal.name), pincount=num)
hal.addf(mux.name, thread)
for n, section in enumerate(sections):
mux.pin('in%i' % n).set(c.find(section, item[1]))
mux.pin('sel').link(selSignal)
mux.pin('out').link(signal)
def _setup_joint_ferror(nr, thread):
cmd_fb_pos = hal.Signal('joint-{}-cmd-fb-pos'.format(nr),
hal.HAL_FLOAT)
fb_in_pos = hal.Signal('joint-{}-fb-in-pos'.format(nr), hal.HAL_FLOAT)
ferror = hal.Signal('joint-{}-ferror'.format(nr), hal.HAL_FLOAT)
ferror_abs = hal.Signal('joint-{}-ferror-abs'.format(nr),
hal.HAL_FLOAT)
ferror_max = hal.Signal('joint-{}-ferror-max'.format(nr),
hal.HAL_FLOAT)
ferror_active = hal.Signal('joint-{}-ferror-active'.format(nr),
hal.HAL_BIT)
sum_ferror = rt.newinst('sum2', 'sum2.joint-{}-ferror'.format(nr))
sum_ferror.pin('in0').link(cmd_fb_pos)
sum_ferror.pin('in1').link(fb_in_pos)
sum_ferror.pin('gain1').set(-1.0)
sum_ferror.pin('out').link(ferror)
abs_ferror = rt.newinst('abs', 'abs.joint-{}-ferror'.format(nr))
abs_ferror.pin('in').link(ferror)
abs_ferror.pin('out').link(ferror_abs)
comp = rt.newinst('comp', 'comp.joint-{}-ferror'.format(nr))
comp.pin('in0').link(ferror_max)
comp.pin('in1').link(ferror_abs)
comp.pin('out').link(ferror_active)
hal.addf(sum_ferror.name, thread.name)
hal.addf(abs_ferror.name, thread.name)
hal.addf(comp.name, thread.name)
def setup_estop(errorSignals, thread):
# Create estop signal chain
estopUser = hal.Signal('estop-user', hal.HAL_BIT)
estopReset = hal.Signal('estop-reset', hal.HAL_BIT)
estopOut = hal.Signal('estop-out', hal.HAL_BIT)
estopIn = hal.Signal('estop-in', hal.HAL_BIT)
estopError = hal.Signal('estop-error', hal.HAL_BIT)
num = len(errorSignals)
orComp = rt.newinst('orn', 'or%i.estop-error' % num, pincount=num)
hal.addf(orComp.name, thread)
for n, sig in enumerate(errorSignals):
orComp.pin('in%i' % n).link(sig)
orComp.pin('out').link(estopError)
estopLatch = rt.newinst('estop_latch', 'estop-latch')
hal.addf(estopLatch.name, thread)
estopLatch.pin('ok-in').link(estopUser)
estopLatch.pin('fault-in').link(estopError)
estopLatch.pin('reset').link(estopReset)
estopLatch.pin('ok-out').link(estopOut)
estopUser.link('iocontrol.0.user-enable-out')
estopReset.link('iocontrol.0.user-request-enable')
# Monitor estop input from hardware
estopIn.link('iocontrol.0.emc-enable-in')
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 usrcomp_watchdog(comps,
enableSignal,
thread,
okSignal=None,
errorSignal=None):
count = len(comps)
watchdog = rt.newinst('watchdog', 'watchdog.usrcomp', pincount=count)
hal.addf('%s.set-timeouts' % watchdog.name, thread)
hal.addf('%s.process' % watchdog.name, 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-%02i' % n).link(sigIn)
watchdog.pin('timeout-%02i' % 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_estop(errorSignals, thread):
# Create estop signal chain
estopUser = hal.Signal('estop-user', hal.HAL_BIT)
estopReset = hal.Signal('estop-reset', hal.HAL_BIT)
estopOut = hal.Signal('estop-out', hal.HAL_BIT)
estopIn = hal.Signal('estop-in', hal.HAL_BIT)
estopError = hal.Signal('estop-error', hal.HAL_BIT)
num = len(errorSignals)
orComp = rt.newinst('orn', 'or%i.estop-error' % num, pincount=num)
hal.addf(orComp.name, thread)
for n, sig in enumerate(errorSignals):
orComp.pin('in%i' % n).link(sig)
orComp.pin('out').link(estopError)
estopLatch = rt.newinst('estop_latch', 'estop-latch')
hal.addf(estopLatch.name, thread)
estopLatch.pin('ok-in').link(estopUser)
estopLatch.pin('fault-in').link(estopError)
estopLatch.pin('reset').link(estopReset)
estopLatch.pin('ok-out').link(estopOut)
estopUser.link('iocontrol.0.user-enable-out')
estopReset.link('iocontrol.0.user-request-enable')
# Monitor estop input from hardware
estopIn.link('iocontrol.0.emc-enable-in')
def _setup_gripper(self):
if not self.tool.startswith('hand_e'):
return
open_close = hal.Signal('gripper-open-close', hal.HAL_BIT)
opened = hal.Signal('gripper-opened', hal.HAL_BIT)
cmd_active = hal.Signal('gripper-cmd-active', hal.HAL_BIT)
mux2 = rt.newinst('mux2', 'mux2.gripper-open-close')
hal.addf(mux2.name, self.thread.name)
mux2.pin('sel').link(open_close)
mux2.pin('in0').set(0xFF)
mux2.pin('in1').set(0x00)
float2u32 = rt.newinst('conv_float_u32', 'conv.gripper-pos')
hal.addf(float2u32.name, self.thread.name)
mux2.pin('out').link(float2u32.pin('in'))
comp = rt.newinst('comp', 'comp.gripper-fb-pos')
hal.addf(comp.name, self.thread.name)
comp.pin('in0').set(254.5)
comp.pin('out').link(opened)
u32tofloat = rt.newinst('conv_u32_float', 'conv-gripper-pos-fb')
hal.addf(u32tofloat.name, self.thread.name)
u32tofloat.pin('out').link(comp.pin('in1'))
robotiq = hal.components['robotiq-gripper']
robotiq.pin('force').set(0xFF)
robotiq.pin('velocity').set(0xFF)
robotiq.pin('position-fb').link(u32tofloat.pin('in'))
robotiq.pin('cmd-active').link(cmd_active)
float2u32.pin('out').link(robotiq.pin('position'))
open_close.set(True) # start opened
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 init_gantry(axisIndex, joints=2, latching=True):
if latching:
comp = 'lgantry'
else:
comp = 'gantry'
rt.newinst(comp, 'gantry.%i' % axisIndex, pincount=joints)
rcomps.create_gantry_rcomp(axisIndex=axisIndex)
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 setup_stepper_multiplexer(stepgenIndex, sections, selSignal, thread):
num = len(sections)
sigBase = 'stepgen-%i' % stepgenIndex
unsignedSignals = [['dirsetup', 'DIRSETUP'],
['dirhold', 'DIRHOLD'],
['steplen', 'STEPLEN'],
['stepspace', 'STEPSPACE']]
floatSignals = [['scale', 'SCALE'],
['max-vel', 'STEPGEN_MAX_VEL'],
['max-acc', 'STEPGEN_MAX_ACC']]
for item in unsignedSignals:
signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_U32)
mux = rt.newinst('muxn_u32', 'mux%i.%s' % (num, signal.name), pincount=num)
hal.addf(mux.name, thread)
for n, section in enumerate(sections):
mux.pin('in%i' % n).set(c.find(section, item[1]))
mux.pin('sel').link(selSignal)
mux.pin('out').link(signal)
for item in floatSignals:
signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_FLOAT)
mux = rt.newinst('muxn', 'mux%i.%s' % (num, signal.name), pincount=num)
hal.addf(mux.name, thread)
for n, section in enumerate(sections):
mux.pin('in%i' % n).set(c.find(section, item[1]))
mux.pin('sel').link(selSignal)
mux.pin('out').link(signal)
def _setup_joint_offset(nr, thread):
home_pos = hal.Signal('joint-{}-home-pos'.format(nr), hal.HAL_FLOAT)
abs_pos = hal.Signal('joint-{}-abs-pos'.format(nr), hal.HAL_FLOAT)
fb_out_pos = hal.Signal('joint-{}-fb-out-pos'.format(nr),
hal.HAL_FLOAT)
fb_in_pos = hal.Signal('joint-{}-fb-in-pos'.format(nr), hal.HAL_FLOAT)
cmd_pos = hal.Signal('joint-{}-cmd-pos'.format(nr), hal.HAL_FLOAT)
real_pos = hal.Signal('joint-{}-real-pos'.format(nr), hal.HAL_FLOAT)
cmd_in_pos = hal.Signal('joint-{}-cmd-in-pos'.format(nr),
hal.HAL_FLOAT)
cmd_out_pos = hal.Signal('joint-{}-cmd-out-pos'.format(nr),
hal.HAL_FLOAT)
pos_offset = hal.Signal('joint-{}-pos-offset'.format(nr),
hal.HAL_FLOAT)
limit_min = hal.Signal('joint-{}-limit-min'.format(nr), hal.HAL_FLOAT)
limit_max = hal.Signal('joint-{}-limit-max'.format(nr), hal.HAL_FLOAT)
son = hal.Signal('son-{}'.format(nr), hal.HAL_BIT)
son_not = hal.Signal('son-{}-not'.format(nr), hal.HAL_BIT)
set_home = hal.Signal('joint-{}-set-home'.format(nr), hal.HAL_BIT)
offset = rt.newinst('offset', 'offset.joint-{}'.format(nr))
offset.pin('offset').link(pos_offset)
offset.pin('fb-in').link(fb_in_pos)
offset.pin('fb-out').link(fb_out_pos)
offset.pin('in').link(cmd_in_pos)
offset.pin('out').link(cmd_out_pos)
abs_joint = rt.newinst('absolute_joint', 'abs-joint.{}'.format(nr))
abs_joint.pin('home-pos').link(home_pos)
abs_joint.pin('abs-pos').link(abs_pos)
abs_joint.pin('real-pos').link(real_pos)
abs_joint.pin('fb-pos').link(fb_in_pos)
abs_joint.pin('offset').link(pos_offset)
abs_joint.pin('set-abs').link(son_not)
abs_joint.pin('set-home').link(set_home)
not_son = rt.newinst('not', 'not.son-{}'.format(nr))
not_son.pin('in').link(son)
not_son.pin('out').link(son_not)
# setup min/max joint limits
limit = rt.newinst('limit1', 'limit1.joint-{}'.format(nr))
limit.pin('min').link(limit_min)
limit.pin('max').link(limit_max)
limit.pin('in').link(cmd_pos)
limit.pin('out').link(cmd_in_pos)
# connect functions in correct order
hal.addf(not_son.name, thread.name)
hal.addf(abs_joint.name, thread.name)
hal.addf(limit.name, thread.name)
hal.addf('{}.update-feedback'.format(offset.name), thread.name)
hal.addf('{}.update-output'.format(offset.name), thread.name)
def _setup_drive_safety_signals(thread):
for i in range(1, NUM_JOINTS + 1):
and2_son = rt.newinst('and2', 'and2.son-{}'.format(i))
hal.addf(and2_son.name, thread.name)
and2_son.pin('in0').link('son-{}'.format(i))
and2_son.pin('in1').link('ok')
and2_son.pin('out').link('son-{}-out'.format(i))
and2_brake = rt.newinst('and2', 'and2.brake-release-{}'.format(i))
hal.addf(and2_brake.name, thread.name)
and2_brake.pin('in0').link('brake-release-{}'.format(i))
and2_brake.pin('in1').link('ok')
and2_brake.pin('out').link('brake-release-{}-out'.format(i))
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 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_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 _init_hm2(self):
mesahandler = MesaHandler(
device='7I80', address=MESA_BOARD_IP, firmware=MESA_FIRMWARE_FILE
)
mesahandler.load_mesacard()
rt.loadrt('hostmot2')
rt.loadrt(
'hm2_eth',
board_ip=MESA_BOARD_IP,
config='"num_encoders={count},num_stepgens={count}"'.format(
count=NUM_JOINTS
),
)
hal.Pin('hm2_7i80.0.watchdog.timeout_ns').set(int(self.thread.period_ns * 2))
hw_watchdog_signal = hal.Signal('hardware-watchdog', hal.HAL_BIT)
hal.Pin('hm2_7i80.0.watchdog.has_bit').link(hw_watchdog_signal)
self.error_signals.append(hw_watchdog_signal)
reset = rt.newinst('reset', 'reset.watchdog')
hal.addf(reset.name, self.thread.name)
reset.pin('trigger').link('power-on')
reset.pin('reset-bit').set(False)
reset.pin('out-bit').link(hw_watchdog_signal)
def _setup_brakes(self):
# pass through SON to brake disable signal
for i in range(1, NUM_JOINTS + 1):
or2 = rt.newinst('or2', 'or2.brake-release-{}'.format(i))
hal.addf(or2.name, self.thread.name)
or2.pin('in0').link('son-{}-out'.format(i))
or2.pin('out').link('brake-release-{}'.format(i))
def __init__(self):
BorunteConfig._setup_joint_offset(nr, hal_config.thread)
# feed back the output for now
sum2 = rt.newinst('sum2', 'sum2-{}-test'.format(nr))
hal.addf(sum2.name, hal_config.thread.name)
sum2.pin('in0').link(self.cmd_out_pos)
sum2.pin('out').link(self.fb_in_pos)
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_gripper(self):
open_close = hal.Signal('gripper-open-close', hal.HAL_BIT)
opened = hal.Signal('gripper-opened', hal.HAL_BIT)
or2 = rt.newinst('or2', 'or2.gripper-open-close')
hal.addf(or2.name, self.thread.name)
or2.pin('in0').link(open_close)
or2.pin('out').link(opened)
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 create_hw_interface(thread):
rt.loadrt('{}/hal_hw_interface'.format(os.environ['COMP_DIR']))
hal.addf('hal_hw_interface', thread.name)
oneshot = rt.newinst('oneshot', 'oneshot.hw_reset')
hal.addf(oneshot.name, thread.name)
oneshot.pin('in').link('power-on')
oneshot.pin('out').link('hw-reset')
oneshot.pin('width').set(HARDWARE_RESET_DELAY_S)
def init_hardware():
watchList = []
#rt=rtapi.RTAPIcommand()
rt.newinst('pp_gpio', 'hpg', 'pincount=6')
rt.newinst('pp_stepgen', 'hps', 'pincount=5')
#print(c.find('HBP', 'THERMISTOR', defaultThermistor))
#raw_input()
# load low-level drivers
if False:
rt.loadrt('hal_bb_gpio',
output_pins='816,822,823,824,825,826,914,923,925',
input_pins='807,808,809,810,817,911,913')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR,
c.find('PRUCONF', 'PRUBIN'))
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
pru=0,
num_stepgens=6,
num_pwmgens=6,
prucode=prubin,
halname='hpg')
# Python user-mode HAL module to read ADC value and generate a thermostat output for PWM
defaultThermistor = 'semitec_103GT_2'
hal.loadusr('hal_temp_bbb',
name='temp',
interval=0.05,
filter_size=1,
cape_board='CRAMPS',
channels='04:%s,05:%s,02:%s,03:%s' %
(c.find('HBP', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_0', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_1', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_2', 'THERMISTOR', defaultThermistor)),
wait_name='temp')
watchList.append(['temp', 0.1])
base.usrcomp_status('temp', 'temp-hw', thread='servo-thread')
base.usrcomp_watchdog(watchList,
'estop-reset',
thread='servo-thread',
errorSignal='watchdog-error')
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_motion(kinematics='trivkins', tp='tp', num_aio=50, num_dio=21):
rt.loadrt(kinematics)
# rt.loadrt(tp)
# hostmot2 setup get names and config from ini file
hostmot2 = bool(c.find('HOSTMOT2', 'DRIVER'))
if hostmot2:
rt.loadrt('hostmot2')
rt.newinst(c.find('HOSTMOT2', 'DRIVER'), c.find('HOSTMOT2', 'DEVNAME'),
"-- ", c.find('HOSTMOT2', 'CONFIG'))
# motion controller, get name and thread periods from ini file
rt.loadrt(c.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=c.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=c.find('TRAJ', 'AXES'),
num_aio=num_aio,
num_dio=num_dio,
tp=tp,
kins=kinematics)
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_extruder_multiplexer(extruders, thread):
extruderSel = hal.Signal('extruder-sel', hal.HAL_S32)
select = rt.newinst('selectn', 'select%i.extruder-sel' % extruders,
pincount=extruders)
hal.addf(select.name, thread)
for n in range(0, extruders):
select.pin('out%i' % n).link('e%i-enable' % n)
select.pin('sel').link(extruderSel)
extruderSel.link('iocontrol.0.tool-prep-number') # driven by T code
def setup_extruder_multiplexer(extruders, thread):
extruderSel = hal.Signal('extruder-sel', hal.HAL_S32)
select = rt.newinst('selectn', 'select%i.extruder-sel' % extruders,
pincount=extruders)
hal.addf(select.name, thread)
for n in range(0, extruders):
select.pin('out%i' % n).link('e%i-enable' % n)
select.pin('sel').link(extruderSel)
extruderSel.link('iocontrol.0.tool-prep-number') # driven by T code
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_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_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_estop_chain(error_signals, thread):
reset = hal.Signal('reset', hal.HAL_BIT)
estop_active = hal.Signal('estop-active', hal.HAL_BIT)
estop_error = hal.Signal('estop-error', hal.HAL_BIT)
estop_in = hal.Signal('estop-in', hal.HAL_BIT)
ok = hal.Signal('ok', hal.HAL_BIT)
num = len(error_signals)
or_comp = rt.newinst('orn',
'or{}.estop-error'.format(num),
pincount=num)
hal.addf(or_comp.name, thread.name)
for n, sig in enumerate(error_signals):
or_comp.pin('in{}'.format(n)).link(sig)
or_comp.pin('out').link(estop_error)
estop_latch = rt.newinst('estop_latch', 'estop-latch')
hal.addf(estop_latch.name, thread.name)
estop_latch.pin('ok-in').link(estop_in)
estop_latch.pin('fault-in').link(estop_error)
estop_latch.pin('fault-out').link(estop_active)
estop_latch.pin('reset').link(reset)
estop_latch.pin('ok-out').link(ok)
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_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 usrcomp_watchdog(comps, enableSignal, thread,
okSignal=None, errorSignal=None):
count = len(comps)
watchdog = rt.newinst('watchdog', 'watchdog.usrcomp', pincount=count)
hal.addf('%s.set-timeouts' % watchdog.name, thread)
hal.addf('%s.process' % watchdog.name, 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-%02i' % n).link(sigIn)
watchdog.pin('timeout-%02i' % 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_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_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 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()
# setup axis feedback
limits = [
[c.find('AXIS_0', 'MAX_LIMIT') - 0.2, c.find('AXIS_0', 'MAX_LIMIT')],
[c.find('AXIS_1', 'MAX_LIMIT') - 0.2, c.find('AXIS_1', 'MAX_LIMIT')],
[c.find('AXIS_2', 'MIN_LIMIT'), 0.0]
]
for n in range(c.find('TRAJ', 'AXES')):
axisPos = hal.newsig('axis-%i-pos' % n, hal.HAL_FLOAT)
hal.Pin('axis.%i.motor-pos-cmd' % n).link(axisPos)
hal.Pin('axis.%i.motor-pos-fb' % n).link(axisPos)
# fake limits
limitHome = hal.newsig('limit-%i-home' % n, hal.HAL_BIT)
hal.Pin('axis.%i.home-sw-in' % n).link(limitHome)
wcomp = rt.newinst('wcomp', 'wcomp.axis-%i-home' % n)
hal.addf(wcomp.name, 'servo-thread')
wcomp.pin('in').link(axisPos)
wcomp.pin('out').link(limitHome)
wcomp.pin('min').set(limits[n][0])
wcomp.pin('max').set(limits[n][1])
numFans = int(c.find('FDM', 'NUM_FANS'))
numExtruders = int(c.find('FDM', 'NUM_EXTRUDERS'))
numLights = int(c.find('FDM', 'NUM_LIGHTS'))
ve.velocity_extrusion(extruders=numExtruders, thread='servo-thread')
# Fans
for i in range(0, numFans):
base.setup_fan('f%i' % i, thread='servo-thread')
def __init__(self, temp_hal, pwm_pin, on_pin,
enable_sig, estop_reset_sig,
config_name, comp_name, hal_name, motion_pin_number):
self.temp_hal = temp_hal
comp = hal.RemoteComponent(comp_name, timer=100)
comp_limit_min_pin = comp.newpin('temp.limit.min', hal.HAL_FLOAT, hal.HAL_IN)
comp_limit_max_pin = comp.newpin('temp.limit.max', hal.HAL_FLOAT, hal.HAL_IN)
comp_standby_pin = comp.newpin('temp.standby', hal.HAL_FLOAT, hal.HAL_IN)
comp_meas_pin = comp.newpin('temp.meas', hal.HAL_FLOAT, hal.HAL_IN)
comp_set_pin = comp.newpin('temp.set', hal.HAL_FLOAT, hal.HAL_IO)
comp_in_range_pin = comp.newpin('temp.in-range', hal.HAL_BIT, hal.HAL_IN)
comp_active_pin = comp.newpin('active', hal.HAL_BIT, hal.HAL_IN)
comp_error_pin = comp.newpin('error', hal.HAL_BIT, hal.HAL_IN)
comp.ready()
# -- General Signals --
meas_sig = hal.newsig('%s.meas' % hal_name, hal.HAL_FLOAT)
meas_sig.link(temp_hal.pin('value'))
meas_sig.link(comp_meas_pin)
meas_sig.link('motion.analog-in-%02d' % motion_pin_number)
set_sig = hal.newsig('%s.set' % hal_name, hal.HAL_FLOAT)
set_sig.link(comp_set_pin)
set_sig.link('motion.analog-out-io-%02d' % motion_pin_number)
limit_min_sig = hal.newsig('%s.limit.min' % hal_name, hal.HAL_FLOAT)
limit_min_sig.link(comp_limit_min_pin)
limit_min_sig.set(config.find(config_name, 'TEMP_LIMIT_MIN', 0.0))
limit_max_sig = hal.newsig('%s.limit.max' % hal_name, hal.HAL_FLOAT)
limit_max_sig.link(comp_limit_max_pin)
limit_max_sig.set(config.find(config_name, 'TEMP_LIMIT_MAX', 30.0))
standby_sig = hal.newsig('%s.standby' % hal_name, hal.HAL_FLOAT)
standby_sig.link(comp_standby_pin)
standby_sig.set(config.find(config_name, 'TEMP_STANDBY', 15.0))
on_sig = hal.newsig('%s.on' % hal_name, hal.HAL_BIT)
on_sig.link(on_pin)
on_sig.link(comp_active_pin)
error_sig = hal.newsig('%s.error' % hal_name, hal.HAL_BIT)
self.error_sig = error_sig
error_sig.link(comp_error_pin)
self.temp_watchdog_sig = hal.newsig('%s.watchdog' % hal_name, hal.HAL_BIT)
temp_hal.pin('watchdog').link(self.temp_watchdog_sig)
# -- Measurement --
# In-Range Signal
in_range_sig = hal.newsig('%s.in-range' % hal_name, hal.HAL_BIT)
in_range_sig.link(comp_in_range_pin)
in_range_sig.link('motion.digital-in-%02d' % motion_pin_number)
range_lb_sum = rtapi.newinst('sum2', '%s.range.lb.sum2' % hal_name)
hal.addf(range_lb_sum.name, SERVO_THREAD)
range_ub_sum = rtapi.newinst('sum2', '%s.range.ub.sum2' % hal_name)
hal.addf(range_ub_sum.name, SERVO_THREAD)
range_lb_sig = hal.newsig('%s.range.lb' % hal_name, hal.HAL_FLOAT)
range_ub_sig = hal.newsig('%s.range.ub' % hal_name, hal.HAL_FLOAT)
range_lb_sum.pin('in0').link(set_sig)
range_lb_sum.pin('in1').set(float(config.find(config_name, 'TEMP_RANGE_NEG_ERROR', -1.0)))
range_lb_sum.pin('out').link(range_lb_sig)
range_ub_sum.pin('in0').link(set_sig)
range_ub_sum.pin('in1').set(float(config.find(config_name, 'TEMP_RANGE_POS_ERROR', 1.0)))
range_ub_sum.pin('out').link(range_ub_sig)
range_wcomp = rtapi.newinst('wcomp', '%s.range.wcomp' % hal_name)
hal.addf(range_wcomp.name, SERVO_THREAD)
range_wcomp.pin('min').link(range_lb_sig)
range_wcomp.pin('max').link(range_ub_sig)
range_wcomp.pin('in').link(meas_sig)
range_wcomp.pin('out').link(in_range_sig)
# -- Output --
pwm_raw_sig = hal.newsig('%s.pwm_raw' % hal_name, hal.HAL_FLOAT)
pwm_sig = hal.newsig('%s.pwm' % hal_name, hal.HAL_FLOAT)
pwm_max = float(config.find(config_name, 'PWM_MAX', 1.0))
# PID
pid = rtapi.newinst('at_pid', '%s.pid' % hal_name)
hal.addf(pid.name + '.do-pid-calcs', SERVO_THREAD)
pid.pin('enable').link(enable_sig)
pid.pin('feedback').link(meas_sig)
pid.pin('command').link(set_sig)
pid.pin('output').link(pwm_raw_sig)
pid.pin('maxoutput').set(pwm_max)
pid.pin('Pgain').set(float(config.find(config_name, 'PID_PGAIN', 0)))
pid.pin('Igain').set(float(config.find(config_name, 'PID_IGAIN', 0)))
pid.pin('Dgain').set(float(config.find(config_name, 'PID_DGAIN', 0)))
pid.pin('maxerrorI').set(float(config.find(config_name, 'PID_MAXERRORI', 1.0)))
pid.pin('bias').set(float(config.find(config_name, 'PID_BIAS', 0.0)))
# PWM Limit (PID can output negative values)
pwm_limit = rtapi.newinst('limit1', '%s.limit1.pwm' % hal_name)
hal.addf(pwm_limit.name, SERVO_THREAD)
pwm_limit.pin('min').set(0.0)
pwm_limit.pin('max').set(pwm_max)
pwm_limit.pin('in').link(pwm_raw_sig)
pwm_limit.pin('out').link(pwm_sig)
pwm_pin.link(pwm_sig)
# -- Safety Check --
check_limit_ok_sig = hal.newsig('%s.check.limit-ok' % hal_name, hal.HAL_BIT)
limit_wcomp = rtapi.newinst('wcomp', '%s.check.limit.wcomp' % hal_name)
hal.addf(limit_wcomp.name, SERVO_THREAD)
limit_wcomp.pin('min').link(limit_min_sig)
limit_wcomp.pin('max').link(limit_max_sig)
limit_wcomp.pin('in').link(meas_sig)
limit_wcomp.pin('out').link(check_limit_ok_sig)
out_range_sig = hal.newsig('%s.out-range' % hal_name, hal.HAL_BIT)
out_range_not = rtapi.newinst('not', '%s.out-range.not' % hal_name)
hal.addf(out_range_not.name, SERVO_THREAD)
out_range_not.pin('in').link(in_range_sig)
out_range_not.pin('out').link(out_range_sig)
thermistor_check_enable_sig = hal.newsig('%s.check.therm.enable' % hal_name, hal.HAL_BIT)
thermistor_check_enable_and = rtapi.newinst('and2', '%s.check.therm.enable.and2' % hal_name)
hal.addf(thermistor_check_enable_and.name, SERVO_THREAD)
thermistor_check_enable_and.pin('in0').link(enable_sig)
thermistor_check_enable_and.pin('in1').link(out_range_sig)
thermistor_check_enable_and.pin('out').link(thermistor_check_enable_sig)
check_thermistor_ok_sig = hal.newsig('%s.check.therm-ok' % hal_name, hal.HAL_BIT)
thermistor_check = rtapi.newinst('thermistor_check', '%s.check.therm-check' % hal_name)
hal.addf(thermistor_check.name, SERVO_THREAD)
thermistor_check.pin('enable').link(thermistor_check_enable_sig)
thermistor_check.pin('pid').link(pwm_sig)
thermistor_check.pin('temp').link(meas_sig)
thermistor_check.pin('min-pid').set(float(config.find(config_name, 'CHECK_MIN_PID', 0.25)))
thermistor_check.pin('min-temp').set(float(config.find(config_name, 'CHECK_MIN_TEMP', 2.0)))
thermistor_check.pin('wait').set(float(config.find(config_name, 'CHECK_WAIT', 30.0)))
thermistor_check.pin('no-error').link(check_thermistor_ok_sig)
check_all_ok_sig = hal.newsig('%s.check.all-ok' % hal_name, hal.HAL_BIT)
check_all_ok = rtapi.newinst('and2', '%s.check.all.and2' % hal_name)
hal.addf(check_all_ok.name, SERVO_THREAD)
check_all_ok.pin('in0').link(check_limit_ok_sig)
check_all_ok.pin('in1').link(check_thermistor_ok_sig)
check_all_ok.pin('out').link(check_all_ok_sig)
check_error_sig = hal.newsig('%s.check.error' % hal_name, hal.HAL_BIT)
check_error = rtapi.newinst('not', '%s.check.error.not' % hal_name)
hal.addf(check_error.name, SERVO_THREAD)
check_error.pin('in').link(check_all_ok_sig)
check_error.pin('out').link(check_error_sig)
error_latch = rtapi.newinst('flipflop', '%s.check.error.flipflop' % hal_name)
hal.addf(error_latch.name, SERVO_THREAD)
error_latch.pin('set').link(check_error_sig)
error_latch.pin('reset').link(estop_reset_sig)
error_latch.pin('out').link(error_sig)
def setup_joints():
"""
Create joints commands and feedbacks signal for the system
"""
commands = [None] * AXIS_TOTAL
feedbacks = [None] * AXIS_TOTAL
for i in xrange(AXIS_TOTAL):
commands[i] = hal.newsig('machine.joint.%d.command' % i, hal.HAL_FLOAT)
feedbacks[i] = hal.newsig('machine.joint.%d.feedback' % i, hal.HAL_FLOAT)
core_xy = config.find('FDM','CORE_XY')
if core_xy is not None and int(core_xy) > 0:
sum_cmd_a = rtapi.newinst('sum2', 'corexy.sum2.cmd.a')
sum_fb_x = rtapi.newinst('sum2', 'corexy.sum2.fb.x')
sum_cmd_b = rtapi.newinst('sum2', 'corexy.sum2.cmd.b')
sum_fb_y = rtapi.newinst('sum2', 'corexy.sum2.fb.y')
hal.addf(sum_cmd_a.name, SERVO_THREAD)
hal.addf(sum_cmd_b.name, SERVO_THREAD)
hal.addf(sum_fb_x.name, SERVO_THREAD)
hal.addf(sum_fb_y.name, SERVO_THREAD)
sum_cmd_a.pin('gain0').set(1)
sum_cmd_a.pin('gain1').set(1)
sum_cmd_b.pin('gain0').set(1)
sum_cmd_b.pin('gain1').set(-1)
sum_fb_x.pin('gain0').set(0.5)
sum_fb_x.pin('gain1').set(0.5)
sum_fb_y.pin('gain0').set(0.5)
sum_fb_y.pin('gain1').set(-0.5)
corex_cmd = hal.newsig('machine.joint.corex.command', hal.HAL_FLOAT)
corey_cmd = hal.newsig('machine.joint.corey.command', hal.HAL_FLOAT)
corex_cmd.link('axis.0.motor-pos-cmd')
corey_cmd.link('axis.1.motor-pos-cmd')
sum_cmd_a.pin('in0').link(corex_cmd)
sum_cmd_a.pin('in1').link(corey_cmd)
sum_cmd_b.pin('in0').link(corex_cmd)
sum_cmd_b.pin('in1').link(corey_cmd)
sum_cmd_a.pin('out').link(commands[0])
sum_cmd_b.pin('out').link(commands[1])
sum_fb_x.pin('in0').link(feedbacks[0])
sum_fb_x.pin('in1').link(feedbacks[1])
sum_fb_y.pin('in0').link(feedbacks[0])
sum_fb_y.pin('in1').link(feedbacks[1])
sum_fb_x.pin('out').link('axis.0.motor-pos-fb')
sum_fb_y.pin('out').link('axis.1.motor-pos-fb')
else:
commands[0].link('axis.0.motor-pos-cmd')
feedbacks[0].link('axis.0.motor-pos-fb')
commands[1].link('axis.1.motor-pos-cmd')
feedbacks[1].link('axis.1.motor-pos-fb')
for i in xrange(AXIS_TOTAL):
if i >= 2:
commands[i].link('axis.%d.motor-pos-cmd' % i)
feedbacks[i].link('axis.%d.motor-pos-fb' % i)
return (commands, feedbacks)
def setup_extruders(replicape, extruder_sel_sig):
motion_vel_sig = hal.newsig('ve-motion-vel', hal.HAL_FLOAT)
motion_vel_sig.link('motion.current-vel')
extruders = [None] * EXTRUDER_TOTAL
for i in xrange(EXTRUDER_TOTAL):
extruders[i] = Extruder(i, motion_vel_sig, config)
setup_extruder(replicape, extruders[i], i)
shared_signals = [
# Shared Signal Type Attribute fdm-ve-jog fdm-ve-params analog-io
('enable', hal.HAL_BIT, 'enable_sig', None, None, None),
('cross-section', hal.HAL_FLOAT, 'cross_section_sig', None, None, 41),
('jog-vel', hal.HAL_FLOAT, 'jog_vel_sig', 'velocity', None, None),
('jog-direction', hal.HAL_BIT, 'jog_direction_sig', 'direction', None, None),
('jog-distance', hal.HAL_FLOAT, 'jog_distance_sig', 'distance', None, None),
('jog-trigger', hal.HAL_BIT, 'jog_trigger_sig', 'trigger', None, None),
('jog-continuous',hal.HAL_BIT, 'jog_continuous_sig','continuous', None, None),
('jog-dtg', hal.HAL_FLOAT, 'jog_dtg_sig', 'dtg', None, None),
('max-jog-vel', hal.HAL_FLOAT, 'max_jog_vel_sig', 'max-velocity',None, None),
('filament-dia', hal.HAL_FLOAT, 'filament_dia_sig', None, 'filament-dia', 44),
('extrude-scale', hal.HAL_FLOAT, 'extrude_scale_sig', None, 'extrude-scale', None),
('accel-gain', hal.HAL_FLOAT, 'accel_gain_sig', None, 'accel-adj-gain', None),
('retract-vel', hal.HAL_FLOAT, 'retract_vel_sig', None, 'retract-vel', None),
('retract-len', hal.HAL_FLOAT, 'retract_len_sig', None, 'retract-len', None),
('extrude-vel', hal.HAL_FLOAT, 'extrude_vel_sig', None, None, None),
('retracting', hal.HAL_BIT , 'retracting_sig', None, None, None),
]
comp_jog = hal.RemoteComponent('fdm-ve-jog', timer=100)
comp_params = hal.RemoteComponent('fdm-ve-params', timer=100)
for (signal_name, signal_type, attr_name, comp_jog_name, comp_params_name, analog_io_pin) in shared_signals:
signal = hal.newsig('ve.%s' % signal_name, signal_type)
comp_dir_type = hal.HAL_IO
if signal_name == 'jog-dtg' or signal_name == 'max-jog-vel' or signal_name == 'extrude-vel':
mux_type = 'muxn'
comp_dir_type = hal.HAL_IN
elif signal_name == 'retracting':
mux_type = 'muxn_bit'
comp_dir_type = hal.HAL_IN
else:
mux_type = 'io_muxn' if signal_type == hal.HAL_FLOAT else 'io_muxn_bit'
mux = rtapi.newinst(
mux_type,
'ex.mux%d.%s' % (EXTRUDER_TOTAL, signal_name),
pincount=EXTRUDER_TOTAL)
hal.addf(mux.name, SERVO_THREAD)
mux.pin('out').link(signal)
mux.pin('sel').link(extruder_sel_sig)
for j in xrange(EXTRUDER_TOTAL):
mux.pin('in%i' % j).link(getattr(extruders[j], attr_name))
if comp_jog_name is not None:
pin = comp_jog.newpin(comp_jog_name, signal_type, comp_dir_type)
pin.link(signal)
if comp_params_name is not None:
pin = comp_params.newpin(comp_params_name, signal_type, comp_dir_type)
pin.link(signal)
if analog_io_pin is not None:
signal.link('motion.analog-out-io-%d' % analog_io_pin)
if signal_name == 'enable':
signal.link('motion.digital-out-io-01')
hal.Signal('ve.retracting').link('motion.feed-hold')
extruder_sel_sig.link('iocontrol.0.tool-prep-number')
comp_jog.newpin('extruder-sel', hal.HAL_S32, hal.HAL_IN).link(extruder_sel_sig)
comp_jog.newpin('extruder-count', hal.HAL_U32, hal.HAL_IN).set(EXTRUDER_TOTAL)
comp_jog.ready()
comp_params.ready()
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_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)
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 __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 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)
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 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'))
