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 insert_component(sourcepin, target1, source1):
source = hal.Pin(sourcepin)
if source.linked:
sig = source.signal
sigtype = sig.type
sigreaders = sig.readers
targets_found = 0
targets = []
for pinname in hal.pins():
temppin = hal.Pin(pinname)
if (temppin.signal == sig) and (temppin.name != source.name):
targets_found += 1
targets.append(temppin.name)
if targets_found == sigreaders:
break
# now we've found the signal, delete it
# link from source to new comp
# link with signal name from new comp to original targets
oldname = sig.name
newname = 'new_{}'.format(sig.name)
res = hal.delsig(sig.name)
sig1 = hal.Signal(oldname, sigtype)
sig2 = hal.Signal(newname, sigtype)
sig1.link(source.name)
if target1 != '':
newpin_in = hal.Pin(target1)
sig1.link(target1)
newpin_out = hal.Pin(source1)
sig2.link(source1)
for t in targets:
sig2.link(t)
def connect_jplan(joint_nr, joints_config):
# enable jplan with high
hal.Pin('joint%i_jplan.0.enable' % joint_nr).set(1)
hal.Pin('joint%i_jplan.0.max-acc' % joint_nr).set(1000)
hal.Pin('joint%i_jplan.0.max-vel' % joint_nr).set(1000)
# get current value of joint*_offset.fb-out
# this is the actual position
act_pos = hal.Pin('joint%i_offset.fb-out' % joint_nr).get()
# now set this value to jplan pos cmd
hal.Pin('joint%i_jplan.0.pos-cmd' % joint_nr).set(act_pos)
# if active, wait on active
prev_active_pin = hal.Pin('joint%i_jplan.0.active' % joint_nr).get()
falling_edge = False
first_time = True
while (not falling_edge):
time.sleep(0.1)
active_pin = hal.Pin('joint%i_jplan.0.active' % joint_nr).get()
if first_time == True:
prev_active_pin = 1
if (prev_active_pin == 1) and (active_pin == 0):
falling_edge = True
prev_active_pin = active_pin
# success, pos-cmd and actual position are equal
max_vel = joints_config.get('joint_%s' % joint_nr, 'max_vel')
max_acc = joints_config.get('joint_%s' % joint_nr, 'max_acc')
hal.Pin('joint%i_jplan.0.max-acc' % joint_nr).set(float(max_acc))
hal.Pin('joint%i_jplan.0.max-vel' % joint_nr).set(float(max_vel))
# connect jplan with offset
hal.Pin('joint%i_jplan.0.curr-pos' % joint_nr).link('joint%i_offset.in' %
joint_nr)
def setup_fan_io(name):
if name[0] == 'f':
index = 12 + int(name[1])
else:
raise RuntimeError('fan io name not known')
hal.Pin('motion.analog-out-io-%02i' % index).link('%s-set' % name)
hal.Pin('motion.analog-in-%02i' % index).link('%s-set' % name)
def setup_ve_storage(extruders):
for n in range(0, extruders):
hal.Pin('storage.e%i.retract-vel' % n).link('ve-retract-vel-e%i' % n)
hal.Pin('storage.e%i.retract-len' % n).link('ve-retract-len-e%i' % n)
hal.Pin('storage.e%i.filament-dia' % n).link('ve-filament-dia-e%i' % n)
hal.Pin('storage.e%i.jog-velocity' % n).link('ve-jog-velocity-e%i' % n)
hal.Pin('storage.e%i.extrude-scale' % n).link('ve-extrude-scale-e%i' %
n)
def connect_hw_interface():
for nr in range(1, NUM_JOINTS + 1):
hal.Pin('hal_hw_interface.joint_{}.pos-cmd'.format(nr)).link(
'joint-{}-cmd-pos'.format(nr))
hal.Pin('hal_hw_interface.joint_{}.pos-fb'.format(nr)).link(
'joint-{}-fb-out-pos'.format(nr))
hal.Pin('hal_hw_interface.reset').link('hw-reset')
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_light_io(name):
if name[0] == 'l':
index = 26 + int(name[1]) * 4
else:
raise RuntimeError('light io name not known')
for m, color in enumerate(('r', 'g', 'b', 'w')):
hal.Pin('motion.analog-out-io-%02i' % (index + m)) \
.link('%s-%s' % (name, color))
hal.Pin('motion.analog-in-%02i' % (index + m)) \
.link('%s-%s' % (name, color))
def connect_lcec_to_plumbing(joint_nr, slave_nr):
# in my dev setup the 6th joint is slave 3
#
# first connect the feedback from the encoder to the plumbing
# lcec.0.3.position-actual-value
# this will then show the actual position in radians, and
# take that once to set the position pin joint6_offset.in
flt_to_s32 = 'joint%i_cmd_s32.out' % joint_nr
s32_to_flt = 'joint%i_fb_flt.in' % joint_nr
lcec_cmd = 'lcec.0.%i.target-position' % slave_nr
lcec_fb = 'lcec.0.%i.position-actual-value' % slave_nr
hal.Pin(flt_to_s32).link(lcec_cmd)
hal.Pin(lcec_fb).link(s32_to_flt)
def setup_temperature_io(name):
index = 0
if name == 'hbp':
index = 0
elif name == 'hbc':
index = 1
elif name[0] == 'e':
index = 2 + int(name[1])
else:
raise RuntimeError('temperature io name not known')
hal.Pin('motion.analog-out-io-%02i' % index).link('%s-temp-set' % name)
hal.Pin('motion.analog-in-%02i' % index).link('%s-temp-meas' % name)
hal.Pin('motion.digital-in-%02i' % index).link('%s-temp-in-range' % name)
def __init__(self, name):
self.name = name
self.machine = Machine(model=self,
states=MiniMachine.states,
transitions=MiniMachine.transitions,
initial='init')
self.rt = rt
#self.rt.init_RTAPI()
self.switch_takeout = hal.Signal('input_switch_takeout')
self.switch_cart = hal.Signal('input_switch_cart')
self.go_jerry = hal.Signal('go_jerry')
self.jplan_x_active = hal.Pin('jplan_x.0.active')
self.jplan_y_active = hal.Pin('jplan_y.0.active')
self.jplan_z_active = hal.Pin('jplan_z.0.active')
def setup_motion(kinematics='trivkins'):
rt.loadrt(kinematics)
if kinematics is 'lineardeltakins':
hal.Pin('lineardeltakins.L').set(c.find('MACHINE', 'CF_ROD'))
hal.Pin('lineardeltakins.R').set(c.find('MACHINE', 'DELTA_R'))
rt.loadrt('tp')
# 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=51,
num_dio=21,
kins=kinematics)
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 usrcomp_watchdog(comps,
enableSignal,
thread,
okSignal=None,
errorSignal=None):
count = len(comps)
watchdog = rt.loadrt('watchdog', num_inputs=count)
hal.addf('watchdog.set-timeouts', thread)
hal.addf('watchdog.process', thread)
for n, comp in enumerate(comps):
compname = comp[0]
comptime = comp[1]
sigIn = hal.newsig('%s-watchdog' % compname, hal.HAL_BIT)
hal.Pin('%s.watchdog' % compname).link(sigIn)
watchdog.pin('input-%i' % n).link(sigIn)
watchdog.pin('timeout-%i' % n).set(comptime)
watchdog.pin('enable-in').link(enableSignal)
if not okSignal:
okSignal = hal.newsig('watchdog-ok', hal.HAL_BIT)
watchdog.pin('ok-out').link(okSignal)
if errorSignal:
notComp = rt.newinst('not', 'not.watchdog-error')
hal.addf(notComp.name, thread)
notComp.pin('in').link(okSignal)
notComp.pin('out').link(errorSignal)
def _setup_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 create_joint_plumbing(st, joint_nr):
# offset
rt.newinst('offsetv2', 'joint%i_offset' % joint_nr)
hal.addf('joint%i_offset.update-output' % joint_nr, "%s" % st)
hal.addf('joint%i_offset.update-feedback' % joint_nr, st)
# scaling
rt.newinst('mult2v2', 'joint%i_scale_pos_cmd' % joint_nr)
hal.addf('joint%i_scale_pos_cmd.funct' % joint_nr, st)
rt.newinst('div2v2', 'joint%i_scale_pos_fb' % joint_nr)
hal.addf('joint%i_scale_pos_fb.funct' % joint_nr, st)
jointscale = hal.newsig('joint%i_scale' % joint_nr, hal.HAL_FLOAT)
hal.Pin('joint%i_scale_pos_fb.deadband' % joint_nr).set(0.00000001)
# converting components
rt.newinst('conv_float_s32', 'joint%i_cmd_s32' % joint_nr)
rt.newinst('conv_s32_float', 'joint%i_fb_flt' % joint_nr)
hal.addf('joint%i_cmd_s32.funct' % joint_nr, st)
hal.addf('joint%i_fb_flt.funct' % joint_nr, st)
# now connect the components, scale to mult and div
jointscale.link('joint%i_scale_pos_cmd.in1' % joint_nr)
jointscale.link('joint%i_scale_pos_fb.in1' % joint_nr)
# offset joint (ROS) position to scale mult
hal.Pin('joint%i_offset.out' % joint_nr).link('joint%i_scale_pos_cmd.in0' %
joint_nr)
# (de)offset scale div to joint (ROS) position
hal.Pin('joint%i_scale_pos_fb.out' % joint_nr).link(
'joint%i_offset.fb-in' % joint_nr)
# connect converting components
hal.Pin('joint%i_scale_pos_cmd.out' % joint_nr).link('joint%i_cmd_s32.in' %
joint_nr)
hal.Pin('joint%i_fb_flt.out' % joint_nr).link('joint%i_scale_pos_fb.in0' %
joint_nr)
# create jplan component but do nothing for now
# because
# - the current position must first be retrieved from the drive
# - then the joint*_jplan.0.pos-cmd must be set
# - acc and vel must be high
# - wait on joint*_jplan.0.active falling edge (target reached)
# - then set correct values for acc and vel values
# - then connect with a signal
rt.newinst('jplan', 'joint%i_jplan' % joint_nr)
hal.addf('joint%i_jplan.update' % joint_nr, st)
def setup_tclab():
#import hal_tclab
# from hal_tclab import prepare
# prepare()
hal.loadusr("hal_tclab",
name="hal_tclab",
wait_name="hal_tclab",
wait_timeout=30)
hal.Pin("hal_tclab.enable").link(hal.Signal("ALLenable"))
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 test_pin_properties(self, setUp):
in0 = hal.Pin("or2.0.in0")
assert in0.type == hal.HAL_BIT
assert in0.dir == hal.HAL_IN
assert in0.linked == False
in1 = hal.Pin("or2.0.in1")
assert in1.type == hal.HAL_BIT
assert in1.dir == hal.HAL_IN
assert in1.linked == False
out = hal.Pin("or2.0.out")
assert out.type == hal.HAL_BIT
assert out.dir == hal.HAL_OUT
assert out.linked == False
# unlinked default values
assert in0.get() == False
assert in1.get() == False
# output value at default value
assert out.get() == False
in0.set(True)
time.sleep(0.1) # we urgently need a 1-shot thread function!
assert out.get() == True
in0.set(False)
time.sleep(0.1)
assert out.get() == False
in1.set(True)
time.sleep(0.1)
assert out.get() == True
in1.set(False)
time.sleep(0.1)
assert out.get() == False
in1.set(True)
in0.set(True)
time.sleep(0.1)
assert out.get() == True
def test_runthread(self):
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 test_pin_attributes():
n = c.pins() # pin names of this comp
assert len(n) == 1
# access properties through wrapper:
global p
p = hal.Pin(n[0])
assert p.name == fqpname
assert p.type == hal.HAL_S32
assert p.dir == hal.HAL_OUT
assert p.eps == epsindex
assert fnear(p.epsilon, hal.epsilon[epsindex])
assert p.handle > 0
assert p.linked == False
def setup_linearDeltaKins(c):
# c.load_ini(os.environ['INI_FILE_NAME'])
deltaCfRod = c.find('MACHINE', 'CF_ROD')
deltaRadius = c.find('MACHINE', 'DELTA_R')
deltaJoin0Offset = c.find('MACHINE', 'JOINT_0_OFFSET', 0)
deltaJoin1Offset = c.find('MACHINE', 'JOINT_1_OFFSET', 0)
deltaJoin2Offset = c.find('MACHINE', 'JOINT_2_OFFSET', 0)
deltaJoin1AngleOffset = c.find('MACHINE', 'JOINT_1_ANGLE_OFFSET', 0)
deltaJoin2AngleOffset = c.find('MACHINE', 'JOINT_2_ANGLE_OFFSET', 0)
deltaJoin1RadiusOffset = c.find('MACHINE', 'JOINT_1_RADIUS_OFFSET', 0)
deltaJoin2RadiusOffset = c.find('MACHINE', 'JOINT_2_RADIUS_OFFSET', 0)
c.load_ini(os.environ['INI_FILE_NAME'])
hal.Pin('lineardeltakins.L').set(deltaCfRod)
hal.Pin('lineardeltakins.R').set(deltaRadius)
hal.Pin('lineardeltakins.J0off').set(deltaJoin0Offset)
hal.Pin('lineardeltakins.J1off').set(deltaJoin1Offset)
hal.Pin('lineardeltakins.J2off').set(deltaJoin2Offset)
hal.Pin('lineardeltakins.A1off').set(deltaJoin1AngleOffset)
hal.Pin('lineardeltakins.A2off').set(deltaJoin2AngleOffset)
hal.Pin('lineardeltakins.R1off').set(deltaJoin1RadiusOffset)
hal.Pin('lineardeltakins.R2off').set(deltaJoin2RadiusOffset)
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 test_params(self):
global rt
# verify module params, compiled-in defaults
rt.loadrt("icomp")
answer_value = hal.Pin("icomp.answer_value")
assert answer_value.get() == 42
text_len = hal.Pin("icomp.text_len")
assert text_len.get() == 6
rt.unloadrt("icomp")
# verify module params, explicitly passed params
rt.loadrt("icomp", "answer=1234567", 'text="foobarbaz"')
answer_value = hal.Pin("icomp.answer_value")
assert answer_value.get() == 1234567
text_len = hal.Pin("icomp.text_len")
assert text_len.get() == 9
rt.unloadrt("icomp")
# verify instance default parameters
# as compiled in
rt.loadrt("icomp")
rt.newinst("icomp", "inst")
repeat_value = hal.Pin("inst.repeat_value")
assert repeat_value.get() == 1
prefix_len = hal.Pin("inst.prefix_len")
assert prefix_len.get() == 3
# verify explicit instance params
rt.newinst("icomp", "explicit", "repeat=314", 'prefix="foobarbaz"')
repeat_value = hal.Pin("explicit.repeat_value")
assert repeat_value.get() == 314
prefix_len = hal.Pin("explicit.prefix_len")
assert prefix_len.get() == 9
def setup_hardware(thread):
# PWM
#hal.Pin('hm2_repl.0.pwmgen.00.pwm_frequency').set(20000) # 100Hz
# HBP
# hal.Pin('hm2_repl.0.pwmgen.00.enable').set(True)
os.system('halcmd setp hm2_repl.0.pwmgen.00.enable true')
hal.Pin('hm2_repl.0.pwmgen.00.value').link('hbp-temp-pwm')
# configure extruders
for n in range(0, 1):
# hal.Pin('hm2_repl.0.pwmgen.%02i.enable' % (n + 1)).set(True)
os.system('halcmd setp hm2_repl.0.pwmgen.%02i.enable true' % (n + 1))
hal.Pin('hm2_repl.0.pwmgen.%02i.value' % (n + 1)).link('e%i-temp-pwm' %
n)
# configure fans
for n in range(0, 1):
hal.Pin('hm2_repl.0.pwmgen.%02i.enable' % (n + 4)).link(
'f%i-pwm-enable' % n)
hal.Pin('hm2_repl.0.pwmgen.%02i.value' % (n + 4)).link('f%i-pwm' % n)
hal.Signal('f%i-pwm-enable' % n).set(True)
# configure exps
for n in range(0, 1):
hal.Pin('hm2_repl.0.pwmgen.%02i.enable' % (n + 5)).link(
'exp%i-pwm-enable' % n)
hal.Pin('hm2_repl.0.pwmgen.%02i.value' % (n + 5)).link('exp%i-pwm' % n)
hal.Signal('exp%i-pwm' % n).set(1.0)
# configure leds
# none
# GPIO
# Adjust as needed for your switch polarity
hal.Pin('hm2_repl.0.gpio.025.in_not').link('limit-0-max') # X
hal.Pin('hm2_repl.0.gpio.027.in_not').link('limit-1-max') # Y
hal.Pin('hm2_repl.0.gpio.028.in_not').link('limit-2-0-home') # ZR
hal.Pin('hm2_repl.0.gpio.029.in_not').link('limit-2-1-home') # ZL
hal.Pin('hm2_repl.0.gpio.024.in').link('limit-0-home') # X
hal.Pin('hm2_repl.0.gpio.026.in').link('limit-1-home') # Y
# probe ...
# hal.Pin('hm2_repl.0.capsense.00.trigged').link('probe-signal') #
# ADC
# hal.Pin('temp.ch-00.value').link('hbp-temp-meas')
# hal.Pin('temp.ch-01.value').link('e0-temp-meas')
# hal.Pin('temp.ch-02.value').link('e1-temp-meas')
# hal.Pin('temp.ch-03.value').link('e2-temp-meas')
# machine power
os.system('halcmd setp hm2_repl.0.gpio.033.is_output true')
hal.Pin('hm2_repl.0.gpio.033.out').link('emcmot-0-enable')
# Monitor estop input from hardware
hal.Pin('hm2_repl.0.gpio.034.in_not').link('estop-in')
# drive estop-sw
os.system('halcmd setp hm2_repl.0.gpio.035.is_output true')
os.system('halcmd setp hm2_repl.0.gpio.035.invert_output true')
hal.Pin('hm2_repl.0.gpio.035.out').link('estop-out')
# Tie machine power signal to the Parport Cape LED
# Feel free to tie any other signal you like to the LED
os.system('halcmd setp hm2_repl.0.gpio.031.is_output true')
hal.Pin('hm2_repl.0.gpio.031.out').link('emcmot-0-enable')
# link emcmot.xx.enable to stepper driver enable signals
os.system('halcmd setp hm2_repl.0.gpio.032.is_output true')
os.system('halcmd setp hm2_repl.0.gpio.032.invert_output true')
hal.Pin('hm2_repl.0.gpio.032.out').link('emcmot-0-enable')
#-*- coding: utf-8 -*-
import os
import telegram
import linuxcnc
from machinekit import hal
import logging
from telegram import (ReplyKeyboardMarkup, ReplyKeyboardRemove)
from telegram.ext import (Updater, CommandHandler, MessageHandler, Filters, ConversationHandler)
from subprocess import check_output
logging.basicConfig(format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',level=logging.INFO)
logger = logging.getLogger(__name__)
err = hal.Pin("edge.out")
CHOOSING, TYPING_REPLY, TYPING_CHOICE = range(3)
reply_keyboard = [['bianca']]
markup = ReplyKeyboardMarkup(reply_keyboard, one_time_keyboard=False)
try:
import cv2
cv2_cap = cv2.VideoCapture(0)
if not cv2_cap.isOpened():
cv2_cap = cv2.VideoCapture(1)
if not cv2_cap.isOpened():
print ("Camera not found!")
cv2_cap = None
except ImportError:
from machinekit import hal
try:
launcher.check_installation()
launcher.cleanup_session() # kill any running Machinekit instances
launcher.start_realtime() # start Machinekit realtime environment
while 'ul_iface_component.joint_6.pos-cmd' not in hal.pins:
time.sleep(0.5)
print('found HAL pin, connecting')
# create 6 signals between pos-cmd and pos-fb pins
# when machinekit_ros_control has started with
# parameter machinekit_interface: 0
for i in range(1, 7):
hal.Pin('ul_iface_component.joint_%s.pos-cmd' % i).link(
'ul_iface_component.joint_%s.pos-fb' % i)
launcher.register_exit_handler(
) # enable on ctrl-C, needs to executed after HAL files
while True:
launcher.check_processes()
time.sleep(1)
except subprocess.CalledProcessError:
launcher.end_session()
sys.exit(1)
sys.exit(0)
def _setup_joints(self):
with open(JOINT_CONFIG_FILE, 'r') as f:
config = yaml.safe_load(f)
for i in range(1, NUM_JOINTS + 1):
c = config['joint_{}'.format(i)]
scale = c['gear_ratio'] * c['steps_per_rev'] / (2.0 * pi)
nr = 6 - i
# delay stepgen enable to allow brakes to release
timedelay = rt.newinst('timedelay', 'timedelay.stepgen-{}-enable'.format(i))
hal.addf(timedelay.name, self.thread.name)
timedelay.pin('in').link('brake-release-{}-out'.format(i))
timedelay.pin('out').link('stepgen-{}-enable'.format(i))
timedelay.pin('on-delay').set(BRAKE_RELEASE_DELAY_S)
timedelay.pin('off-delay').set(0.0)
# stepgen
stepgen = PinGroup('hm2_7i80.0.stepgen.{:02}'.format(nr))
stepgen.pin('step_type').set(0) # 0 = Step/Dir, 1 = Up/Down, 2 = Quadrature
stepgen.pin('control-type').set(0) # position mode
stepgen.pin('dirhold').set(c['dirhold_ns'])
stepgen.pin('dirsetup').set(c['dirsetup_ns'])
stepgen.pin('steplen').set(c['steplen_ns'])
stepgen.pin('stepspace').set(c['stepspace_ns'])
stepgen.pin('position-scale').set(scale)
stepgen.pin('maxvel').set(c['max_vel_rad_s'])
stepgen.pin('maxaccel').set(c['max_accel_rad_s2'])
stepgen.pin('enable').link('stepgen-{}-enable'.format(i))
# tristate.pin('out').link(stepgen.pin('position-cmd'))
stepgen.pin('position-cmd').link('joint-{}-cmd-out-pos'.format(i))
stepgen.pin('position-fb').link('joint-{}-cmd-fb-pos'.format(i))
# encoder
encoder = PinGroup('hm2_7i80.0.encoder.{:02}'.format(nr))
encoder.pin('index-enable').set(False)
encoder.pin('latch-enable').set(False)
encoder.pin('reset').set(False)
# Filter incoming:
# True: 15clk@50MHz = 30ns, False: 3clk@MHz = 6ns
encoder.pin('filter').set(True)
encoder.pin('scale').set(-scale)
encoder.pin('position').link('joint-{}-fb-in-pos'.format(i))
# encoder abs
encoder_abs = PinGroup('i620p-abs.{}'.format(i))
encoder_abs.pin('scale').set(scale)
encoder_abs.pin('abs-pos').link('joint-{}-abs-pos'.format(i))
hal.Signal('joint-{}-home-pos'.format(i)).set(c['home_pos'])
# setup limits
hal.Signal('joint-{}-limit-min'.format(i)).set(c['min_limit_rad'])
hal.Signal('joint-{}-limit-max'.format(i)).set(c['max_limit_rad'])
hal.Signal('joint-{}-ferror-max'.format(i)).set(c['max_ferror_rad'])
# set rs-485 tx enable pins
tx0_en = PinGroup('hm2_7i80.0.gpio.071')
tx0_en.pin('is_output').set(True)
tx0_en.pin('out').set(False)
tx3_en = PinGroup('hm2_7i80.0.gpio.048')
tx3_en.pin('is_output').set(True)
tx3_en.pin('out').set(False)
# enable abs_encoder only when powered-off
not_power_on = rt.newinst('not', 'not.power-on')
hal.addf(not_power_on.name, self.thread.name)
not_power_on.pin('in').link('power-on')
not_power_on.pin('out').link(hal.Pin('i620p-abs.enabled'))
def setup_hardware(thread):
# PWM
hal.Pin('hpg.pwmgen.00.pwm_period').set(10000000) # 100Hz
hal.Pin('hpg.pwmgen.00.out.00.pin').set(811)
hal.Pin('hpg.pwmgen.00.out.01.pin').set(915)
hal.Pin('hpg.pwmgen.00.out.02.pin').set(927)
hal.Pin('hpg.pwmgen.00.out.03.pin').set(921)
hal.Pin('hpg.pwmgen.00.out.04.pin').set(941)
hal.Pin('hpg.pwmgen.00.out.05.pin').set(922)
# HBP
hal.Pin('hpg.pwmgen.00.out.00.enable').set(True)
hal.Pin('hpg.pwmgen.00.out.00.value').link('hbp-temp-pwm')
# configure extruders
for n in range(0, 3):
hal.Pin('hpg.pwmgen.00.out.%02i.enable' % (n + 1)).set(True)
hal.Pin('hpg.pwmgen.00.out.%02i.value' % (n + 1)).link('e%i-temp-pwm' %
n)
# configure fans
for n in range(0, 1):
hal.Pin('hpg.pwmgen.00.out.%02i.enable' % (n + 4)).link(
'f%i-pwm-enable' % n)
hal.Pin('hpg.pwmgen.00.out.%02i.value' % (n + 4)).link('f%i-pwm' % n)
hal.Signal('f%i-pwm-enable' % n).set(True)
# configure hotend cooling fan
hal.Pin('hpg.pwmgen.00.out.05.enable').link('exp0-pwm-enable')
hal.Pin('hpg.pwmgen.00.out.05.value').set(1.0)
hal.Pin('hpg.pwmgen.00.out.05.value').link('exp0-pwm')
# configure leds
# none
# GPIO
hal.Pin('bb_gpio.p8.in-07').link('limit-0-home') # X
hal.Pin('bb_gpio.p8.in-08').link('limit-0-min') # X
hal.Pin('bb_gpio.p8.in-09').link('limit-1-home') # Y
hal.Pin('bb_gpio.p8.in-10').link('limit-1-min') # Y
hal.Pin('bb_gpio.p9.in-11').link('limit-2-home') # Z
hal.Pin('bb_gpio.p9.in-13').link('limit-2-min') # Z
# probe ...
# Adjust as needed for your switch polarity
hal.Pin('bb_gpio.p8.in-08.invert').set(False)
hal.Pin('bb_gpio.p8.in-07.invert').set(False)
hal.Pin('bb_gpio.p8.in-10.invert').set(False)
hal.Pin('bb_gpio.p8.in-09.invert').set(False)
hal.Pin('bb_gpio.p9.in-13.invert').set(False)
hal.Pin('bb_gpio.p9.in-11.invert').set(False)
# ADC
hal.Pin('temp.ch-04.value').link('hbp-temp-meas')
hal.Pin('temp.ch-05.value').link('e0-temp-meas')
hal.Pin('temp.ch-02.value').link('e1-temp-meas')
hal.Pin('temp.ch-03.value').link('e2-temp-meas')
# Stepper
hal.Pin('hpg.stepgen.00.steppin').set(813)
hal.Pin('hpg.stepgen.00.dirpin').set(812)
hal.Pin('hpg.stepgen.01.steppin').set(815)
hal.Pin('hpg.stepgen.01.dirpin').set(814)
hal.Pin('hpg.stepgen.02.steppin').set(819)
hal.Pin('hpg.stepgen.02.dirpin').set(818)
hal.Pin('hpg.stepgen.03.steppin').set(916)
hal.Pin('hpg.stepgen.03.dirpin').set(912)
hal.Pin('hpg.stepgen.04.steppin').set(917)
hal.Pin('hpg.stepgen.04.dirpin').set(918)
hal.Pin('hpg.stepgen.05.steppin').set(924)
hal.Pin('hpg.stepgen.05.dirpin').set(926)
# machine power
hal.Pin('bb_gpio.p9.out-23').link('emcmot-0-enable')
#hal.Pin('bb_gpio.p9.out-23.invert').set(True)
# Monitor estop input from hardware
hal.Pin('bb_gpio.p8.in-17').link('estop-in')
hal.Pin('bb_gpio.p8.in-17.invert').set(True)
# drive estop-sw
hal.Pin('bb_gpio.p8.out-26').link('estop-out')
hal.Pin('bb_gpio.p8.out-26.invert').set(True)
# Tie machine power signal to the Parport Cape LED
# Feel free to tie any other signal you like to the LED
hal.Pin('bb_gpio.p9.out-25').link('emcmot-0-enable')
# hal.Pin('bb_gpio.p9.out-25.invert').set(True)
# link emcmot.xx.enable to stepper driver enable signals
hal.Pin('bb_gpio.p9.out-14').link('emcmot-0-enable')
hal.Pin('bb_gpio.p9.out-14.invert').set(True)
def setup_gripper_pins():
hal.Pin('hal_io.digital_out_1').link('gripper-open-close')
hal.Pin('hal_io.digital_in_1').link('gripper-opened')
