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_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_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 instantiate_components(arguments):
demosetup = arguments.demo
testsetup = arguments.testsetup
configfile = arguments.config
# name of the servothread
st = 'st'
print('instantiating components')
if demosetup == False:
setup_lcec(configfile)
# write lcec-read-all first
hal.addf('lcec.read-all', st)
# load other stuff
print(configfile)
if configfile != './ek1100el1008el2008.xml':
setup_joints(st)
connect_lcec(st, testsetup)
# do some final writing of functions to the thread
# write lcec-write-all last
hal.addf('lcec.write-all', st)
else:
# create a demo joint for when EtherCAT not available
setup_joints(st)
connect_sim(st)
# start threads for executing functions
hal.start_threads()
# create jplan joints and wire them to the plumbing
if configfile != './ek1100el1008el2008.xml':
finish_jplan_plumbing()
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_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 hardware_read():
#hal.addf('hpg.capture-position', 'servo-thread')
print(dir(hal))
print(hal.components())
print(hal.members())
hal.addf('hpg.funct', 'servo-thread')
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 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_loadrt_or2():
global rt
rt.newinst("or2", "or2.0")
rt.newthread("servo-thread", 1000000, fp=True)
hal.addf("or2.0", "servo-thread")
hal.start_threads()
time.sleep(0.2)
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 test_loadrt_or2():
global rt
rt.newinst("or2", "or2.0")
rt.newthread("servo-thread", 1000000, fp=True)
hal.addf("or2.0", "servo-thread")
hal.start_threads()
time.sleep(0.2)
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_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_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_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 __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 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 setUp(self):
self.cfg = ConfigParser()
self.cfg.read(os.getenv("MACHINEKIT_INI"))
self.uuid = self.cfg.get("MACHINEKIT", "MKUUID")
self.rt = rtapi.RTAPIcommand(uuid=self.uuid)
self.rt.newinst("or2", "or2.0")
self.rt.newthread("servo-thread", 1000000, fp=True)
hal.addf("or2.0", "servo-thread")
hal.start_threads()
def setUp(self):
self.cfg = ConfigParser.ConfigParser()
self.cfg.read(os.getenv("MACHINEKIT_INI"))
self.uuid = self.cfg.get("MACHINEKIT", "MKUUID")
self.rt = rtapi.RTAPIcommand(uuid=self.uuid)
self.rt.newinst("or2", "or2.0")
self.rt.newthread("servo-thread",1000000,fp=True)
hal.addf("or2.0","servo-thread")
hal.start_threads()
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 instantiate_components(arguments):
configfile = arguments.config
# name of the servothread
st = 'st'
print('instantiating components')
setup_lcec(configfile)
# write lcec-read-all first
hal.addf('lcec.0.read', st)
# write lcec-write-all last
hal.addf('lcec.0.write', st)
# start threads for executing functions
hal.start_threads()
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_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_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 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_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_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_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 __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 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 _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_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_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 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 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 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_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 test_runthread():
cpe = hal.Pin("charge-pump.enable")
cpe.set(0)
rt.newthread("fast",1000000, fp=True)
rt.newthread("slow",100000000, fp=True)
hal.addf("ringread","fast")
hal.addf("ringwrite","slow")
hal.addf("charge-pump","slow")
hal.start_threads()
cpe.set(1) # enable charge_pump
time.sleep(3) # let rt thread write a bit to ring
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 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 __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 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 hardware_read():
hal.addf('hpg.capture-position', 'servo-thread')
hal.addf('bb_gpio.read', 'servo-thread')
def test_loadrt_ringwrite():
rt.loadrt("ringwrite","ring=ring1")
rt.newthread("servo-thread",1000000,fp=True)
hal.addf("ringwrite","servo-thread")
hal.start_threads()
time.sleep(1) # let rt thread write a bit to ring
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()
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 gantry_write(gantryAxis, thread):
hal.addf('gantry.%i.write' % gantryAxis, thread)
def gantry_read(gantryAxis, thread):
hal.addf('gantry.%i.read' % gantryAxis, thread)
from fdm.config import storage
from fdm.config import motion
import cramps as hardware
# initialize the RTAPI command client
rt.init_RTAPI()
# loads the ini file passed by linuxcnc
c.load_ini(os.environ['INI_FILE_NAME'])
motion.setup_motion()
hardware.init_hardware()
storage.init_storage('storage.ini')
# reading functions
hardware.hardware_read()
hal.addf('motion-command-handler', 'servo-thread')
hal.addf('motion-controller', 'servo-thread')
numFans = c.find('FDM', 'NUM_FANS')
numExtruders = c.find('FDM', 'NUM_EXTRUDERS')
numLights = c.find('FDM', 'NUM_LIGHTS')
hasHbp = c.find('FDM', 'HAS_HBP')
# Axis-of-motion Specific Configs (not the GUI)
ve.velocity_extrusion(extruders=numExtruders, thread='servo-thread')
# X [0] Axis
base.setup_stepper(section='AXIS_0', axisIndex=0, stepgenIndex=0, thread='servo-thread')
# Y [1] Axis
base.setup_stepper(section='AXIS_1', axisIndex=1, stepgenIndex=1, thread='servo-thread')
# Z [2] Axis
base.setup_stepper(section='AXIS_2', axisIndex=2, stepgenIndex=2, thread='servo-thread')
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 hardware_write():
hal.addf('hpg.update', 'servo-thread')
hal.addf('bb_gpio.write', 'servo-thread')
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 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
hal.start_threads()
