def setup_thread(cgname=None):
thread_period = 1e7 if SIM_MODE else 1e6
thread = HalThread(name='main_thread', period_ns=thread_period)
kwargs = {}
if cgname and not SIM_MODE:
kwargs['cgname'] = cgname
rt.newthread(thread.name, thread.period_ns, fp=True, **kwargs)
return thread
def init_hardware():
# we need a thread to execute the component functions
rt.newthread(MAIN_THREAD, 1e6, fp=True)
rt.loadrt('hal_bb_gpio', output_pins='926,807', input_pins='')
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
'prucode=' + c.find('PRUCONF', 'PRUBIN'),
pru=0,
num_pwmgens=3,
num_stepgens=3,
halname='hpg')
def hal_config():
from machinekit import launcher
launcher.cleanup_session()
comp_path = os.path.join(
os.path.dirname(os.path.realpath(__file__)),
'../components/absolute_joint.icomp',
)
launcher.install_comp(comp_path)
launcher.start_realtime()
rt.init_RTAPI()
config = HalConfig(thread=THREAD)
rt.newthread(config.thread.name, config.thread.period_ns, fp=True)
hal.start_threads()
yield config
hal.stop_threads()
launcher.end_session()
from machinekit import hal
from machinekit import rtapi as rt
# we need a thread to execute the component functions
rt.newthread('main-thread', 1000000, fp=False)
# create the signal for connecting the components
input0 = hal.newsig('input0', hal.HAL_BIT)
input1 = hal.newsig('input1', hal.HAL_BIT)
output = hal.newsig('output', hal.HAL_BIT)
# and2 component
and2 = rt.newinst('and2', 'and2.demo')
and2.pin('in0').link(input0)
and2.pin('in1').link(input1)
and2.pin('out').link(output)
hal.addf(and2.name, 'main-thread')
# create remote component
rcomp = hal.RemoteComponent('anddemo', timer=100)
rcomp.newpin('button0', hal.HAL_BIT, hal.HAL_OUT)
rcomp.newpin('button1', hal.HAL_BIT, hal.HAL_OUT)
rcomp.newpin('led', hal.HAL_BIT, hal.HAL_IN)
rcomp.ready()
# link remote component pins
rcomp.pin('button0').link(input0)
rcomp.pin('button1').link(input1)
rcomp.pin('led').link(output)
# ready to start the threads
hpg = hal.Component('hpg')
hpg.pin('pwmgen.00.out.00.pin').set(IN1)
hpg.pin('pwmgen.00.out.01.pin').set(IN2)
hpg.pin('encoder.00.chan.00.A-pin').set(pru0_input_pins[ENC_A])
hpg.pin('encoder.00.chan.00.B-pin').set(pru0_input_pins[ENC_B])
hpg.pin('encoder.00.chan.00.index-pin').set(17) # ignore
hpg.pin('encoder.00.chan.00.counter-mode').set(0) # Quadrature Mode
gpio = hal.Component('bb_gpio')
setup_storage()
# we need a thread to execute the component functions
rt.newthread('main-thread', 1000000, fp=True)
hal.addf('bb_gpio.read', 'main-thread')
hal.addf('hpg.capture-position', 'main-thread')
# motor = setup_servo(name='m1',
# thread='main-thread',
# encoder='hpg.encoder.00.chan.00',
# pwmDown='hpg.pwmgen.00.out.00',
# pwmUp='hpg.pwmgen.00.out.01',
# enableDown=get_io_pin(EN1, PinDirection.OUT).name,
# enableUp=get_io_pin(EN2, PinDirection.OUT).name)
hal.addf('hpg.update', 'main-thread')
hal.addf('bb_gpio.write', 'main-thread')
def instantiate_threads():
print('instantiating thread(s)')
rt.newthread('st', 500000, fp=True)
halname='hpg')
# storage
setupStorage()
# pru pwmgens
hal.Pin('hpg.pwmgen.00.pwm_period').set(500000)
# motor left
hal.Pin('hpg.pwmgen.00.out.00.pin').set(911)
hal.Pin('hpg.pwmgen.00.out.01.pin').set(913)
# motor right
hal.Pin('hpg.pwmgen.00.out.02.pin').set(808)
hal.Pin('hpg.pwmgen.00.out.03.pin').set(810)
baseThread = 'base_thread'
rt.newthread(baseThread, 1000000, fp=True)
hal.addf('bb_gpio.read', baseThread)
hal.addf('eqep.update', baseThread)
ml = Motor(name='ml',
eqep='eQEP0',
eqepScale=-2797.0,
pwmUp='hpg.pwmgen.00.out.01',
pwmDown='hpg.pwmgen.00.out.00',
enableDown='bb_gpio.p9.out-15',
enableUp='bb_gpio.p9.out-17')
mr = Motor(name='mr',
eqep='eQEP2',
eqepScale=2797.0,
pwmUp='hpg.pwmgen.00.out.02',
pwmDown='hpg.pwmgen.00.out.03',
rt.loadrt(c.find('PRUCONF', 'DRIVER'), 'prucode=' + c.find('PRUCONF', 'PRUBIN'), pru=0, num_pwmgens=7, halname='hpg')
# storage
setupStorage()
# pru pwmgens
hal.Pin('hpg.pwmgen.00.pwm_period').set(500000)
# motor left
hal.Pin('hpg.pwmgen.00.out.00.pin').set(911)
hal.Pin('hpg.pwmgen.00.out.01.pin').set(913)
# motor right
hal.Pin('hpg.pwmgen.00.out.02.pin').set(808)
hal.Pin('hpg.pwmgen.00.out.03.pin').set(810)
baseThread = 'base_thread'
rt.newthread(baseThread, 1000000, fp=True)
hal.addf('bb_gpio.read', baseThread)
hal.addf('eqep.update', baseThread)
ml = Motor(name='ml', eqep='eQEP0', eqepScale=-2797.0,
pwmUp='hpg.pwmgen.00.out.01',
pwmDown='hpg.pwmgen.00.out.00',
enableDown='bb_gpio.p9.out-15',
enableUp='bb_gpio.p9.out-17')
mr = Motor(name='mr', eqep='eQEP2', eqepScale=2797.0,
pwmUp='hpg.pwmgen.00.out.02',
pwmDown='hpg.pwmgen.00.out.03',
enableDown='bb_gpio.p8.out-38',
enableUp='bb_gpio.p8.out-40')
setupGyro()
# load the ring for use with Charles' joint_stream component.
# for this you need to have built with catkin_make
# https://github.com/cdsteinkuehler/ros_hal_tests.git
# and also installed the component `comp --install joint_stream.comp`
# newring jointpos 16384
r1 = hal.Ring("jointpos", size=16384)
rt.loadrt("joint_stream", "ring=jointpos")
# create servo_thread
servothread = "servo_thread"
# create a new thread
rt.newthread('%s' % servothread, 1000000, fp=True)
# add components to the thread
if hardware == 'mesa-5i20':
hal.addf('%s.read' % card, servothread)
hal.addf('siggen.0.update', servothread)
hal.addf('joint_stream', servothread)
hal.addf('%s.write' % card, servothread)
hal.addf('%s.pet_watchdog' % card, servothread)
if ((hardware == 'bbb-cramps') or (hardware == 'bbb-bebopr++')):
hal.addf('%s.capture-position' % card, servothread)
hal.addf('bb_gpio.read', servothread)
hal.addf('siggen.0.update', servothread)
hal.addf('joint_stream', servothread)
# load the ring for use with Charles' joint_stream component.
# for this you need to have built with catkin_make
# https://github.com/cdsteinkuehler/ros_hal_tests.git
# and also installed the component `comp --install joint_stream.comp`
# newring jointpos 16384
r1 = hal.Ring("jointpos", size=16384)
rt.loadrt("joint_stream","ring=jointpos")
# create servo_thread
servothread = "servo_thread"
# create a new thread
rt.newthread('%s' % servothread, 1000000, fp=True)
# add components to the thread
if hardware == 'mesa-5i20':
hal.addf('%s.read' % card, servothread)
hal.addf('siggen.0.update', servothread)
hal.addf('joint_stream', servothread)
hal.addf('%s.write' % card, servothread)
hal.addf('%s.pet_watchdog' % card, servothread)
if ((hardware == 'bbb-cramps') or (hardware == 'bbb-bebopr++')) :
hal.addf('%s.capture-position' % card, servothread)
hal.addf('bb_gpio.read', servothread)
hal.addf('siggen.0.update', servothread)
hal.addf('joint_stream', servothread)
hal.Pin('hpg.stepgen.00.steplen').set(c.find('AXIS_0', 'STEPLEN'))
hal.Pin('hpg.stepgen.00.stepspace').set(c.find('AXIS_0', 'STEPSPACE'))
hal.Pin('hpg.stepgen.00.position-scale').set(c.find('AXIS_0', 'SCALE'))
hal.Pin('hpg.stepgen.00.maxvel').set(c.find('AXIS_0', 'STEPGEN_MAX_VEL'))
hal.Pin('hpg.stepgen.00.maxaccel').set(c.find('AXIS_0', 'STEPGEN_MAX_ACC'))
hal.Pin('hpg.stepgen.00.minvel').set(c.find('AXIS_0', 'STEPGEN_MIN_VEL'))
#sets XStep P8.12
hal.Pin('hpg.stepgen.00.steppin').set(812)
#sets XDir P8.11
hal.Pin('hpg.stepgen.00.dirpin').set(811)
servo_thread = 'servo_thread'
rt.newthread(servo_thread, c.find('TASK', 'CYCLE_TIME') * 1e9, fp=True)
hal.addf('hpg.capture-position', servo_thread)
hal.addf('bb_gpio.read', servo_thread)
# TODO: HAL comps
rcomp = hal.RemoteComponent('command-interface', timer=100)
for i in range(_MAX_AXES):
rcomp.newpin('joint%i.position-cmd' % i, hal.HAL_FLOAT, hal.HAL_OUT)
rcomp.newpin('joint%i.position-fb' % i, hal.HAL_FLOAT, hal.HAL_IN)
rcomp.newpin('joint%i.enable' % i, hal.HAL_BIT, hal.HAL_IO)
rcomp.newpin('joint%i.position-min' % i, hal.HAL_FLOAT, hal.HAL_IO)
rcomp.newpin('joint%i.position-max' % i, hal.HAL_FLOAT, hal.HAL_IO)
rcomp.ready()
for i in range(_MAX_AXES):
def instantiate_threads(arguments):
print('instantiating thread(s)')
cycletime = arguments.cycle
rt.newthread('veryfast', 50000)
rt.newthread('st', int(cycletime), fp=True)