def init_hardware():
watchList = []
# load low-level drivers
rt.loadrt('hal_bb_gpio',
output_pins='807,808,810,819,828,840,841',
input_pins='809,831,832,833,835,837,838')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR, c.find('PRUCONF', 'PRUBIN'))
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
pru=1,
num_stepgens=4,
num_pwmgens=3,
prucode=prubin,
halname='hpg')
# Python user-mode HAL module to read ADC value and generate a thermostat output for PWM
defaultThermistor = 'semitec_103GT_2'
hal.loadusr(
'hal_temp_bbb',
name='temp',
interval=0.10,
filter_size=5,
cape_board='CRAMPS',
r_pu=2185, # measured value of my 2.2k pullup resistor
channels='04:%s,05:%s' %
(c.find('HBP', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_0', 'THERMISTOR', defaultThermistor)),
wait_name='temp')
watchList.append(['temp', 0.2])
base.usrcomp_status('temp', 'temp-hw', thread='servo-thread')
base.usrcomp_watchdog(watchList,
'estop-reset',
thread='servo-thread',
errorSignal='watchdog-error')
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 init_hardware():
watchList = []
# load low-level drivers
rt.loadrt('hal_bb_gpio', output_pins='816,822,823,824,825,826,914,923,925', input_pins='807,808,809,810,817,911,913')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR, c.find('PRUCONF', 'PRUBIN'))
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
pru=0, num_stepgens=6, num_pwmgens=6,
prucode=prubin, halname='hpg')
# Python user-mode HAL module to read ADC value and generate a thermostat output for PWM
defaultThermistor = 'semitec_103GT_2'
hal.loadusr('hal_temp_bbb',
name='temp',
interval=0.05,
filter_size=1,
cape_board='CRAMPS',
channels='04:%s,05:%s,02:%s,03:%s'
% (c.find('HBP', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_0', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_1', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_2', 'THERMISTOR', defaultThermistor)),
wait_name='temp')
watchList.append(['temp', 0.1])
base.usrcomp_status('temp', 'temp-hw', thread='servo-thread')
base.usrcomp_watchdog(watchList, 'estop-reset', thread='servo-thread',
errorSignal='watchdog-error')
def init_hardware():
watchList = []
# load low-level drivers
rt.loadrt('hal_bb_gpio',
output_pins='816,822,823,824,825,826,914,923,925',
input_pins='807,808,809,810,817,911,913')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR, c.find('PRUCONF', 'PRUBIN'))
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
pru=0,
num_stepgens=6,
num_pwmgens=6,
prucode=prubin,
halname='hpg')
# Python user-mode HAL module to read ADC value and generate a thermostat output for PWM
defaultThermistor = 'semitec_103GT_2'
hal.loadusr('hal_temp_bbb',
name='temp',
interval=0.05,
filter_size=1,
cape_board='CRAMPS',
channels='04:%s,05:%s,02:%s,03:%s' %
(c.find('HBP', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_0', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_1', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_2', 'THERMISTOR', defaultThermistor)),
wait_name='temp')
watchList.append(['temp', 0.1])
base.usrcomp_status('temp', 'temp-hw', thread='servo-thread')
base.usrcomp_watchdog(watchList,
'estop-reset',
thread='servo-thread',
errorSignal='watchdog-error')
def create_hw_interface(thread):
rt.loadrt('{}/hal_hw_interface'.format(os.environ['COMP_DIR']))
hal.addf('hal_hw_interface', thread.name)
oneshot = rt.newinst('oneshot', 'oneshot.hw_reset')
hal.addf(oneshot.name, thread.name)
oneshot.pin('in').link('power-on')
oneshot.pin('out').link('hw-reset')
oneshot.pin('width').set(HARDWARE_RESET_DELAY_S)
def init_hardware():
# 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 init_hardware():
watchList = []
# load low-level drivers
rt.loadrt('hal_parport', cfg='0x0378')
rt.loadrt('stepgen', step_type='0,0,0,0', ctrl_type="p,p,p,p")
#rt.loadrt("ilowpass", names="ilowpass.jog.x")
deb = rt.newinst('debounce', 'debounce')
# witches-check=
rt.newinst("orn", "switches-check", pincount="4")
# pause-check=
rt.newinst("andn", "pause-home", pincount="2")
rt.newinst("edge", "edge")
def init_hardware():
# load low-level drivers
rt.loadrt('hal_bb_gpio',
output_pins='807,808,810,819,828,841',
input_pins='809,831,832,833,835,837,838')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR, c.find('PRUCONF', 'PRUBIN'))
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
pru=0,
num_stepgens=4,
num_pwmgens=4,
pru_period=2500,
prucode=prubin,
halname='hpg')
def setup_motion(kinematics='trivkins', tp='tp', num_aio=50, num_dio=21):
rt.loadrt(kinematics)
rt.loadrt(tp)
# hostmot2 setup get names and config from ini file
hostmot2 = bool(c.find('HOSTMOT2', 'DRIVER'))
if hostmot2:
rt.loadrt('hostmot2')
os.system('halcmd newinst hm2_soc_ol hm2-socfpga0 already_programmed=1 -- config="num_pwmgens=1 num_stepgens=1 num_bspis=1 enable_adc=0"')
# rt.newinst(c.find('HOSTMOT2', 'DRIVER'),
# c.find('HOSTMOT2', 'DEVNAME'),
# "-- ",
# c.find('HOSTMOT2', 'CONFIG'))
# motion controller, get name and thread periods from ini file
rt.loadrt(c.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=c.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=c.find('TRAJ', 'AXES'),
num_aio=num_aio,
num_dio=num_dio,
tp=tp,
kins=kinematics)
def loadrt_local(modname):
"""Load a locally-built HAL component not installed in the standard
module directory
"""
if modname in hal.components:
return
for path in os.environ.get("LD_LIBRARY_PATH", "").split(":"):
rospy.logdebug(f"Checking for {modname}.so in {path}")
modpath = os.path.join(path, f"{modname}")
if os.path.exists(f"{modpath}.so"):
break
else:
raise RuntimeError(f"Unable to locate {modname} module")
rtapi.loadrt(modpath)
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 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 __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 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 __init__(self):
self.gpio = rtapi.loadrt('hal_bb_gpio',
output_pins='941',
input_pins='810,809,924,818,923,925,928,918,911,913')
self.hwconfig = hal.loadusr(USR_HAL_PATH + 'hal_replicape_B3_hwconfig',
name='replicape_hwconfig',
wait_name='replicape_hwconfig')
for i in xrange(5):
# Default: spreadMode, microstepping=1/16
self.hwconfig.pin('stepper.%i.mode' % i).set(config.find('FDM','STEPPER_%i_MODE' % i, 0x80))
super(ReplicapeB3A, self).__init__()
def __init__(self):
self.gpio = rtapi.loadrt('hal_bb_gpio',
output_pins='941',
input_pins='810,809,924,818,923,925,928,918,911,913')
self.hwconfig = hal.loadusr(USR_HAL_PATH + 'hal_replicape_B3_hwconfig',
name='replicape_hwconfig',
wait_name='replicape_hwconfig')
for i in xrange(5):
# Default: spreadMode, microstepping=1/16
self.hwconfig.pin('stepper.%i.mode' % i).set(config.find('FDM','STEPPER_%i_MODE' % i, 0x80))
super(ReplicapeB3A, self).__init__()
def setup_motion(kinematics='trivkins'):
rt.loadrt(kinematics)
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)
def setup_motion(kinematics='trivkins'):
rt.loadrt(kinematics)
rt.loadrt('tp')
rt.loadrt('hostmot2')
# rt.newinst(c.find('HOSTMOT2', 'DRIVER'),
# c.find('HOSTMOT2', 'DEVNAME'),
# c.find('HOSTMOT2', 'CONFIG'))
os.system(
'halcmd newinst hm2_soc_ol hm2-socfpga0 -- config="firmware=socfpga/dtbo/DE0_Nano_SoC_Cramps.3x24.dtbo num_pwmgens=6 num_stepgens=8 enable_adc=1" debug=1'
)
# 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)
def setup_motion(kinematics='trivkins', tp='tp', num_aio=50, num_dio=21):
rt.loadrt(kinematics)
rt.loadrt(tp)
# hostmot2 setup get names and config from ini file
hostmot2 = bool(c.find('HOSTMOT2', 'DRIVER'))
if hostmot2:
rt.loadrt('hostmot2')
rt.newinst(c.find('HOSTMOT2', 'DRIVER'), c.find('HOSTMOT2', 'DEVNAME'),
"-- ", c.find('HOSTMOT2', 'CONFIG'))
# motion controller, get name and thread periods from ini file
rt.loadrt(c.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=c.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=c.find('TRAJ', 'AXES'),
num_aio=num_aio,
num_dio=num_dio,
tp=tp,
kins=kinematics)
def setup_motion(kinematics='trivkins'):
print ("file of config is ",c.__file__)
rt.loadrt(kinematics)
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'),
base_period_nsec=50000,
num_joints=c.find('TRAJ', 'AXES'),
num_aio=51,
num_dio=21,
kins=kinematics)
def __init__(self):
self.gpio = rtapi.loadrt('hal_bb_gpio',
output_pins='',
input_pins='810,809,924,818,923,925,916,918,911,913')
self.dac = hal.loadusr(USR_HAL_PATH + 'hal_replicape_dac',
name='replicape_dac',
wait_name='replicape_dac')
self.hwconfig = hal.loadusr(USR_HAL_PATH + 'hal_replicape_A4_hwconfig',
name='replicape_hwconfig',
wait_name='replicape_hwconfig')
for i in xrange(5):
# Default: microstepping=1/32, fast decay
self.hwconfig.pin('stepper.%i.microstepping' % i).set(config.find('FDM','STEPPER_%i_MICROSTEPPING' % i, 5))
self.hwconfig.pin('stepper.%i.decay' % i).set(config.find('FDM','STEPPER_%i_DECAY' % i, 0))
super(ReplicapeA4A, self).__init__()
def __init__(self):
self.gpio = rtapi.loadrt('hal_bb_gpio',
output_pins='',
input_pins='810,809,924,818,923,925,916,918,911,913')
self.dac = hal.loadusr(USR_HAL_PATH + 'hal_replicape_dac',
name='replicape_dac',
wait_name='replicape_dac')
self.hwconfig = hal.loadusr(USR_HAL_PATH + 'hal_replicape_A4_hwconfig',
name='replicape_hwconfig',
wait_name='replicape_hwconfig')
for i in xrange(5):
# Default: microstepping=1/32, fast decay
self.hwconfig.pin('stepper.%i.microstepping' % i).set(config.find('FDM','STEPPER_%i_MICROSTEPPING' % i, 5))
self.hwconfig.pin('stepper.%i.decay' % i).set(config.find('FDM','STEPPER_%i_DECAY' % i, 0))
super(ReplicapeA4A, self).__init__()
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 __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 __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 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 init_hardware():
config.load_ini(os.environ['INI_FILE_NAME'])
check_version()
# we need to make the gpio pins used by the pru visible to userspace (??) in /sys/class/gpio
os.system('sudo ' + HARDWARE_PATH + 'set_pru_gpio.sh')
rtapi.init_RTAPI()
error_sigs = []
watchdog_sigs = []
global AXIS_TOTAL
AXIS_TOTAL = int(config.find('TRAJ', 'AXES', '3'))
if AXIS_TOTAL < 3:
raise RuntimeError("AXES must be >= 3")
global EXTRUDER_TOTAL
EXTRUDER_TOTAL = int(config.find('FDM', 'EXTRUDERS', '1'))
if EXTRUDER_TOTAL < 1:
raise RuntimeError("EXTRUDERS must be >= 1")
global NUM_FANS
NUM_FANS = int(config.find('FDM','NUM_FANS', 4))
system_fan = int(config.find('FDM','SYSTEM_FAN', 0))
if (NUM_FANS < system_fan):
raise RuntimeError("NUM_FANS must be >= 1 if SYSTEM_FAN > 0")
if (NUM_FANS > 4) or (NUM_FANS < 0 ):
raise RuntimeError("NUM_FANS must be > 0 and < 5")
rtapi.loadrt('tp')
if config.find('MACHINE','DELTA_R') is not None:
kinematics = 'lineardeltakins'
rtapi.loadrt(kinematics)
hal.Pin('lineardeltakins.L').set(config.find('MACHINE', 'CF_ROD'))
hal.Pin('lineardeltakins.R').set(config.find('MACHINE', 'DELTA_R'))
else:
kinematics = 'trivkins'
rtapi.loadrt(kinematics)
rtapi.loadrt(config.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=config.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=str(AXIS_TOTAL),
num_aio=51,
num_dio=21,
kins = kinematics)
hal.addf('motion-command-handler', SERVO_THREAD)
hal.addf('motion-controller', SERVO_THREAD)
estop_reset = hal.newsig('estop-reset', hal.HAL_BIT)
(main_enable, main_enable_inv) = setup_enable_chain()
if BOARD_REV == 'B3A':
replicape = hardware.ReplicapeB3A()
if BOARD_REV == 'A4A':
replicape = hardware.ReplicapeA4A()
watchdog_sigs.extend(replicape.get_watchdog_sigs())
replicape.link_enable(main_enable, main_enable_inv)
(joint_commands, joint_feedbacks) = setup_joints()
for i in xrange(AXIS_TOTAL):
setup_axis(replicape, joint_commands[i], joint_feedbacks[i], i)
setup_limit_switches(replicape)
extruder_sel_sig = hal.newsig('extruder-sel', hal.HAL_S32)
setup_extruders(replicape, extruder_sel_sig)
for i in xrange(EXTRUDER_TOTAL):
t = ExtruderTemperature(replicape, i, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
if config.find('HBP', 'THERMISTOR'):
t = HbpTemperature(replicape, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
# setup_system_fan(replicape)
setup_fans(replicape)
setup_estop(error_sigs, watchdog_sigs, estop_reset, SERVO_THREAD)
connect_tool_changer()
def _init_encoders(self):
rt.loadrt('encoder', num_chan=NUM_JOINTS)
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=True)
# load GPIO driver
rt.loadrt('hal_chip_gpio', output_pins='0,1,2,3', input_pins='4,5,6,7')
gpio = hal.Component('chip_gpio')
gpio.pin('out-00').link('square')
# load siggen
rt.loadrt('siggen')
siggen = hal.Component('siggen.0')
siggen.pin('frequency').set(10.0)
siggen.pin('clock').link('square')
# setup update functions
hal.addf('chip_gpio.read', 'main-thread')
hal.addf('siggen.0.update', 'main-thread')
hal.addf('chip_gpio.write', 'main-thread')
# ready to start the threads
hal.start_threads()
# start haltalk server after everything is initialized
# else binding the remote components on the UI might fail
#hal.loadusr('haltalk')
for argument, description in hardwarelist.iteritems():
print "{:<15} {:<20}".format(argument, description[0])
exit(1)
else:
print("card description : %s, %s") % (hardware, hardwarelist[hardware][0])
print("hal component name: %s") % (hardwarelist[hardware][1])
# try to connect to HAL
rt.init_RTAPI()
# set the correct string for HAL component, depending on the hardware
card = hardwarelist[hardware][1]
#load mesa card with firmware
if hardware == 'mesa-5i20':
rt.loadrt('hostmot2')
rt.loadrt('hm2_pci', config="firmware=hm2/5i20/SVST8_4.BIT \
num_pwmgens=3 \
num_stepgens=4")
if hardware == 'bbb-cramps':
os.system('./setup.sh')
rt.loadrt('hal_bb_gpio',
output_pins='816,822,823,824,825,826,914,923,925',
input_pins='807,808,809,810,817,911,913')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR, 'xenomai/pru_generic.bin')
rt.loadrt('hal_pru_generic',
pru=0, num_stepgens=5,
num_pwmgens=0,
prucode=prubin,
halname=card)
for argument, description in hardwarelist.iteritems():
print "{:<15} {:<20}".format(argument, description[0])
exit(1)
else:
print("card description : %s, %s") % (hardware, hardwarelist[hardware][0])
print("hal component name: %s") % (hardwarelist[hardware][1])
# try to connect to HAL
rt.init_RTAPI()
# set the correct string for HAL component, depending on the hardware
card = hardwarelist[hardware][1]
#load mesa card with firmware
if hardware == 'mesa-5i20':
rt.loadrt('hostmot2')
rt.loadrt('hm2_pci',
config="firmware=hm2/5i20/SVST8_4.BIT \
num_pwmgens=3 \
num_stepgens=4")
if hardware == 'bbb-cramps':
os.system('./setup.sh')
rt.loadrt('hal_bb_gpio',
output_pins='816,822,823,824,825,826,914,923,925',
input_pins='807,808,809,810,817,911,913')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR, 'xenomai/pru_generic.bin')
rt.loadrt('hal_pru_generic',
pru=0,
num_stepgens=5,
num_pwmgens=0,
ENC_B = io_map[18]
IS2 = io_map[19]
ENC_A = io_map[20]
EN1 = io_map[21]
EN2 = io_map[22]
IN1 = io_map[23]
IN2 = io_map[24]
output_pins = [EN1, EN2]
input_pins = [IS1, IS2]
output_pins_joined = ','.join(str(x) for x in output_pins)
input_pins_joined = ','.join(str(x) for x in input_pins)
rt.loadrt('hal_bb_gpio',
output_pins=output_pins_joined,
input_pins=input_pins_joined)
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
'prucode={}/{}'.format(c.Config().EMC2_RTLIB_DIR,
c.find('PRUCONF', 'PRUBIN')),
pru=0,
num_stepgens=0,
num_pwmgens=2,
num_encoders=1,
halname='hpg')
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])
def init_hardware():
check_version()
rtapi.init_RTAPI()
config.load_ini(os.environ['INI_FILE_NAME'])
error_sigs = []
watchdog_sigs = []
global AXIS_TOTAL
AXIS_TOTAL = int(config.find('TRAJ', 'AXES', '3'))
if AXIS_TOTAL < 3:
raise RuntimeError("AXES must be >= 3")
global EXTRUDER_TOTAL
EXTRUDER_TOTAL = int(config.find('FDM', 'EXTRUDERS', '1'))
if EXTRUDER_TOTAL < 1:
raise RuntimeError("EXTRUDERS must be >= 1")
rtapi.loadrt('tp')
rtapi.loadrt('trivkins')
rtapi.loadrt(config.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=config.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=str(AXIS_TOTAL),
num_aio=51,
num_dio=21)
hal.addf('motion-command-handler', SERVO_THREAD)
hal.addf('motion-controller', SERVO_THREAD)
estop_reset = hal.newsig('estop-reset', hal.HAL_BIT)
(main_enable, main_enable_inv) = setup_enable_chain()
if BOARD_REV == 'B3A':
replicape = hardware.ReplicapeB3A()
if BOARD_REV == 'A4A':
replicape = hardware.ReplicapeA4A()
watchdog_sigs.extend(replicape.get_watchdog_sigs())
replicape.link_enable(main_enable, main_enable_inv)
(joint_commands, joint_feedbacks) = setup_joints()
for i in xrange(AXIS_TOTAL):
setup_axis(replicape, joint_commands[i], joint_feedbacks[i], i)
setup_limit_switches(replicape)
extruder_sel_sig = hal.newsig('extruder-sel', hal.HAL_S32)
setup_extruders(replicape, extruder_sel_sig)
for i in xrange(EXTRUDER_TOTAL):
t = ExtruderTemperature(replicape, i, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
if config.find('HBP', 'THERMISTOR'):
t = HbpTemperature(replicape, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
setup_system_fan(replicape)
setup_estop(error_sigs, watchdog_sigs, estop_reset, SERVO_THREAD)
connect_tool_changer()
def setup_lcec(configfile=None):
# load ethercat config parser
print('lcec_conf %s' % configfile)
hal.loadusr('lcec_conf %s' % configfile, wait=True)
# load ethercat realtime module
rt.loadrt('lcec')
#for i in range(0, numExtruders):
# errorSignals.append('e%i-error' % i)
#base.setup_estop(errorSignals, thread='servo-thread')
base.setup_estop(errorSignals, thread='servo-thread')
base.setup_tool_loopback()
# Probe
#base.setup_probe(thread='servo-thread')
# Setup Hardware
hardware.setup_hardware(thread='servo-thread')
# write out functions
hal.addf('motion-controller', 'servo-thread')
hardware.hardware_write()
# trinamic_dbspi component: ---------------------------------------------------------------------#
rt.loadrt('trinamic_dbspi', dbspi_chans='hm2_5i25.0.dbspi.0')
trinamicdbspi = hal.Component('trinamic-dbspi.0')
# create the signals for connecting the components
drvctrl = hal.newsig('drvctrl-reg-unsigned', hal.HAL_U32)
chopconf = hal.newsig('chopconf-reg-unsigned', hal.HAL_U32)
smarten = hal.newsig('smarten-reg-unsigned', hal.HAL_U32)
sgcsconf = hal.newsig('sgcsconf-reg-unsigned', hal.HAL_U32)
drvconf = hal.newsig('drvconf-reg-unsigned', hal.HAL_U32)
fullval_0 = hal.newsig('full-val-0-unsigned', hal.HAL_U32)
fullval_1 = hal.newsig('full-val-1-unsigned', hal.HAL_U32)
fullval_2 = hal.newsig('full-val-2-unsigned', hal.HAL_U32)
fullval_3 = hal.newsig('full-val-3-unsigned', hal.HAL_U32)
fullval_4 = hal.newsig('full-val-4-unsigned', hal.HAL_U32)
fullval_5 = hal.newsig('full-val-5-unsigned', hal.HAL_U32)
def init_hardware():
watchList = []
# load low-level drivers
rt.loadrt('hal_bb_gpio', output_pins='807,819,826,926', input_pins='941')
prubin = '%s/%s' % (c.Config().EMC2_RTLIB_DIR, c.find('PRUCONF', 'PRUBIN'))
rt.loadrt(c.find('PRUCONF', 'DRIVER'),
pru=0,
num_stepgens=5,
num_pwmgens=0,
prucode=prubin,
halname='hpg')
# Python user-mode HAL module to interface with an I2C gpio extender
hal.loadusr('hal_gpio_mcp23017',
name='i2c-gpio',
bus_id=2,
address=32,
interval=0.05,
delay=2.5,
input_pins='A00,A01,A02,A03,A04,A05,A06,A07,B06,B07',
output_pins='B00,B01,B02,B03,B04,B05',
wait_name='i2c-gpio')
watchList.append(['i2c-gpio', 0.1])
# Python user-mode HAL module to interface with an I2C PWM generator
hal.loadusr('hal_pwm_pca9685',
name='i2c-pwm',
bus_id=2,
address=67,
interval=0.1,
delay=2.6,
wait_name='i2c-pwm')
watchList.append(['i2c-pwm', 0.2])
# Python user-mode HAL module to interface with an I2C ADC and convert it to temperature
defaultThermistor = 'semitec_103GT_2'
hal.loadusr(
'hal_temp_ads7828',
name='i2c-temp',
bus_id=2,
address=72,
interval=0.05,
delay=2.7,
filter_size=1,
channels='00:%s,01:%s,02:%s,03:%s,04:%s,05:none,06:none,07:none' %
(c.find('HBP', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_0', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_1', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_2', 'THERMISTOR', defaultThermistor),
c.find('EXTRUDER_3', 'THERMISTOR', defaultThermistor)),
wait_name='i2c-temp')
watchList.append(['i2c-temp', 0.1])
base.usrcomp_status('i2c-gpio', 'gpio-hw', thread='servo-thread')
base.usrcomp_status('i2c-pwm', 'pwm-hw', thread='servo-thread')
base.usrcomp_status('i2c-temp', 'temp-hw', thread='servo-thread')
base.usrcomp_watchdog(watchList,
'estop-reset',
thread='servo-thread',
errorSignal='watchdog-error')
def setup_hardware(thread):
# PWM
hal.Pin('i2c-pwm.frequency').set(100) # 100Hz, keeps EMC radiation low
# HBP
hal.Pin('i2c-pwm.out-00.enable').set(True)
hal.Pin('i2c-pwm.out-00.value').link('hbp-temp-pwm')
# configure extruders
for n in range(0, 4):
hal.Pin('i2c-pwm.out-%02i.enable' % (n + 1)).set(True)
hal.Pin('i2c-pwm.out-%02i.value' % (n + 1)).link('e%i-temp-pwm' % n)
# configure fans
for n in range(0, 4):
hal.Pin('i2c-pwm.out-%02i.enable' % (n + 5)).link('f%i-pwm-enable' % n)
hal.Pin('i2c-pwm.out-%02i.value' % (n + 5)).link('f%i-pwm' % n)
hal.Signal('f%i-pwm-enable' % n).set(True)
# configure exps
for n in range(0, 2):
hal.Pin('i2c-pwm.out-%02i.enable' % (n + 9)).link('exp%i-pwm-enable' %
n)
hal.Pin('i2c-pwm.out-%02i.value' % (n + 9)).link('exp%i-pwm' % n)
hal.Signal('exp%i-pwm' % n).set(1.0)
# configure leds
for n, color in enumerate(('g', 'r', 'b', 'w')):
hal.Pin('i2c-pwm.out-%02i.enable' % (n + 11)).set(True)
hal.Pin('i2c-pwm.out-%02i.value' % (n + 11)).link('l0-%s-out' % color)
# PWM0
hal.Pin('i2c-pwm.out-15.enable').set(True)
hal.Pin('i2c-pwm.out-15.value').set(0.0)
# GPIO
hal.Pin('i2c-gpio.A.in-00').link('limit-0-home') # X
hal.Pin('i2c-gpio.A.in-01').link('limit-1-home') # Y
hal.Pin('i2c-gpio.A.in-02').link('limit-2-1-home') # ZR
hal.Pin('i2c-gpio.A.in-03').link('limit-2-0-home') # ZL
hal.Pin('i2c-gpio.A.in-04').link('probe-signal')
# hal.Pin('i2c-gpio.A.in-05').link('sensor1')
# hal.Pin('i2c-gpio.A.in-06').link('sensor1')
# hal.Pin('i2c-gpio.A.in-07').link('sensor1')
hal.Pin('i2c-gpio.B.out-00').link('e0-enable')
hal.Pin('i2c-gpio.B.out-01').link('e1-enable')
hal.Pin('i2c-gpio.B.out-02').link('e2-enable')
hal.Pin('i2c-gpio.B.out-03').link('e3-enable')
hal.Pin('i2c-gpio.B.out-04').link('led-hbp-info')
hal.Pin('i2c-gpio.B.out-05').link('led-hbp-hot')
hal.Pin('i2c-gpio.B.in-06').link('pwr-mon')
hal.Pin('i2c-gpio.B.in-07').link('hbp-mon')
# Adjust as needed for your switch polarity
hal.Pin('i2c-gpio.A.in-00.invert').set(True)
hal.Pin('i2c-gpio.A.in-01.invert').set(True)
hal.Pin('i2c-gpio.A.in-02.invert').set(True)
hal.Pin('i2c-gpio.A.in-03.invert').set(True)
hal.Pin('i2c-gpio.A.in-04.invert').set(True)
hal.Pin('i2c-gpio.A.in-05.invert').set(True)
hal.Pin('i2c-gpio.A.in-06.invert').set(True)
hal.Pin('i2c-gpio.A.in-07.invert').set(True)
hal.Pin('i2c-gpio.B.out-00.invert').set(False)
hal.Pin('i2c-gpio.B.out-01.invert').set(False)
hal.Pin('i2c-gpio.B.out-02.invert').set(False)
hal.Pin('i2c-gpio.B.out-03.invert').set(False)
hal.Pin('i2c-gpio.B.out-04.invert').set(False)
hal.Pin('i2c-gpio.B.out-05.invert').set(False)
hal.Pin('i2c-gpio.B.in-06.invert').set(False)
hal.Pin('i2c-gpio.B.in-07.invert').set(False)
# Enable pullup for mechanical endstops
hal.Pin('i2c-gpio.A.in-00.pullup').set(True)
hal.Pin('i2c-gpio.A.in-01.pullup').set(True)
hal.Pin('i2c-gpio.A.in-02.pullup').set(True)
hal.Pin('i2c-gpio.A.in-03.pullup').set(True)
hal.Pin('i2c-gpio.A.in-04.pullup').set(True)
hal.Pin('i2c-gpio.A.in-05.pullup').set(False)
hal.Pin('i2c-gpio.A.in-06.pullup').set(False)
hal.Pin('i2c-gpio.A.in-07.pullup').set(False)
hal.Pin('i2c-gpio.B.in-06.pullup').set(False)
hal.Pin('i2c-gpio.B.in-07.pullup').set(False)
# ADC
hal.Pin('i2c-temp.ch-00.value').link('hbp-temp-meas')
hal.Pin('i2c-temp.ch-01.value').link('e0-temp-meas')
hal.Pin('i2c-temp.ch-02.value').link('e1-temp-meas')
hal.Pin('i2c-temp.ch-03.value').link('e2-temp-meas')
hal.Pin('i2c-temp.ch-04.value').link('e3-temp-meas')
#hal.Pin('i2c-temp.ch-05.value').link('ain0')
#hal.Pin('i2c-temp.ch-06.value').link('ain1')
#hal.Pin('i2c-temp.ch-07.value').link('ain2')
# Stepper
hal.Pin('hpg.stepgen.00.steppin').set(812) # XStep
hal.Pin('hpg.stepgen.00.dirpin').set(811) # XDir
hal.Pin('hpg.stepgen.01.steppin').set(816) # YStep
hal.Pin('hpg.stepgen.01.dirpin').set(815) # YDir
hal.Pin('hpg.stepgen.02.steppin').set(922) # AStep - ZL
hal.Pin('hpg.stepgen.02.dirpin').set(921) # ADir
hal.Pin('hpg.stepgen.03.steppin').set(913) # ZStep - ZR
hal.Pin('hpg.stepgen.03.dirpin').set(925) # ZDir
hal.Pin('hpg.stepgen.04.steppin').set(911) # BStep
hal.Pin('hpg.stepgen.04.dirpin').set(942) # BDir
# charge pump
chargePump = rt.loadrt('charge_pump')
hal.addf('charge-pump', thread)
hal.Pin('charge-pump.out').link('charge-pump-out')
hal.Pin('charge-pump.enable').link('emcmot-0-enable')
# charge pump tied to machine power
hal.Pin('bb_gpio.p8.out-19').link('charge-pump-out')
# Monitor estop input from hardware
hal.Pin('bb_gpio.p9.in-41').link('estop-in')
hal.Pin('bb_gpio.p9.in-41.invert').set(True)
# drive estop-sw
hal.Pin('bb_gpio.p8.out-07').link('estop-out')
hal.Pin('bb_gpio.p8.out-07.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.p8.out-26').link('emcmot-0-enable')
hal.Pin('bb_gpio.p8.out-26.invert').set(True)
def setupStorage():
hal.loadusr('hal_storage',
name='storage',
file='storage.ini',
autosave=True,
wait_name='storage')
def readStorage():
hal.Pin('storage.read-trigger').set(True) # trigger read
rt.init_RTAPI()
c.load_ini('hardware.ini')
rt.loadrt('hal_bb_gpio', output_pins='915,917,838,840')
rt.loadrt('hal_arm335xQEP', encoders='eQEP0,eQEP2')
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)
kalman.pin('new-rate').link(sigNewRate)
def setupStorage():
hal.loadusr('hal_storage', name='storage', file='storage.ini',
autosave=True, wait_name='storage')
def readStorage():
hal.Pin('storage.read-trigger').set(True) # trigger read
rt.init_RTAPI()
c.load_ini('hardware.ini')
rt.loadrt('hal_bb_gpio', output_pins='915,917,838,840')
rt.loadrt('hal_arm335xQEP', encoders='eQEP0,eQEP2')
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)
