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 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 _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 connect_tool_changer():
p = hal.Signal('tool-prepared', hal.HAL_BIT)
p.link('iocontrol.0.tool-prepare')
p.link('iocontrol.0.tool-prepared')
c = hal.Signal('tool-changed', hal.HAL_BIT)
c.link('iocontrol.0.tool-change')
c.link('iocontrol.0.tool-changed')
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_rcomp():
rcomp = hal.RemoteComponent('control', timer=100)
rcomp.newpin('vel-cmd', hal.HAL_FLOAT, hal.HAL_OUT)
rcomp.newpin('enable', hal.HAL_BIT, hal.HAL_OUT)
rcomp.ready()
rcomp.pin('vel-cmd').link(hal.Signal('motor-vel'))
rcomp.pin('enable').link(hal.Signal('motor-enable'))
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, 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 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 _init_hm2(self):
mesahandler = MesaHandler(
device='7I80', address=MESA_BOARD_IP, firmware=MESA_FIRMWARE_FILE
)
mesahandler.load_mesacard()
rt.loadrt('hostmot2')
rt.loadrt(
'hm2_eth',
board_ip=MESA_BOARD_IP,
config='"num_encoders={count},num_stepgens={count}"'.format(
count=NUM_JOINTS
),
)
hal.Pin('hm2_7i80.0.watchdog.timeout_ns').set(int(self.thread.period_ns * 2))
hw_watchdog_signal = hal.Signal('hardware-watchdog', hal.HAL_BIT)
hal.Pin('hm2_7i80.0.watchdog.has_bit').link(hw_watchdog_signal)
self.error_signals.append(hw_watchdog_signal)
reset = rt.newinst('reset', 'reset.watchdog')
hal.addf(reset.name, self.thread.name)
reset.pin('trigger').link('power-on')
reset.pin('reset-bit').set(False)
reset.pin('out-bit').link(hw_watchdog_signal)
def setup_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 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 configure_stepgen():
# stepgen config
hal.Pin('hpg.stepgen.00.steppin').set(812)
hal.Pin('hpg.stepgen.00.dirpin').set(811)
hal.Pin('hpg.stepgen.00.control-type').set(1) # velocity mode
hal.Pin('hpg.stepgen.00.position-scale').set(c.find('STEPPER', 'SCALE'))
hal.Pin('hpg.stepgen.00.dirsetup').set(c.find('STEPPER', 'DIRSETUP'))
hal.Pin('hpg.stepgen.00.dirhold').set(c.find('STEPPER', 'DIRHOLD'))
hal.Pin('hpg.stepgen.00.steplen').set(c.find('STEPPER', 'STEPLEN'))
hal.Pin('hpg.stepgen.00.stepspace').set(c.find('STEPPER', 'STEPSPACE'))
hal.Pin('hpg.stepgen.00.velocity-cmd').link(
hal.Signal('motor-vel', hal.HAL_FLOAT))
hal.Pin('hpg.stepgen.00.enable').link(
hal.Signal('motor-enable', hal.HAL_BIT))
# machine power
hal.Pin('bb_gpio.p9.out-26').link(hal.Signal('motor-enable', hal.HAL_BIT))
hal.Pin('bb_gpio.p8.out-07').link(hal.Signal('motor-enable', hal.HAL_BIT))
hal.Pin('bb_gpio.p8.out-07.invert').set(True)
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_joint_ferror(nr, thread):
cmd_fb_pos = hal.Signal('joint-{}-cmd-fb-pos'.format(nr),
hal.HAL_FLOAT)
fb_in_pos = hal.Signal('joint-{}-fb-in-pos'.format(nr), hal.HAL_FLOAT)
ferror = hal.Signal('joint-{}-ferror'.format(nr), hal.HAL_FLOAT)
ferror_abs = hal.Signal('joint-{}-ferror-abs'.format(nr),
hal.HAL_FLOAT)
ferror_max = hal.Signal('joint-{}-ferror-max'.format(nr),
hal.HAL_FLOAT)
ferror_active = hal.Signal('joint-{}-ferror-active'.format(nr),
hal.HAL_BIT)
sum_ferror = rt.newinst('sum2', 'sum2.joint-{}-ferror'.format(nr))
sum_ferror.pin('in0').link(cmd_fb_pos)
sum_ferror.pin('in1').link(fb_in_pos)
sum_ferror.pin('gain1').set(-1.0)
sum_ferror.pin('out').link(ferror)
abs_ferror = rt.newinst('abs', 'abs.joint-{}-ferror'.format(nr))
abs_ferror.pin('in').link(ferror)
abs_ferror.pin('out').link(ferror_abs)
comp = rt.newinst('comp', 'comp.joint-{}-ferror'.format(nr))
comp.pin('in0').link(ferror_max)
comp.pin('in1').link(ferror_abs)
comp.pin('out').link(ferror_active)
hal.addf(sum_ferror.name, thread.name)
hal.addf(abs_ferror.name, thread.name)
hal.addf(comp.name, thread.name)
def test_signal():
global s1
s1 = hal.Signal(signame, hal.HAL_S32)
assert s1.name == signame
assert s1.type == hal.HAL_S32
assert s1.readers == 0
assert s1.bidirs == 0
assert s1.writers == 0
assert s1.handle > 0
# basic value setting
s1.set(12345)
assert s1.get() == 12345
def _setup_estop_chain(error_signals, thread):
reset = hal.Signal('reset', hal.HAL_BIT)
estop_active = hal.Signal('estop-active', hal.HAL_BIT)
estop_error = hal.Signal('estop-error', hal.HAL_BIT)
estop_in = hal.Signal('estop-in', hal.HAL_BIT)
ok = hal.Signal('ok', hal.HAL_BIT)
num = len(error_signals)
or_comp = rt.newinst('orn',
'or{}.estop-error'.format(num),
pincount=num)
hal.addf(or_comp.name, thread.name)
for n, sig in enumerate(error_signals):
or_comp.pin('in{}'.format(n)).link(sig)
or_comp.pin('out').link(estop_error)
estop_latch = rt.newinst('estop_latch', 'estop-latch')
hal.addf(estop_latch.name, thread.name)
estop_latch.pin('ok-in').link(estop_in)
estop_latch.pin('fault-in').link(estop_error)
estop_latch.pin('fault-out').link(estop_active)
estop_latch.pin('reset').link(reset)
estop_latch.pin('ok-out').link(ok)
def main():
movel("home", v=0.08, a=0.08)
try:
rt.init_RTAPI()
notify("HAL initialized")
sleep(1.0)
except RuntimeError as e:
print(e.message)
#if e.message == "signal 'mod_success' does not exist"
#pass
try:
hal.Signal("mod_success")
except RuntimeError as e:
if e.message == "signal 'mod_success' does not exist":
change_configuration.change_config()
notify("HAL configuration changed")
sleep(1.0)
if hal.Signal('mod_success').get() == True:
l = mastering_app.Leveller(name="Masterer")
l.init_attributes()
l.fine_calibration = True
l.calib_fast_iter = 2
l.calib_slow_iter = 4
l.max_vel = 0.4
l.max_acc = 0.8
notify("Please press OK to start the mastering program.", warning=True)
l.info_callback = notify
l.start()
notify(
"THIS IS NOT AN ERROR. The mastering of the robot has finished. The robot has moved to the pose that should hold all zeros as joint angles. Press ABORT to exit this program, power off the robot, restart, and press ZERO ALL JOINTS from the SETTINGS tab before powering on again.",
error=True)
else:
notify(
"It looks like this configuration is not suitable for mastering the robot. Please look for details in the applicable instructions.",
error=True)
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
# stepgen
stepgen = hal.instances['stepgen.{}'.format(nr)]
stepgen.pin('position-scale').set(scale)
stepgen.pin('maxvel').set(c['max_vel_rad_s'] / 10)
stepgen.pin('maxaccel').set(c['max_accel_rad_s2'] / 10)
stepgen.pin('enable').link('brake-release-{}-out'.format(i))
stepgen.pin('position-cmd').link('joint-{}-cmd-out-pos'.format(i))
stepgen.pin('position-fb').link('joint-{}-cmd-fb-pos'.format(i))
# encoder
sum2 = rt.newinst('sum2', 'sum2.encoder-{}-fb'.format(nr))
hal.addf(sum2.name, self.thread.name)
sum2.pin('in0').link('joint-{}-cmd-fb-pos'.format(i))
sum2.pin('in1').set(
0.0
) # can be used to set an artificial offset to trigger the alarm
sum2.pin('out').link('joint-{}-fb-in-pos'.format(i))
# encoder abs
hal.Signal('joint-{}-abs-pos'.format(i)).set(c['home_pos'])
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'])
def setup_hal():
sim = rospy.get_param('/sim_mode', True)
if sim:
os.environ['SIM_MODE'] = '1'
from borunte_hal.robot import setup_thread, configure_hal
cgname = rospy.get_param('/hal_mgr/hal_cgroup_name')
thread = setup_thread(cgname)
create_hw_interface(thread)
configure_hal(thread)
time.sleep(1.0)
connect_hw_interface()
# start the sim config "powered on"
if sim:
hal.Signal('power-on').set(True)
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 change_config():
# connect to HAL
try:
rt.init_RTAPI()
except RuntimeError as e:
pass
#hal.stop_threads()
time.sleep(0.1)
# check for an existing signal "mod_success"
try:
hal.Signal("mod_success")
except RuntimeError as e:
if e.message == "signal 'mod_success' does not exist":
# good to go, no previous attempt
insert_jplanners()
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)
class Ferror(object):
cmd_fb_pos = hal.Signal('joint-{}-cmd-fb-pos'.format(nr),
hal.HAL_FLOAT)
fb_in_pos = hal.Signal('joint-{}-fb-in-pos'.format(nr), hal.HAL_FLOAT)
ferror = hal.Signal('joint-{}-ferror'.format(nr), hal.HAL_FLOAT)
ferror_abs = hal.Signal('joint-{}-ferror-abs'.format(nr),
hal.HAL_FLOAT)
ferror_max = hal.Signal('joint-{}-ferror-max'.format(nr),
hal.HAL_FLOAT)
ferror_active = hal.Signal('joint-{}-ferror-active'.format(nr),
hal.HAL_BIT)
def __init__(self):
BorunteConfig._setup_joint_ferror(nr, hal_config.thread)
def _init_modbus(self):
name = 'i620p-abs'
interval_s = 0.2
hal.loadusr(
'i620p_modbus.py -c {count} -n {name} -i {interval} -s {serial}'.format(
count=NUM_JOINTS,
name=name,
interval=interval_s,
serial=I620P_USB_SERIAL_ID,
),
wait_name='i620p-abs',
)
self.user_comps.append(
UserComp(name=name, timeout=(interval_s * TIMEOUT_OVERHEAD))
)
comp = hal.components[name]
error = hal.Signal('{}-error'.format(name), hal.HAL_BIT)
comp.pin('error').link(error)
self.error_signals.append(error)
def _init_gripper(self):
if not self.tool.startswith('hand_e'):
return
name = 'robotiq-gripper'
interval_s = 0.1
hal.loadusr(
'robotiq_modbus.py -n {name} -i {interval} -s {serial}'.format(
name=name, interval=interval_s, serial=ROBOTIQ_USB_SERIAL_ID
),
wait_name=name,
)
self.user_comps.append(
UserComp(name=name, timeout=(interval_s * TIMEOUT_OVERHEAD))
)
comp = hal.components[name]
error = hal.Signal('{}-error'.format(name), hal.HAL_BIT)
comp.pin('error').link(error)
self.error_signals.append(error)
def setup_hbp_led(thread):
tempMeas = hal.Signal('hbp-temp-meas')
ledHbpHot = hal.newsig('led-hbp-hot', hal.HAL_BIT)
ledHbpInfo = hal.newsig('led-hbp-info', hal.HAL_BIT)
# low temp
comp = rt.newinst('comp', 'comp.hbp-info')
hal.addf(comp.name, thread)
comp.pin('in0').link(tempMeas)
comp.pin('in1').set(50.0)
comp.pin('hyst').set(2.0)
comp.pin('out').link(ledHbpInfo)
# high temp
comp = rt.newinst('comp', 'comp.hbp-hot')
hal.addf(comp.name, thread)
comp.pin('in0').set(50.0)
comp.pin('in1').link(tempMeas)
comp.pin('hyst').set(2.0)
comp.pin('out').link(ledHbpHot)
def setup_stepper(stepgenIndex,
section,
axisIndex=None,
stepgenType='hpg.stepgen',
gantry=False,
gantryJoint=0,
velocitySignal=None,
thread=None):
stepgen = '%s.%01i' % (stepgenType, stepgenIndex)
if axisIndex is not None:
axis = 'axis.%i' % axisIndex
hasMotionAxis = (axisIndex is not None) and (not gantry
or gantryJoint == 0)
velocityControlled = velocitySignal is not None
# axis enable chain
enableIndex = axisIndex
if axisIndex is None:
enableIndex = 0 # use motor enable signal
enable = hal.Signal('emcmot-%i-enable' % enableIndex, hal.HAL_BIT)
if hasMotionAxis:
enable.link('%s.amp-enable-out' % axis)
enable.link('%s.enable' % stepgen)
# expose timing parameters so we can multiplex them later
sigBase = 'stepgen.%i' % stepgenIndex
assign_param(sigBase, "position-scale", c.find(section, 'SCALE'))
assign_param(sigBase, "maxaccel", c.find(section, 'STEPGEN_MAXACC'))
#assign_param(sigBase,"maxvel",c.find("ABP", 'STEPGEN_MAXVEL'))
# position command and feedback
limitHome = hal.newsig('limit-%i-home' % axisIndex, hal.HAL_BIT)
limitMin = hal.newsig('limit-%i-min' % axisIndex, hal.HAL_BIT)
limitMax = hal.newsig('limit-%i-max' % axisIndex, hal.HAL_BIT)
limitHome.link('%s.home-sw-in' % axis)
limitMin.link('%s.neg-lim-sw-in' % axis)
limitMax.link('%s.pos-lim-sw-in' % axis)
if velocityControlled:
hal.net(velocitySignal, '%s.velocity-cmd' % stepgen)
def _setup_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)