def setupGyro(thread='base_thread'): name = 'balance' sigReq = hal.newsig('%s-req' % name, hal.HAL_BIT) sigAck = hal.newsig('%s-ack' % name, hal.HAL_BIT) sigDt = hal.newsig('%s-dt' % name, hal.HAL_FLOAT) sigNewAngle = hal.newsig('%s-new-angle' % name, hal.HAL_FLOAT) sigNewRate = hal.newsig('%s-new-rate' % name, hal.HAL_FLOAT) gyroaccel = hal.loadusr('./hal_gyroaccel', name='gyroaccel', bus_id=1, interval=0.05, wait_name='gyroaccel') gyroaccel.pin('req').link(sigReq) gyroaccel.pin('ack').link(sigAck) gyroaccel.pin('dt').link(sigDt) gyroaccel.pin('angle').link(sigNewAngle) gyroaccel.pin('rate').link(sigNewRate) gyroaccel.pin('invert').set(True) # invert the output since we mounted the gyro upside down kalman = rt.loadrt('kalman', 'names=kalman') hal.addf(kalman.name, thread) kalman.pin('req').link(sigReq) kalman.pin('ack').link(sigAck) kalman.pin('dt').link(sigDt) kalman.pin('new-angle').link(sigNewAngle) kalman.pin('new-rate').link(sigNewRate)
def setup_fan(name, thread): setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT) pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT) enable = hal.newsig('%s-enable' % name, hal.HAL_BIT) # reset fan when estop is cleared reset = rt.newinst('reset', 'reset.%s-set' % name) hal.addf(reset.name, thread) reset.pin('reset-float').set(0.0) reset.pin('out-float').link(setSig) reset.pin('rising').set(True) reset.pin('falling').set(False) reset.pin('trigger').link('estop-reset') scale = rt.newinst('scale', 'scale.%s' % name) hal.addf(scale.name, thread) scale.pin('in').link(setSig) scale.pin('out').link(pwmSig) scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion setSig.set(0.0) enable.set(True) rcomps.create_fan_rcomp(name) motion.setup_fan_io(name)
def 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 setup_fans(replicape): if NUM_FANS == 0: return en = config.find('FDM','SYSTEM_FAN', 0) fan_out = [None] * NUM_FANS fan_scale = [None] * NUM_FANS fan_in = [None] * NUM_FANS #on-board pwm input is 0.0 to 1.0, M106 sends 0 to 255 for i in xrange(NUM_FANS): fan_out[i] = hal.newsig('fan-output-%d' % i, hal.HAL_FLOAT) replicape.get_fan_pwm_pin(i).link(fan_out[i]) # the system fan is connected to the last fan pwm output if (int(en) > 0) and (i == NUM_FANS-1): fan_out[i].set(1.0) else: fan_in[i] = hal.newsig('fan-input-%d' % i, hal.HAL_FLOAT) fan_in[i].link('motion.analog-out-io-%d' % (FAN_IO_START + i)) fan_scale[i] = rtapi.newinst('div2', 'fan%d.div2.scale-pwm' % i) hal.addf(fan_scale[i].name, SERVO_THREAD) fan_scale[i].pin('in0').link(fan_in[i]) fan_scale[i].pin('in1').set(255.0) fan_scale[i].pin('out').link(fan_out[i]) comp = hal.RemoteComponent('fdm-f%d' % i, timer=100) comp.newpin('set',hal.HAL_FLOAT, hal.HAL_IO) comp.ready() comp.pin('set').link(fan_in[i])
def test_newsig(): l.log() hal.newsig("floatsig1", hal.HAL_FLOAT) try: hal.newsig("floatsig1", hal.HAL_FLOAT) # RuntimeError: Failed to create signal floatsig1: HAL: ERROR: duplicate signal 'floatsig1' raise "should not happen" except RuntimeError: pass try: hal.newsig(32423 * 32432, hal.HAL_FLOAT) raise "should not happen" except TypeError: pass try: hal.newsig(None, hal.HAL_FLOAT) raise "should not happen" except TypeError: pass try: hal.newsig("badtype", 1234) raise "should not happen" except TypeError: pass
def setup_fan(name, thread): setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT) pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT) enable = hal.newsig('%s-enable' % name, hal.HAL_BIT) # reset fan when estop is cleared reset = rt.newinst('reset', 'reset.%s-set' % name) hal.addf(reset.name, thread) reset.pin('reset-float').set(0.0) reset.pin('out-float').link(setSig) reset.pin('rising').set(True) reset.pin('falling').set(False) reset.pin('trigger').link('estop-reset') scale = rt.newinst('scale', 'scale.%s' % name) hal.addf(scale.name, thread) scale.pin('in').link(setSig) scale.pin('out').link(pwmSig) scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion setSig.set(0.0) enable.set(True) rcomps.create_fan_rcomp(name) motion.setup_fan_io(name)
def test_newsig(): l.log() hal.newsig("floatsig1", hal.HAL_FLOAT) try: hal.newsig("floatsig1", hal.HAL_FLOAT) # RuntimeError: Failed to create signal floatsig1: HAL: ERROR: duplicate signal 'floatsig1' raise "should not happen" except RuntimeError: pass try: hal.newsig(32423 * 32432, hal.HAL_FLOAT) raise "should not happen" except TypeError: pass try: hal.newsig(None, hal.HAL_FLOAT) raise "should not happen" except TypeError: pass try: hal.newsig("badtype", 1234) raise "should not happen" except TypeError: pass
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 test_net_existing_signal_with_bad_type(): hal.newsig("f", hal.HAL_FLOAT) try: hal.net("f", "c1.s32out") raise "should not happen" except TypeError: pass del hal.signals["f"]
def test_net_existing_signal_with_bad_type(): hal.newsig("f", hal.HAL_FLOAT) try: hal.net("f", "c1.s32out") raise "should not happen" except TypeError: pass del hal.signals["f"]
def setup_light(name, thread): for color in ('r', 'g', 'b', 'w'): inSig = hal.newsig('%s-%s' % (name, color), hal.HAL_FLOAT) outSig = hal.newsig('%s-%s-out' % (name, color), hal.HAL_FLOAT) ledDim = rt.newinst('led_dim', 'led-dim.%s-%s' % (name, color)) hal.addf(ledDim.name, thread) ledDim.pin('in').link(inSig) ledDim.pin('out').link(outSig) rcomps.create_light_rcomp(name) storage.setup_light_storage(name) motion.setup_light_io(name)
def setup_light(name, thread): for color in ('r', 'g', 'b', 'w'): inSig = hal.newsig('%s-%s' % (name, color), hal.HAL_FLOAT) outSig = hal.newsig('%s-%s-out' % (name, color), hal.HAL_FLOAT) ledDim = rt.newinst('led_dim', 'led-dim.%s-%s' % (name, color)) hal.addf(ledDim.name, thread) ledDim.pin('in').link(inSig) ledDim.pin('out').link(outSig) rcomps.create_light_rcomp(name) storage.setup_light_storage(name) motion.setup_light_io(name)
def setup_probe(thread): probeEnable = hal.newsig('probe-enable', hal.HAL_BIT) probeInput = hal.newsig('probe-input', hal.HAL_BIT) probeSignal = hal.newsig('probe-signal', hal.HAL_BIT) and2 = rt.newinst('and2', 'and2.probe-input') hal.addf(and2.name, thread) and2.pin('in0').link(probeSignal) and2.pin('in1').link(probeEnable) and2.pin('out').link(probeInput) probeInput += 'motion.probe-input' motion.setup_probe_io()
def setup_probe(thread): probeEnable = hal.newsig('probe-enable', hal.HAL_BIT) probeInput = hal.newsig('probe-input', hal.HAL_BIT) probeSignal = hal.newsig('probe-signal', hal.HAL_BIT) and2 = rt.newinst('and2', 'and2.probe-input') hal.addf(and2.name, thread) and2.pin('in0').link(probeSignal) and2.pin('in1').link(probeEnable) and2.pin('out').link(probeInput) probeInput += 'motion.probe-input' motion.setup_probe_io()
def test_net_existing_signal(): hal.newsig("s32", hal.HAL_S32) assert hal.pins["c1.s32out"].linked == False hal.net("s32", "c1.s32out") assert hal.pins["c1.s32out"].linked == True hal.newsig("s32too", hal.HAL_S32) try: hal.net("s32too", "c1.s32out") raise "should not happen" except RuntimeError: pass del hal.signals["s32"]
def test_net_existing_signal(): hal.newsig("s32", hal.HAL_S32) assert hal.pins["c1.s32out"].linked == False hal.net("s32", "c1.s32out") assert hal.pins["c1.s32out"].linked == True hal.newsig("s32too", hal.HAL_S32) try: hal.net("s32too", "c1.s32out") raise "should not happen" except RuntimeError: pass del hal.signals["s32"]
def _setup_usrcomp_watchdog(comps, thread): power_on = hal.Signal('power-on', hal.HAL_BIT) watchdog_ok = hal.Signal('watchdog-ok', hal.HAL_BIT) watchdog_error_raw = hal.Signal('watchdog-error-raw', hal.HAL_BIT) watchdog_error = hal.Signal('watchdog-error', hal.HAL_BIT) watchdog = rt.newinst('watchdog', 'watchdog.usrcomp', pincount=len(comps)) hal.addf('{}.set-timeouts'.format(watchdog.name), thread.name) hal.addf('{}.process'.format(watchdog.name), thread.name) for n, comp in enumerate(comps): sig_in = hal.newsig('{}-watchdog'.format(comp.name), hal.HAL_BIT) hal.Pin('{}.watchdog'.format(comp.name)).link(sig_in) watchdog.pin('input-{:02}'.format(n)).link(sig_in) watchdog.pin('timeout-{:02}'.format(n)).set(comp.timeout) watchdog.pin('enable-in').link(power_on) watchdog.pin('ok-out').link(watchdog_ok) not_comp = rt.newinst('not', 'not.watchdog-error') hal.addf(not_comp.name, thread.name) not_comp.pin('in').link(watchdog_ok) not_comp.pin('out').link(watchdog_error_raw) and2 = rt.newinst('and2', 'and2.watchdog-error') hal.addf(and2.name, thread.name) and2.pin('in0').link(watchdog_error_raw) and2.pin('in1').link(power_on) and2.pin('out').link(watchdog_error)
def setup_fan(name, thread): setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT) pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT) enable = hal.newsig('%s-enable' % name, hal.HAL_BIT) scale = rt.newinst('scale', 'scale.%s' % name) hal.addf(scale.name, thread) scale.pin('in').link(setSig) scale.pin('out').link(pwmSig) scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion setSig.set(0.0) enable.set(True) rcomps.create_fan_rcomp(name) motion.setup_fan_io(name)
def setup_fan(name, thread): setSig = hal.newsig('%s-set' % name, hal.HAL_FLOAT) pwmSig = hal.newsig('%s-pwm' % name, hal.HAL_FLOAT) enable = hal.newsig('%s-enable' % name, hal.HAL_BIT) scale = rt.newinst('scale', 'scale.%s' % name) hal.addf(scale.name, thread) scale.pin('in').link(setSig) scale.pin('out').link(pwmSig) scale.pin('gain').set(1.0 / 255.0) # 255 steps from motion setSig.set(0.0) enable.set(True) rcomps.create_fan_rcomp(name) motion.setup_fan_io(name)
def __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 test_net_existing_signal(self): l.log() hal.newsig("s32", hal.HAL_S32) assert hal.pins["c1.s32out"].linked is False hal.net("s32", "c1.s32out") assert hal.pins["c1.s32out"].linked is True hal.newsig("s32too", hal.HAL_S32) try: hal.net("s32too", "c1.s32out") raise "should not happen" except RuntimeError: pass del hal.signals["s32"] assert 's32' not in hal.signals
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 setup_system_fan(replicape): en = config.find('FDM','SYSTEM_FAN', 0) if int(en) == 0: return fan_sig = hal.newsig('fan-output', hal.HAL_FLOAT) replicape.get_fan_pwm_pin(3).link(fan_sig) fan_sig.set(1.0)
def setup_system_fan(replicape): en = config.find('FDM','SYSTEM_FAN', 0) if int(en) == 0: return fan_sig = hal.newsig('fan-output', hal.HAL_FLOAT) replicape.get_fan_pwm_pin(3).link(fan_sig) fan_sig.set(1.0)
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 usrcomp_status(compname, signame, thread, resetSignal='estop-reset'): sigIn = hal.newsig('%s-error-in' % signame, hal.HAL_BIT) sigOut = hal.newsig('%s-error' % signame, hal.HAL_BIT) sigOk = hal.newsig('%s-ok' % signame, hal.HAL_BIT) sigIn.link('%s.error' % compname) safetyLatch = rt.newinst('safety_latch', 'safety-latch.%s-error' % signame) hal.addf(safetyLatch.name, thread) safetyLatch.pin('error-in').link(sigIn) safetyLatch.pin('error-out').link(sigOut) safetyLatch.pin('reset').link(resetSignal) safetyLatch.pin('threshold').set(500) # 500ms error safetyLatch.pin('latching').set(True) notComp = rt.newinst('not', 'not.%s-no-error' % signame) hal.addf(notComp.name, thread) notComp.pin('in').link(sigOut) notComp.pin('out').link(sigOk)
def setup_servo_toggle(servoIndex, section, pwm, selectSignal, thread=None): servo = '%s.%02i' % ('rc_servo', servoIndex) mux2 = rt.newinst('mux2', '%s-mux2' % servo) hal.addf(mux2.name, thread) pwmout = hal.newsig('%s-pwm-out' % servo, hal.HAL_FLOAT) mux2.pin('out').link(pwmout) hal.Pin('%s.value' % pwm).link(pwmout) enable = hal.Signal('emcmot-0-enable', hal.HAL_BIT) hal.Pin('%s.enable' % pwm).link(enable) sigBase = 'rc-servo-%i' % servoIndex smin = hal.newsig('%s-min' % sigBase, hal.HAL_FLOAT) smax = hal.newsig('%s-max' % sigBase, hal.HAL_FLOAT) hal.Pin('%s-mux2.in1' % servo).link(smin) hal.Pin('%s-mux2.in0' % servo).link(smax) smin.set(c.find(section, 'SERVO_MIN', 0.1)) smax.set(c.find(section, 'SERVO_MAX', 0.2)) mux2.pin('sel').link('%s' % selectSignal)
def 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 usrcomp_status(compname, signame, thread, resetSignal='estop-reset'): sigIn = hal.newsig('%s-error-in' % signame, hal.HAL_BIT) sigOut = hal.newsig('%s-error' % signame, hal.HAL_BIT) sigOk = hal.newsig('%s-ok' % signame, hal.HAL_BIT) sigIn.link('%s.error' % compname) safetyLatch = rt.newinst('safety_latch', 'safety-latch.%s-error' % signame) hal.addf(safetyLatch.name, thread) safetyLatch.pin('error-in').link(sigIn) safetyLatch.pin('error-out').link(sigOut) safetyLatch.pin('reset').link(resetSignal) safetyLatch.pin('threshold').set(500) # 500ms error safetyLatch.pin('latching').set(True) notComp = rt.newinst('not', 'not.%s-no-error' % signame) hal.addf(notComp.name, thread) notComp.pin('in').link(sigOut) notComp.pin('out').link(sigOk)
def setup_hbp_led(thread): tempMeas = hal.Signal('hbp-temp-meas') ledHbpHot = hal.newsig('led-hbp-hot', hal.HAL_BIT) ledHbpInfo = hal.newsig('led-hbp-info', hal.HAL_BIT) # low temp comp = rt.newinst('comp', 'comp.hbp-info') hal.addf(comp.name, thread) comp.pin('in0').link(tempMeas) comp.pin('in1').set(50.0) comp.pin('hyst').set(2.0) comp.pin('out').link(ledHbpInfo) # high temp comp = rt.newinst('comp', 'comp.hbp-hot') hal.addf(comp.name, thread) comp.pin('in0').set(50.0) comp.pin('in1').link(tempMeas) comp.pin('hyst').set(2.0) comp.pin('out').link(ledHbpHot)
def setup_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_fans(replicape): if NUM_FANS == 0: return en = config.find('FDM','SYSTEM_FAN', 0) fan_out = [None] * NUM_FANS fan_scale = [None] * NUM_FANS fan_in = [None] * NUM_FANS #on-board pwm input is 0.0 to 1.0, M106 sends 0 to 255 for i in xrange(NUM_FANS): fan_out[i] = hal.newsig('fan-out-%d' % i, hal.HAL_FLOAT) replicape.get_fan_pwm_pin(i).link(fan_out[i]) # the system fan is connected to the last fan pwm output if (int(en) > 0) and (i == NUM_FANS-1): fan_out[i].set(1.0) else: fan_in[i] = hal.newsig('fan-in-%d' % i, hal.HAL_FLOAT) fan_in[i].link('motion.analog-out-io-%d' % (FAN_IO_START + i)) fan_scale[i] = rtapi.newinst('div2', 'fan%d.div2.scale-pwm' % i) hal.addf(fan_scale[i].name, SERVO_THREAD) fan_scale[i].pin('in0').link(fan_in[i]) fan_scale[i].pin('in1').set(255.0) fan_scale[i].pin('out').link(fan_out[i])
def 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 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 setup_enable_chain(): """ Create enable and enable_inv signal for the system """ main_enable = hal.net('main-enable', 'axis.0.amp-enable-out') n = rtapi.newinst('not', 'main-enable.not') hal.addf(n.name, SERVO_THREAD) main_enable_inv = hal.newsig('main-enable-inv', hal.HAL_BIT) n.pin('in').link('main-enable') n.pin('out').link('main-enable-inv') return (main_enable, main_enable_inv)
def setup_enable_chain(): """ Create enable and enable_inv signal for the system """ main_enable = hal.net('main-enable', 'axis.0.amp-enable-out') n = rtapi.newinst('not', 'main-enable.not') hal.addf(n.name, SERVO_THREAD) main_enable_inv = hal.newsig('main-enable-inv', hal.HAL_BIT) n.pin('in').link('main-enable') n.pin('out').link('main-enable-inv') return (main_enable, main_enable_inv)
def setup_limit_switches(replicape): limit_x_sig = hal.newsig('limit-x', hal.HAL_BIT) limit_x_sig.link('axis.0.home-sw-in') limit_x_pos = config.find('AXIS_0', 'HOME_SEARCH_VEL', 0) if limit_x_pos < 0: replicape.get_limit_pin('X', False).link(limit_x_sig) limit_x_sig.link('axis.0.neg-lim-sw-in') if limit_x_pos > 0: replicape.get_limit_pin('X', True).link(limit_x_sig) limit_x_sig.link('axis.0.pos-lim-sw-in') limit_y_sig = hal.newsig('limit-y', hal.HAL_BIT) limit_y_sig.link('axis.1.home-sw-in') limit_y_pos = config.find('AXIS_1', 'HOME_SEARCH_VEL', 0) if limit_y_pos < 0: replicape.get_limit_pin('Y', False).link(limit_y_sig) limit_y_sig.link('axis.1.neg-lim-sw-in') if limit_y_pos > 0: replicape.get_limit_pin('Y', True).link(limit_y_sig) limit_y_sig.link('axis.1.pos-lim-sw-in') limit_z_sig = hal.newsig('limit-z', hal.HAL_BIT) limit_z_sig.link('axis.2.home-sw-in') limit_z_pos = config.find('AXIS_2', 'HOME_SEARCH_VEL', 0) if limit_z_pos < 0: replicape.get_limit_pin('Z', False).link(limit_z_sig) limit_z_sig.link('axis.2.neg-lim-sw-in') if limit_z_pos > 0: replicape.get_limit_pin('Z', True).link(limit_z_sig) limit_z_sig.link('axis.2.pos-lim-sw-in') probe_pin = replicape.get_probe_pin(); if probe_pin is not None: probe_sig = hal.newsig('limit-probe', hal.HAL_BIT) probe_sig.link(probe_pin) probe_sig.link('motion.probe-input')
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_limit_switches(replicape): limit_x_sig = hal.newsig('limit-x', hal.HAL_BIT) limit_x_sig.link('axis.0.home-sw-in') limit_x_pos = config.find('AXIS_0', 'HOME_SEARCH_VEL', 0) if limit_x_pos < 0: replicape.get_limit_pin('X', False).link(limit_x_sig) limit_x_sig.link('axis.0.neg-lim-sw-in') if limit_x_pos > 0: replicape.get_limit_pin('X', True).link(limit_x_sig) limit_x_sig.link('axis.0.pos-lim-sw-in') limit_y_sig = hal.newsig('limit-y', hal.HAL_BIT) limit_y_sig.link('axis.1.home-sw-in') limit_y_pos = config.find('AXIS_1', 'HOME_SEARCH_VEL', 0) if limit_y_pos < 0: replicape.get_limit_pin('Y', False).link(limit_y_sig) limit_y_sig.link('axis.1.neg-lim-sw-in') if limit_y_pos > 0: replicape.get_limit_pin('Y', True).link(limit_y_sig) limit_y_sig.link('axis.1.pos-lim-sw-in') limit_z_sig = hal.newsig('limit-z', hal.HAL_BIT) limit_z_sig.link('axis.2.home-sw-in') limit_z_pos = config.find('AXIS_2', 'HOME_SEARCH_VEL', 0) if limit_z_pos < 0: replicape.get_limit_pin('Z', False).link(limit_z_sig) limit_z_sig.link('axis.2.neg-lim-sw-in') if limit_z_pos > 0: replicape.get_limit_pin('Z', True).link(limit_z_sig) limit_z_sig.link('axis.2.pos-lim-sw-in') probe_pin = replicape.get_probe_pin(); if probe_pin is not None: probe_sig = hal.newsig('limit-probe', hal.HAL_BIT) probe_sig.link(probe_pin) probe_sig.link('motion.probe-input')
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 demo_subscriber(compname, prefix, topic, count=6): ''' retrieve msg type of <topic> subscribe to <topic> create pins <compname>.<prefix>.fieldname of appropriate type update pin values accordingly ''' global pins msgtype = get_topic_type(topic) pins = gen_halcomp(compname, prefix, msgtype, hal.HAL_OUT, count) # create signal from joint_states.robot.position.0 to hm2_5i20.0.stepgen.00.position-cmd # net joint0 joint_states.robot.position.0 hm2_5i20.0.stepgen.00.position-cmd for i in range(0, 2): sig_joint = hal.newsig('sig-joint%s' % i, hal.HAL_FLOAT) sig_joint.link('joint_states.robot.position.%s' % i) sig_joint.link('hm2_5i20.0.stepgen.0%s.position-cmd' % i) rospy.init_node(compname, anonymous=True) rospy.Subscriber(topic, msgtype, callback=demo_callback, callback_args=pins) rospy.spin()
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 _init_signals(self): # Input signal self.enable_sig = hal.newsig('ve%d.enable' % self.nr, hal.HAL_BIT) self.cross_section_sig = hal.newsig('ve%d.cross-section' % self.nr, hal.HAL_FLOAT) self.jog_vel_sig = hal.newsig('ve%d.jog-vel' % self.nr, hal.HAL_FLOAT) self.jog_direction_sig = hal.newsig('ve%d.jog-direction' % self.nr, hal.HAL_BIT) self.jog_distance_sig = hal.newsig('ve%d.jog-distance' % self.nr, hal.HAL_FLOAT) self.jog_trigger_sig = hal.newsig('ve%d.jog-trigger' % self.nr, hal.HAL_BIT) self.jog_continuous_sig = hal.newsig('ve%d.jog-continuous' % self.nr, hal.HAL_BIT) self.jog_dtg_sig = hal.newsig('ve%d.jog-dtg' % self.nr, hal.HAL_FLOAT) # Intermediate signal self.jog_vel_output_sig = hal.newsig('ve%d.jog-vel-output' % self.nr, hal.HAL_FLOAT) # Parameter signal self.filament_dia_sig = hal.newsig('ve%d.filament-dia' % self.nr, hal.HAL_FLOAT) self.extrude_scale_sig = hal.newsig('ve%d.extrude-scale' % self.nr, hal.HAL_FLOAT) self.accel_gain_sig = hal.newsig('ve%d.accel-gain' % self.nr, hal.HAL_FLOAT) self.max_jog_vel_sig = hal.newsig('ve%d.max-jog-vel' % self.nr, hal.HAL_FLOAT) self.retract_vel_sig = hal.newsig('ve%d.retract-vel' % self.nr, hal.HAL_FLOAT) self.retract_len_sig = hal.newsig('ve%d.retract-len' % self.nr, hal.HAL_FLOAT) # Output signal self.extrude_vel_sig = hal.newsig('ve%d.extrude-vel' % self.nr, hal.HAL_FLOAT) self.retracting_sig = hal.newsig('ve%d.retracting' % self.nr, hal.HAL_BIT)
def setup_stepper(stepgenIndex, section, axisIndex=None, stepgenType='hpg.stepgen', gantry=False, gantryJoint=0, velocitySignal=None, thread=None): stepgen = '%s.%02i' % (stepgenType, stepgenIndex) if axisIndex is not None: axis = 'axis.%i' % axisIndex hasMotionAxis = (axisIndex is not None) and (not gantry or gantryJoint == 0) velocityControlled = velocitySignal is not None # axis enable chain enableIndex = axisIndex if axisIndex is None: enableIndex = 0 # use motor enable signal enable = hal.Signal('emcmot-%i-enable' % enableIndex, hal.HAL_BIT) if hasMotionAxis: enable.link('%s.amp-enable-out' % axis) enable.link('%s.enable' % stepgen) # expose timing parameters so we can multiplex them later sigBase = 'stepgen-%i' % stepgenIndex dirsetup = hal.newsig('%s-dirsetup' % sigBase, hal.HAL_U32) dirhold = hal.newsig('%s-dirhold' % sigBase, hal.HAL_U32) steplen = hal.newsig('%s-steplen' % sigBase, hal.HAL_U32) stepspace = hal.newsig('%s-stepspace' % sigBase, hal.HAL_U32) scale = hal.newsig('%s-scale' % sigBase, hal.HAL_FLOAT) maxVel = hal.newsig('%s-max-vel' % sigBase, hal.HAL_FLOAT) maxAcc = hal.newsig('%s-max-acc' % sigBase, hal.HAL_FLOAT) controlType = hal.newsig('%s-control-type' % sigBase, hal.HAL_BIT) hal.Pin('%s.dirsetup' % stepgen).link(dirsetup) hal.Pin('%s.dirhold' % stepgen).link(dirhold) dirsetup.set(c.find(section, 'DIRSETUP')) dirhold.set(c.find(section, 'DIRHOLD')) hal.Pin('%s.steplen' % stepgen).link(steplen) hal.Pin('%s.stepspace' % stepgen).link(stepspace) steplen.set(c.find(section, 'STEPLEN')) stepspace.set(c.find(section, 'STEPSPACE')) hal.Pin('%s.position-scale' % stepgen).link(scale) scale.set(c.find(section, 'SCALE')) hal.Pin('%s.maxvel' % stepgen).link(maxVel) hal.Pin('%s.maxaccel' % stepgen).link(maxAcc) maxVel.set(c.find(section, 'STEPGEN_MAX_VEL')) maxAcc.set(c.find(section, 'STEPGEN_MAX_ACC')) hal.Pin('%s.control-type' % stepgen).link(controlType) # position command and feedback if not velocityControlled: if hasMotionAxis: # per axis fb and cmd posCmd = hal.newsig('emcmot-%i-pos-cmd' % axisIndex, hal.HAL_FLOAT) posCmd.link('%s.motor-pos-cmd' % axis) if not gantry: posCmd.link('%s.position-cmd' % stepgen) else: posCmd.link('gantry.%i.position-cmd' % axisIndex) posFb = hal.newsig('emcmot-%i-pos-fb' % axisIndex, hal.HAL_FLOAT) posFb.link('%s.motor-pos-fb' % axis) if not gantry: posFb.link('%s.position-fb' % stepgen) else: posFb.link('gantry.%i.position-fb' % axisIndex) if gantry: # per joint fb and cmd posCmd = hal.newsig('emcmot-%i-%i-pos-cmd' % (axisIndex, gantryJoint), hal.HAL_FLOAT) posCmd.link('gantry.%i.joint.%02i.pos-cmd' % (axisIndex, gantryJoint)) posCmd.link('%s.position-cmd' % stepgen) posFb = hal.newsig('emcmot-%i-%i-pos-fb' % (axisIndex, gantryJoint), hal.HAL_FLOAT) posFb.link('%s.position-fb' % stepgen) posFb.link('gantry.%i.joint.%02i.pos-fb' % (axisIndex, gantryJoint)) else: # velocity control hal.net(velocitySignal, '%s.velocity-cmd' % stepgen) controlType.set(1) # enable velocity control # limits if hasMotionAxis: limitHome = hal.newsig('limit-%i-home' % axisIndex, hal.HAL_BIT) limitMin = hal.newsig('limit-%i-min' % axisIndex, hal.HAL_BIT) limitMax = hal.newsig('limit-%i-max' % axisIndex, hal.HAL_BIT) limitHome.link('%s.home-sw-in' % axis) limitMin.link('%s.neg-lim-sw-in' % axis) limitMax.link('%s.pos-lim-sw-in' % axis) if gantry: if gantryJoint == 0: axisHoming = hal.newsig('emcmot-%i-homing' % axisIndex, hal.HAL_BIT) axisHoming.link('%s.homing' % axis) hal.Pin('gantry.%i.search-vel' % axisIndex).set(c.find(section, 'HOME_SEARCH_VEL')) hal.Pin('gantry.%i.homing' % axisIndex).link(axisHoming) hal.Pin('gantry.%i.home' % axisIndex).link(limitHome) or2 = rt.newinst('or2', 'or2.limit-%i-min' % axisIndex) hal.addf(or2.name, thread) or2.pin('out').link(limitMin) or2 = rt.newinst('or2', 'or2.limit-%i-max' % axisIndex) hal.addf(or2.name, thread) or2.pin('out').link(limitMax) limitHome = hal.newsig('limit-%i-%i-home' % (axisIndex, gantryJoint), hal.HAL_BIT) limitMin = hal.newsig('limit-%i-%i-min' % (axisIndex, gantryJoint), hal.HAL_BIT) limitMax = hal.newsig('limit-%i-%i-max' % (axisIndex, gantryJoint), hal.HAL_BIT) homeOffset = hal.Signal('home-offset-%i-%i' % (axisIndex, gantryJoint), hal.HAL_FLOAT) limitHome.link('gantry.%i.joint.%02i.home' % (axisIndex, gantryJoint)) limitMin.link('or2.limit-%i-min.in%i' % (axisIndex, gantryJoint)) limitMax.link('or2.limit-%i-max.in%i' % (axisIndex, gantryJoint)) homeOffset.link('gantry.%i.joint.%02i.home-offset' % (axisIndex, gantryJoint)) storage.setup_gantry_storage(axisIndex, gantryJoint)
def _init_extrude(self, motion_vel_sig): material_flowrate_sig = hal.newsig('ve%d.material-flowrate' % self.nr, hal.HAL_FLOAT) material_flowrate = rtapi.newinst('mult2', 've%d.mult2.material-flowrate' % self.nr) hal.addf(material_flowrate.name, SERVO_THREAD) material_flowrate.pin('in0').link(motion_vel_sig) material_flowrate.pin('in1').link(self.cross_section_sig) material_flowrate.pin('out').link(material_flowrate_sig) filament_dia_sq_sig = hal.newsig('ve%d.filament-dia-sq' % self.nr, hal.HAL_FLOAT) filament_dia_sq = rtapi.newinst('mult2', 've%d.mult2.filament-dia-sq' % self.nr) hal.addf(filament_dia_sq.name, SERVO_THREAD) filament_dia_sq.pin('in0').link(self.filament_dia_sig) filament_dia_sq.pin('in1').link(self.filament_dia_sig) filament_dia_sq.pin('out').link(filament_dia_sq_sig) filament_area_sig = hal.newsig('ve%d.filament-area' % self.nr, hal.HAL_FLOAT) filament_area = rtapi.newinst('mult2', 've%d.mult2.filament-area' % self.nr) hal.addf(filament_area.name, SERVO_THREAD) filament_area.pin('in0').link(filament_dia_sq_sig) filament_area.pin('in1').set(math.pi / 4) filament_area.pin('out').link(filament_area_sig) extrude_rate_sig = hal.newsig('ve%d.extrude-rate' % self.nr, hal.HAL_FLOAT) extrude_rate = rtapi.newinst('div2', 've%d.div2.extrude-rate' % self.nr) hal.addf(extrude_rate.name, SERVO_THREAD) extrude_rate.pin('in0').link(material_flowrate_sig) extrude_rate.pin('in1').link(filament_area_sig) extrude_rate.pin('out').link(extrude_rate_sig) extrude_rate_scaled_sig = hal.newsig('ve%d.extrude-rate-scaled' % self.nr, hal.HAL_FLOAT) extrude_rate_scaled = rtapi.newinst('mult2', 've%d.mult2.extrude-rate-scaled' % self.nr) hal.addf(extrude_rate_scaled.name, SERVO_THREAD) extrude_rate_scaled.pin('in0').link(extrude_rate_sig) extrude_rate_scaled.pin('in1').link(self.extrude_scale_sig) extrude_rate_scaled.pin('out').link(extrude_rate_scaled_sig) extrude_accel_sig = hal.newsig('ve%d.extrude-accel' % self.nr, hal.HAL_FLOAT) extrude_accel = rtapi.newinst('ddt', 've%d.ddt.extrude-accel' % self.nr) hal.addf(extrude_accel.name, SERVO_THREAD) extrude_accel.pin('in').link(extrude_rate_scaled_sig) extrude_accel.pin('out').link(extrude_accel_sig) extrude_accel_adj_sig = hal.newsig('ve%d.extrude-accel-adj' % self.nr, hal.HAL_FLOAT) extrude_accel_adj = rtapi.newinst('mult2', 've%d.mult2.extrude-accel-adj' % self.nr) hal.addf(extrude_accel_adj.name, SERVO_THREAD) extrude_accel_adj.pin('in0').link(extrude_accel_sig) extrude_accel_adj.pin('in1').link(self.accel_gain_sig) extrude_accel_adj.pin('out').link(extrude_accel_adj_sig) extrude_rate_comp_sig = hal.newsig('ve%d.extrude-rate-comp' % self.nr, hal.HAL_FLOAT) extrude_rate_comp = rtapi.newinst('sum2', 've%d.sum2.extrude-rate-comp' % self.nr) hal.addf(extrude_rate_comp.name, SERVO_THREAD) extrude_rate_comp.pin('in0').link(extrude_rate_scaled_sig) extrude_rate_comp.pin('in1').link(extrude_accel_adj_sig) extrude_rate_comp.pin('out').link(extrude_rate_comp_sig) retract_vel_neg_sig = hal.newsig('ve%d.retract-vel-neg' % self.nr, hal.HAL_FLOAT) retract_time_sig = hal.newsig('ve%d.retract-time' % self.nr, hal.HAL_FLOAT) retract_vel_neg = rtapi.newinst('neg', 've%d.neg.retract-vel-neg' % self.nr) hal.addf(retract_vel_neg.name, SERVO_THREAD) retract_vel_neg.pin('in').link(self.retract_vel_sig) retract_vel_neg.pin('out').link(retract_vel_neg_sig) retract_time = rtapi.newinst('div2', 've%d.div2.retract-time' % self.nr) hal.addf(retract_time.name, SERVO_THREAD) retract_time.pin('in0').link(self.retract_len_sig) retract_time.pin('in1').link(self.retract_vel_sig) retract_time.pin('out').link(retract_time_sig) retracting_delay = rtapi.newinst('oneshot', 've%d.oneshot.retracting' % self.nr) hal.addf(retracting_delay.name, SERVO_THREAD) retracting_delay.pin('rising').set(True) retracting_delay.pin('falling').set(True) retracting_delay.pin('retriggerable').set(True) retracting_delay.pin('width').link(retract_time_sig) retracting_delay.pin('in').link(self.enable_sig) retracting_delay.pin('out').link(self.retracting_sig) extrude_vel = rtapi.newinst('mux4', 've%d.mux4.extrude-vel' % self.nr) hal.addf(extrude_vel.name, SERVO_THREAD) extrude_vel.pin('sel0').link(self.retracting_sig) extrude_vel.pin('sel1').link(self.enable_sig) extrude_vel.pin('in0').link(self.jog_vel_output_sig) extrude_vel.pin('in1').link(retract_vel_neg_sig) extrude_vel.pin('in2').link(extrude_rate_comp_sig) extrude_vel.pin('in3').link(self.retract_vel_sig) extrude_vel.pin('out').link(self.extrude_vel_sig)
def setup_stepper(stepgenIndex, section, axisIndex=None, stepgenType='hpg.stepgen', gantry=False, gantryJoint=0, velocitySignal=None, thread=None): stepgen = '%s.%02i' % (stepgenType, stepgenIndex) if axisIndex is not None: axis = 'axis.%i' % axisIndex hasMotionAxis = (axisIndex is not None) and (not gantry or gantryJoint == 0) velocityControlled = velocitySignal is not None # axis enable chain enableIndex = axisIndex if axisIndex is None: enableIndex = 0 # use motor enable signal enable = hal.Signal('emcmot-%i-enable' % enableIndex, hal.HAL_BIT) if hasMotionAxis: enable.link('%s.amp-enable-out' % axis) enable.link('%s.enable' % stepgen) # expose timing parameters so we can multiplex them later sigBase = 'stepgen-%i' % stepgenIndex dirsetup = hal.newsig('%s-dirsetup' % sigBase, hal.HAL_U32) dirhold = hal.newsig('%s-dirhold' % sigBase, hal.HAL_U32) steplen = hal.newsig('%s-steplen' % sigBase, hal.HAL_U32) stepspace = hal.newsig('%s-stepspace' % sigBase, hal.HAL_U32) scale = hal.newsig('%s-scale' % sigBase, hal.HAL_FLOAT) maxVel = hal.newsig('%s-max-vel' % sigBase, hal.HAL_FLOAT) maxAcc = hal.newsig('%s-max-acc' % sigBase, hal.HAL_FLOAT) controlType = hal.newsig('%s-control-type' % sigBase, hal.HAL_BIT) hal.Pin('%s.dirsetup' % stepgen).link(dirsetup) hal.Pin('%s.dirhold' % stepgen).link(dirhold) dirsetup.set(c.find(section, 'DIRSETUP')) dirhold.set(c.find(section, 'DIRHOLD')) hal.Pin('%s.steplen' % stepgen).link(steplen) hal.Pin('%s.stepspace' % stepgen).link(stepspace) steplen.set(c.find(section, 'STEPLEN')) stepspace.set(c.find(section, 'STEPSPACE')) hal.Pin('%s.position-scale' % stepgen).link(scale) scale.set(c.find(section, 'SCALE')) hal.Pin('%s.maxvel' % stepgen).link(maxVel) hal.Pin('%s.maxaccel' % stepgen).link(maxAcc) maxVel.set(c.find(section, 'STEPGEN_MAX_VEL')) maxAcc.set(c.find(section, 'STEPGEN_MAX_ACC')) hal.Pin('%s.control-type' % stepgen).link(controlType) # position command and feedback if not velocityControlled: if hasMotionAxis: # per axis fb and cmd posCmd = hal.newsig('emcmot-%i-pos-cmd' % axisIndex, hal.HAL_FLOAT) posCmd.link('%s.motor-pos-cmd' % axis) if not gantry: posCmd.link('%s.position-cmd' % stepgen) else: posCmd.link('gantry.%i.position-cmd' % axisIndex) posFb = hal.newsig('emcmot-%i-pos-fb' % axisIndex, hal.HAL_FLOAT) posFb.link('%s.motor-pos-fb' % axis) if not gantry: posFb.link('%s.position-fb' % stepgen) else: posFb.link('gantry.%i.position-fb' % axisIndex) if gantry: # per joint fb and cmd posCmd = hal.newsig( 'emcmot-%i-%i-pos-cmd' % (axisIndex, gantryJoint), hal.HAL_FLOAT) posCmd.link('gantry.%i.joint.%02i.pos-cmd' % (axisIndex, gantryJoint)) posCmd.link('%s.position-cmd' % stepgen) posFb = hal.newsig( 'emcmot-%i-%i-pos-fb' % (axisIndex, gantryJoint), hal.HAL_FLOAT) posFb.link('%s.position-fb' % stepgen) posFb.link('gantry.%i.joint.%02i.pos-fb' % (axisIndex, gantryJoint)) else: # velocity control hal.net(velocitySignal, '%s.velocity-cmd' % stepgen) controlType.set(1) # enable velocity control # limits if hasMotionAxis: limitHome = hal.newsig('limit-%i-home' % axisIndex, hal.HAL_BIT) limitMin = hal.newsig('limit-%i-min' % axisIndex, hal.HAL_BIT) limitMax = hal.newsig('limit-%i-max' % axisIndex, hal.HAL_BIT) limitHome.link('%s.home-sw-in' % axis) limitMin.link('%s.neg-lim-sw-in' % axis) limitMax.link('%s.pos-lim-sw-in' % axis) if gantry: if gantryJoint == 0: axisHoming = hal.newsig('emcmot-%i-homing' % axisIndex, hal.HAL_BIT) axisHoming.link('%s.homing' % axis) hal.Pin('gantry.%i.search-vel' % axisIndex).set( c.find(section, 'HOME_SEARCH_VEL')) hal.Pin('gantry.%i.homing' % axisIndex).link(axisHoming) hal.Pin('gantry.%i.home' % axisIndex).link(limitHome) or2 = rt.newinst('or2', 'or2.limit-%i-min' % axisIndex) hal.addf(or2.name, thread) or2.pin('out').link(limitMin) or2 = rt.newinst('or2', 'or2.limit-%i-max' % axisIndex) hal.addf(or2.name, thread) or2.pin('out').link(limitMax) limitHome = hal.newsig('limit-%i-%i-home' % (axisIndex, gantryJoint), hal.HAL_BIT) limitMin = hal.newsig('limit-%i-%i-min' % (axisIndex, gantryJoint), hal.HAL_BIT) limitMax = hal.newsig('limit-%i-%i-max' % (axisIndex, gantryJoint), hal.HAL_BIT) homeOffset = hal.Signal('home-offset-%i-%i' % (axisIndex, gantryJoint), hal.HAL_FLOAT) limitHome.link('gantry.%i.joint.%02i.home' % (axisIndex, gantryJoint)) limitMin.link('or2.limit-%i-min.in%i' % (axisIndex, gantryJoint)) limitMax.link('or2.limit-%i-max.in%i' % (axisIndex, gantryJoint)) homeOffset.link('gantry.%i.joint.%02i.home-offset' % (axisIndex, gantryJoint)) storage.setup_gantry_storage(axisIndex, gantryJoint)
def create_temperature_control(name, section, thread, hardwareOkSignal=None, coolingFan=None, hotendFan=None): tempSet = hal.newsig('%s-temp-set' % name, hal.HAL_FLOAT) tempMeas = hal.newsig('%s-temp-meas' % name, hal.HAL_FLOAT) tempInRange = hal.newsig('%s-temp-in-range' % name, hal.HAL_BIT) tempPwm = hal.newsig('%s-temp-pwm' % name, hal.HAL_FLOAT) tempPwmMax = hal.newsig('%s-temp-pwm-max' % name, hal.HAL_FLOAT) tempLimitMin = hal.newsig('%s-temp-limit-min' % name, hal.HAL_FLOAT) tempLimitMax = hal.newsig('%s-temp-limit-max' % name, hal.HAL_FLOAT) tempStandby = hal.newsig('%s-temp-standby' % name, hal.HAL_FLOAT) tempInLimit = hal.newsig('%s-temp-in-limit' % name, hal.HAL_BIT) tempThermOk = hal.newsig('%s-temp-therm-ok' % name, hal.HAL_BIT) error = hal.newsig('%s-error' % name, hal.HAL_BIT) active = hal.newsig('%s-active' % name, hal.HAL_BIT) tempPidPgain = hal.newsig('%s-temp-pid-Pgain' % name, hal.HAL_FLOAT) tempPidIgain = hal.newsig('%s-temp-pid-Igain' % name, hal.HAL_FLOAT) tempPidDgain = hal.newsig('%s-temp-pid-Dgain' % name, hal.HAL_FLOAT) tempPidMaxerrorI = hal.newsig('%s-temp-pid-maxerrorI' % name, hal.HAL_FLOAT) tempPidOut = hal.newsig('%s-temp-pid-out' % name, hal.HAL_FLOAT) tempPidBias = hal.newsig('%s-temp-pid-bias' % name, hal.HAL_FLOAT) tempRangeMin = hal.newsig('%s-temp-range-min' % name, hal.HAL_FLOAT) tempRangeMax = hal.newsig('%s-temp-range-max' % name, hal.HAL_FLOAT) noErrorIn = hal.newsig('%s-no-error-in' % name, hal.HAL_BIT) errorIn = hal.newsig('%s-error-in' % name, hal.HAL_BIT) # reset set temperature when estop is cleared reset = rt.newinst('reset', 'reset.%s-temp-set' % name) hal.addf(reset.name, thread) reset.pin('reset-float').set(0.0) reset.pin('out-float').link(tempSet) reset.pin('rising').set(True) reset.pin('falling').set(False) reset.pin('trigger').link('estop-reset') tempPidBiasOut = tempPidBias # coolingFan compensation if coolingFan: tempPidFanBias = hal.newsig('%s-temp-pid-fan-bias' % name, hal.HAL_FLOAT) tempPidBiasOut = hal.newsig('%s-temp-pid-bias-out' % name, hal.HAL_FLOAT) scale = rt.newinst('scale', 'scale.%s-temp-pid-fan-bias' % name) hal.addf(scale.name, thread) scale.pin('in').link('%s.pwm' % coolingFan) scale.pin('out').link(tempPidFanBias) scale.pin('gain').set(c.find(section, 'FAN_BIAS')) sum2 = rt.newinst('sum2', 'sum2.%s-temp-pid-bias' % name) hal.addf(sum2.name, thread) sum2.pin('in0').link(tempPidBias) sum2.pin('in1').link(tempPidFanBias) sum2.pin('out').link(tempPidBiasOut) # PID pid = rt.newinst('pid', 'pid.%s' % name) hal.addf('%s.do-pid-calcs' % pid.name, thread) pid.pin('enable').link('emcmot-0-enable') # motor enable pid.pin('feedback').link(tempMeas) pid.pin('command').link(tempSet) pid.pin('output').link(tempPidOut) pid.pin('maxoutput').link(tempPwmMax) pid.pin('bias').link(tempPidBias) pid.pin('Pgain').link(tempPidPgain) pid.pin('Igain').link(tempPidIgain) pid.pin('Dgain').link(tempPidDgain) pid.pin('maxerrorI').link(tempPidMaxerrorI) # Limit heater PWM to positive values # PWM mimics hm2 implementation, which generates output for negative values limit1 = rt.newinst('limit1', 'limit.%s-temp-heaterl' % name) hal.addf(limit1.name, thread) limit1.pin('in').link(tempPidOut) limit1.pin('out').link(tempPwm) limit1.pin('min').set(0.0) limit1.pin('max').link(tempPwmMax) # Temperature checking sum2 = rt.newinst('sum2', 'sum2.%s-temp-range-pos' % name) hal.addf(sum2.name, thread) sum2.pin('in0').link(tempSet) sum2.pin('in1').set(c.find(section, 'TEMP_RANGE_POS_ERROR')) sum2.pin('out').link(tempRangeMax) sum2 = rt.newinst('sum2', 'sum2.%s-temp-range-neg' % name) hal.addf(sum2.name, thread) sum2.pin('in0').link(tempSet) sum2.pin('in1').set(c.find(section, 'TEMP_RANGE_NEG_ERROR')) sum2.pin('out').link(tempRangeMin) #the output of this component will say if measured temperature is in range of set value wcomp = rt.newinst('wcomp', 'wcomp.%s-temp-in-range' % name) hal.addf(wcomp.name, thread) wcomp.pin('min').link(tempRangeMin) wcomp.pin('max').link(tempRangeMax) wcomp.pin('in').link(tempMeas) wcomp.pin('out').link(tempInRange) # limit the output temperature to prevent damage when thermistor is broken/removed wcomp = rt.newinst('wcomp', 'wcomp.%s-temp-in-limit' % name) hal.addf(wcomp.name, thread) wcomp.pin('min').link(tempLimitMin) wcomp.pin('max').link(tempLimitMax) wcomp.pin('in').link(tempMeas) wcomp.pin('out').link(tempInLimit) # check the thermistor # net e0.temp.meas => thermistor-check.e0.temp # net e0.temp.in-range => not.e0-temp-range.in # net e0.temp.in-range_n <= not.e0-temp-range.out # net e0.temp.in-range_n => thermistor-check.e0.enable # net e0.heaterl => thermistor-check.e0.pid # net e0.therm-ok <= thermistor-check.e0.no-error # no error chain and3 = rt.newinst('andn', 'and3.%s-no-error-in' % name, pincount=3) hal.addf(and3.name, thread) and3.pin('in0').link(tempThermOk) and3.pin('in1').link(tempInLimit) if hardwareOkSignal: and3.pin('in2').link(hardwareOkSignal) else: and3.pin('in2').set(True) and3.pin('out').link(noErrorIn) tempThermOk.set(True) # thermistor checking for now disabled notComp = rt.newinst('not', 'not.%s-error-in' % name) hal.addf(notComp.name, thread) notComp.pin('in').link(noErrorIn) notComp.pin('out').link(errorIn) safetyLatch = rt.newinst('safety_latch', 'safety-latch.%s-error' % name) hal.addf(safetyLatch.name, thread) safetyLatch.pin('error-in').link(errorIn) safetyLatch.pin('error-out').link(error) safetyLatch.pin('reset').link('estop-reset') safetyLatch.pin('threshold').set(500) # 500ms error safetyLatch.pin('latching').set(True) # active chain comp = rt.newinst('comp', 'comp.%s-active' % name) hal.addf(comp.name, thread) comp.pin('in0').set(0.0001) comp.pin('hyst').set(0.0) comp.pin('in1').link(tempPwm) comp.pin('out').link(active) # Thermistor checking # setp thermistor-check.e0.wait 9.0 # setp thermistor-check.e0.min-pid 1.5 # disable0.25 # setp thermistor-check.e0.min-temp 1.5 # net e0.pid.bias => thermistor-check.e0.bias # Hotend fan if hotendFan: comp = rt.newinst('comp', 'comp.%s-pwm-enable' % hotendFan) hal.addf(comp.name, thread) comp.pin('in0').set(c.find(section, 'HOTEND_FAN_THRESHOLD', 50.0)) comp.pin('in1').link(tempMeas) comp.pin('hyst').set(c.find(section, 'HOTEND_FAN_HYST', 2.0)) comp.pin('out').link('%s-pwm-enable' % hotendFan) hal.Signal('%s-pwm' % hotendFan).set(1.0) rcomps.create_temperature_rcomp(name) motion.setup_temperature_io(name) # init parameter signals tempLimitMin.set(c.find(section, 'TEMP_LIMIT_MIN')) tempLimitMax.set(c.find(section, 'TEMP_LIMIT_MAX')) tempStandby.set(c.find(section, 'TEMP_STANDBY')) tempPwmMax.set(c.find(section, 'PWM_MAX')) tempPidPgain.set(c.find(section, 'PID_PGAIN')) tempPidIgain.set(c.find(section, 'PID_IGAIN')) tempPidDgain.set(c.find(section, 'PID_DGAIN')) tempPidMaxerrorI.set(c.find(section, 'PID_MAXERRORI')) tempPidBias.set(c.find(section, 'PID_BIAS'))
def setup_exp(name): hal.newsig('%s-pwm' % name, hal.HAL_FLOAT, init=0.0) hal.newsig('%s-enable' % name, hal.HAL_BIT, init=False)
def setup_exp(name): hal.newsig('%s-pwm' % name, hal.HAL_FLOAT, init=0.0) hal.newsig('%s-enable' % name, hal.HAL_BIT, init=False)
def velocity_jog(extruders, thread): ''' Velocity extruding jog support ''' # from ui jogVelocity = hal.newsig('ve-jog-velocity', hal.HAL_FLOAT) jogVelocityLimited = hal.newsig('ve-jog-velocity-limited', hal.HAL_FLOAT) jogDirection = hal.newsig('ve-jog-direction', hal.HAL_BIT) jogDistance = hal.newsig('ve-jog-distance', hal.HAL_FLOAT) jogTrigger = hal.newsig('ve-jog-trigger', hal.HAL_BIT) jogDtg = hal.newsig('ve-jog-dtg', hal.HAL_FLOAT) jogContinuous = hal.newsig('ve-jog-continuous', hal.HAL_BIT) # helper signals jogEnable = hal.newsig('ve-jog-enable', hal.HAL_BIT) jogVelocityNeg = hal.newsig('ve-jog-velocity-neg', hal.HAL_FLOAT) jogVelocitySigned = hal.newsig('ve-velocity-signed', hal.HAL_FLOAT) jogTime = hal.newsig('ve-jog-time', hal.HAL_FLOAT) jogTimeLeft = hal.newsig('ve-jog-time-left', hal.HAL_FLOAT) jogActive = hal.newsig('ve-jog-active', hal.HAL_BIT) maxVelocity = hal.Signal('ve-max-velocity', hal.HAL_FLOAT) baseVel = hal.Signal('ve-base-vel') extruderEn = hal.Signal('ve-extruder-en') # multiplexing jog velocity for multiple extruders ioMux = rt.newinst('io_muxn', 'io-mux%i.ve-jog-velocity' % extruders, pincount=extruders) hal.addf(ioMux.name, thread) ioMux.pin('out').link(jogVelocity) ioMux.pin('sel').link('extruder-sel') for n in range(0, extruders): signal = hal.newsig('ve-jog-velocity-e%i' % n, hal.HAL_FLOAT) ioMux.pin('in%i' % n).link(signal) limit1 = rt.newinst('limit1', 'limit1.ve-jog-velocity-limited') hal.addf(limit1.name, thread) limit1.pin('in').link(jogVelocity) limit1.pin('out').link(jogVelocityLimited) limit1.pin('min').set(0.01) # prevent users from setting 0 as jog velocity limit1.pin('max').link(maxVelocity) neg = rt.newinst('neg', 'neg.ve-jog-velocity-neg') hal.addf(neg.name, thread) neg.pin('in').link(jogVelocityLimited) neg.pin('out').link(jogVelocityNeg) mux2 = rt.newinst('mux2', 'mux2.ve-jog-velocity-signed') hal.addf(mux2.name, thread) mux2.pin('in0').link(jogVelocityLimited) mux2.pin('in1').link(jogVelocityNeg) mux2.pin('sel').link(jogDirection) mux2.pin('out').link(jogVelocitySigned) div2 = rt.newinst('div2', 'div2.ve-jog-time') hal.addf(div2.name, thread) div2.pin('in0').link(jogDistance) div2.pin('in1').link(jogVelocityLimited) div2.pin('out').link(jogTime) oneshot = rt.newinst('oneshot', 'oneshot.ve-jog-active') hal.addf(oneshot.name, thread) oneshot.pin('in').link(jogTrigger) oneshot.pin('width').link(jogTime) oneshot.pin('time-left').link(jogTimeLeft) oneshot.pin('rising').set(True) oneshot.pin('falling').set(False) oneshot.pin('retriggerable').set(1) oneshot.pin('out').link(jogActive) reset = rt.newinst('reset', 'reset.ve-jog-trigger') hal.addf(reset.name, thread) reset.pin('reset-bit').set(False) reset.pin('out-bit').link(jogTrigger) reset.pin('rising').set(False) reset.pin('falling').set(True) reset.pin('trigger').link(jogActive) or2 = rt.newinst('or2', 'or2.ve-jog-enable') hal.addf(or2.name, thread) or2.pin('in0').link(jogContinuous) or2.pin('in1').link(jogActive) or2.pin('out').link(jogEnable) mux2 = rt.newinst('mux2', 'mux2.ve-base-vel') hal.addf(mux2.name, thread) mux2.pin('in0').set(0.0) mux2.pin('in1').link(jogVelocitySigned) mux2.pin('sel').link(jogEnable) mux2.pin('out').link(baseVel) mult2 = rt.newinst('mult2', 'mult2.ve-jog-dtg') hal.addf(mult2.name, thread) mult2.pin('in0').link(jogVelocityLimited) mult2.pin('in1').link(jogTimeLeft) mult2.pin('out').link(jogDtg) # disable extruder on jog reset = rt.newinst('reset', 'reset.extruder-en1') hal.addf(reset.name, thread) reset.pin('rising').set(True) reset.pin('falling').set(False) reset.pin('retriggerable').set(True) reset.pin('reset-bit').set(False) reset.pin('trigger').link(jogTrigger) reset.pin('out-bit').link(extruderEn) reset = rt.newinst('reset', 'reset.extruder-en2') hal.addf(reset.name, thread) reset.pin('rising').set(True) reset.pin('falling').set(False) reset.pin('retriggerable').set(True) reset.pin('reset-bit').set(False) reset.pin('trigger').link(jogContinuous) reset.pin('out-bit').link(extruderEn) rcomps.create_ve_jog_rcomp(extruders=extruders)
def velocity_extrusion(extruders, thread): ''' Velocity extrusion support ''' # from motion/ui crossSection = hal.newsig('ve-cross-section', hal.HAL_FLOAT) crossSectionIn = hal.newsig('ve-cross-section-in', hal.HAL_FLOAT) lineWidth = hal.newsig('ve-line-width', hal.HAL_FLOAT) lineHeight = hal.newsig('ve-line-height', hal.HAL_FLOAT) filamentDia = hal.newsig('ve-filament-dia', hal.HAL_FLOAT) extrudeScale = hal.newsig('ve-extrude-scale', hal.HAL_FLOAT) extrudeAccelAdjGain = hal.newsig('ve-extrude-accel-adj-gain', hal.HAL_FLOAT) retractVel = hal.newsig('ve-retract-vel', hal.HAL_FLOAT) retractLen = hal.newsig('ve-retract-len', hal.HAL_FLOAT) maxVelocity = hal.newsig('ve-max-velocity', hal.HAL_FLOAT) # helper signals nozzleVel = hal.newsig('ve-nozzle-vel', hal.HAL_FLOAT) nozzleDischarge = hal.newsig('ve-nozzle-discharge', hal.HAL_FLOAT) filamentDiaSquared = hal.newsig('ve-filament-dia-squared', hal.HAL_FLOAT) filamentArea = hal.newsig('ve-filament-area', hal.HAL_FLOAT) extrudeRate = hal.newsig('ve-extrude-rate', hal.HAL_FLOAT) extrudeRateScaled = hal.newsig('ve-extrude-rate-scaled', hal.HAL_FLOAT) extrudeAccel = hal.newsig('ve-extrude-accel', hal.HAL_FLOAT) extrudeAccelAdj = hal.newsig('ve-extrude-accel-adj', hal.HAL_FLOAT) extrudeRateAdj = hal.newsig('ve-extrude-rate-adj', hal.HAL_FLOAT) extruderEn = hal.newsig('ve-extruder-en', hal.HAL_BIT) retractVelNeg = hal.newsig('ve-retract-vel-neg', hal.HAL_FLOAT) retractTime = hal.newsig('ve-retract-time', hal.HAL_FLOAT) retract = hal.newsig('ve-retract', hal.HAL_BIT) extrudeVel = hal.newsig('ve-extrude-vel', hal.HAL_FLOAT) baseVel = hal.newsig('ve-base-vel', hal.HAL_FLOAT) nozzleVel.link('motion.current-vel') mult2 = rt.newinst('mult2', 'mult2.ve-cross-section') hal.addf(mult2.name, thread) mult2.pin('in0').link(lineWidth) mult2.pin('in1').link(lineHeight) mult2.pin('out').link(crossSectionIn) outToIo = rt.newinst('out_to_io', 'out-to-io.ve-cross-section') hal.addf(outToIo.name, thread) outToIo.pin('in-float').link(crossSectionIn) outToIo.pin('out-float').link(crossSection) # multiply area with speed and we get discharge (mm^3 per second) mult2 = rt.newinst('mult2', 'mult2.ve-nozzle-discharge') hal.addf(mult2.name, thread) mult2.pin('in0').link(crossSection) mult2.pin('in1').link(nozzleVel) mult2.pin('out').link(nozzleDischarge) # calculate filament cross section area # PI divided by 4 mult2 = rt.newinst('mult2', 'mult2.ve-filament-dia') hal.addf(mult2.name, thread) mult2.pin('in0').link(filamentDia) mult2.pin('in1').link(filamentDia) mult2.pin('out').link(filamentDiaSquared) mult2 = rt.newinst('mult2', 'mult2.ve-filament-area') hal.addf(mult2.name, thread) mult2.pin('in0').set(math.pi / 4) mult2.pin('in1').link(filamentDiaSquared) mult2.pin('out').link(filamentArea) # calculate extrude rate div2 = rt.newinst('div2', 'div2.ve-extrude-rate') hal.addf(div2.name, thread) div2.pin('in0').link(nozzleDischarge) div2.pin('in1').link(filamentArea) div2.pin('out').link(extrudeRate) # scale extrude rate mult2 = rt.newinst('mult2', 'mult2.ve-extrude-rate-scaled') hal.addf(mult2.name, thread) mult2.pin('in0').link(extrudeRate) mult2.pin('in1').link(extrudeScale) mult2.pin('out').link(extrudeRateScaled) # these are used for a small offset in velocity during acceleration (buildup pressure inside # the nozzle because of the current speed. Take the maximum accel you've specified in .ini # get acceleration into lincurve ddt = rt.newinst('ddt', 'ddt.ve-extruder-accel') hal.addf(ddt.name, thread) ddt.pin('in').link(extrudeRateScaled) ddt.pin('out').link(extrudeAccel) mult2 = rt.newinst('mult2', 'mult2.ve-extrude-accel-adj') hal.addf(mult2.name, thread) mult2.pin('in0').link(extrudeAccel) mult2.pin('in1').link(extrudeAccelAdjGain) mult2.pin('out').link(extrudeAccelAdj) # get adjusted speed for adding to current speed during acceleration sum2 = rt.newinst('sum2', 'sum2.ve-extrude-rate-adj') hal.addf(sum2.name, thread) sum2.pin('in0').link(extrudeRateScaled) sum2.pin('in1').link(extrudeAccelAdj) sum2.pin('out').link(extrudeRateAdj) # negative retract velocity neg = rt.newinst('neg', 'neg.ve-rectract-vel-neg') hal.addf(neg.name, thread) neg.pin('in').link(retractVel) neg.pin('out').link(retractVelNeg) # calculate retract time div2 = rt.newinst('div2', 'div2.ve-retract-time') hal.addf(div2.name, thread) div2.pin('in0').link(retractLen) div2.pin('in1').link(retractVel) div2.pin('out').link(retractTime) # We want the retract-charge to run for a fixed time: # when sel0 set to "1" meaning motion with extrusion" the on the rising edge # there will temporarily be also sel1 which is high, meaning a pre-charge because the # sel combination is 11 # when sel1 set to "0" meaning decoupling motion with extrusion" then the falling edge # will trigger a 01 combination, meaning a retract # trigger a retract/unretract move when extruder is enable or disabled oneshot = rt.newinst('oneshot', 'oneshot.ve-retract') hal.addf(oneshot.name, thread) oneshot.pin('rising').set(True) oneshot.pin('falling').set(True) oneshot.pin('retriggerable').set(True) oneshot.pin('width').link(retractTime) oneshot.pin('in').link(extruderEn) oneshot.pin('out').link(retract) retract += 'motion.feed-hold' # stop motion until retract/unretract finished # jogging needs to be inserted here velocity_jog(extruders, thread) # now the solution of Andy Pugh for automatically retracting/priming #00 = motion without extrusion, jog #01 = retract #10 = motion with extrusion #11 = unretract, pre-charge mux4 = rt.newinst('mux4', 'mux4.ve-extrude-vel') hal.addf(mux4.name, thread) mux4.pin('in0').link(baseVel) mux4.pin('in1').link(retractVelNeg) mux4.pin('in2').link(extrudeRateAdj) mux4.pin('in3').link(retractVel) mux4.pin('out').link(extrudeVel) mux4.pin('sel0').link(retract) mux4.pin('sel1').link(extruderEn) sections = [[retractLen, 'RETRACT_LEN'], [retractVel, 'RETRACT_VEL'], [filamentDia, 'FILAMENT_DIA'], [maxVelocity, 'MAX_VELOCITY'], [extrudeScale, 'EXTRUDE_SCALE']] for section in sections: ioMux = rt.newinst('io_muxn', 'io-mux%i.%s' % (extruders, section[0].name), pincount=extruders) hal.addf(ioMux.name, thread) ioMux.pin('out').link(section[0]) ioMux.pin('sel').link('extruder-sel') for n in range(0, extruders): signal = hal.newsig('%s-e%i' % (section[0].name, n), hal.HAL_FLOAT) ioMux.pin('in%i' % n).link(signal) signal.set(c.find('EXTRUDER_%i' % n, section[1])) extrudeAccelAdjGain.set(0.05) if baseVel.writers < 1: # can only write when jogging not configured baseVel.set(0.0) rcomps.create_ve_params_rcomp() storage.setup_ve_storage(extruders=extruders) motion.setup_ve_io()
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
def test_check_net_args(): l.log() try: hal.net() except TypeError: pass assert 'c1-s32out' not in hal.signals try: hal.net(None, "c1.s32out") except TypeError: pass assert 'c1-s32out' not in hal.signals # single pin argument assert hal.pins["c1.s32out"].linked is False try: hal.net("c1.s32out") # RuntimeError: net: at least one pin name expected except RuntimeError: pass # XXX die beiden gehen daneben: # der pin wird trotz runtime error gelinkt und das signal erzeugt: # offensichtlich hal_net.pyx:39 vor dem test in hal_net.pyx:60 assert hal.pins["c1.s32out"].linked is False assert 'c1-s32out' not in hal.signals # single signal argument try: hal.net("noexiste") # RuntimeError: net: at least one pin name expected except RuntimeError: pass # two signals hal.newsig('sig1', hal.HAL_FLOAT) hal.newsig('sig2', hal.HAL_FLOAT) try: hal.net('sig1', 'sig2') # NameError: no such pin: sig2 except NameError: pass assert "noexiste" not in hal.signals hal.net("noexiste", "c1.s32out") assert "noexiste" in hal.signals ne = hal.signals["noexiste"] assert ne.writers == 1 assert ne.readers == 0 assert ne.bidirs == 0 try: hal.net("floatsig1", "c1.s32out") raise "should not happen" except RuntimeError: pass
def __init__(self, name='motor', thread='base_thread', eqep='eQEP0', eqepScale=2797.0, pwmDown='hpg.pwmgen.00.out.00', pwmUp='hpg.pwmgen.00.out.01', enableDown='bb_gpio.p9.out-15', enableUp='bb_gpio.p9.out-17'): 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) sigCmdVel = hal.newsig('%s-cmd-vel' % name, hal.HAL_FLOAT) sigPwmIn = hal.newsig('%s-pwm-in' % name, hal.HAL_FLOAT) sigPos = hal.newsig('%s-pos' % name, hal.HAL_FLOAT) sigVel = hal.newsig('%s-vel' % name, hal.HAL_FLOAT) sigAcc = hal.newsig('%s-acc' % name, hal.HAL_FLOAT) sigUp = hal.newsig('%s-pwm-up' % name, hal.HAL_FLOAT) sigDown = hal.newsig('%s-pwm-down' % name, hal.HAL_FLOAT) sigEnable = hal.newsig('%s-enable' % name, hal.HAL_BIT) sigPwmEn = hal.newsig('%s-pwm-enable' % name, hal.HAL_BIT) sigTuneStart = hal.newsig('%s-tune-start' % name, hal.HAL_BIT) sigTuneMode = hal.newsig('%s-tune-mode' % name, hal.HAL_BIT) # 43.7:1 gear # encoder resolution of 64 counts per revolution of the motor shaft, # 2797 counts per revolution of the gearboxs output shaft. # for eQEP0.position in shaft revs: hal.Pin('%s.position-scale' % eqep).set(eqepScale) hal.Pin('%s.capture-prescaler' % eqep).set(5) hal.Pin('%s.min-speed-estimate' % eqep).set(0.001) # feed into PID sigPos.link('%s.position' % eqep) # for UI feedback sigVel.link('%s.velocity' % eqep) # ddt for accel ddt = rt.newinst('ddt', 'ddt.%s-acc' % name) hal.addf(ddt.name, thread) ddt.pin('in').link(sigVel) ddt.pin('out').link(sigAcc) # PID pid = rt.newinst('at_pid', 'pid.%s-vel' % 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('command').link(sigCmdVel) pid.pin('output').link(sigPwmIn) pid.pin('feedback').link(sigVel) pid.pin('enable').link(sigEnable) # auto tuning pid.pin('tuneCycles').set(200) pid.pin('tuneEffort').set(0.15) pid.pin('tuneMode').link(sigTuneMode) pid.pin('tuneStart').link(sigTuneStart) # automatically start auto tuning when switched to tune mode timedelay = rt.newinst('timedelay', 'timedelay.%s' % sigTuneStart.name) hal.addf(timedelay.name, thread) timedelay.pin('in').link(sigTuneMode) timedelay.pin('on-delay').set(0.1) timedelay.pin('off-delay').set(0.0) # convert out singnal to IO outToIo = rt.newinst('out_to_io', 'out-to-io.%s' % sigTuneStart.name) hal.addf(outToIo.name, thread) timedelay.pin('out').link(outToIo.pin('in-bit')) outToIo.pin('out-bit').link(sigTuneStart) # reset the tune mode to false once tuning is finished reset = rt.newinst('reset', 'reset.%s' % sigTuneMode.name) hal.addf(reset.name, thread) reset.pin('out-bit').link(sigTuneMode) reset.pin('reset-bit').set(False) reset.pin('trigger').link(sigTuneStart) reset.pin('rising').set(False) reset.pin('falling').set(True) # hbridge hbridge = rt.newinst('hbridge', 'hbridge.%s' % name) hal.addf(hbridge.name, thread) hbridge.pin('up').link(sigUp) hbridge.pin('down').link(sigDown) hbridge.pin('enable').link(sigEnable) hbridge.pin('enable-out').link(sigPwmEn) hbridge.pin('command').link(sigPwmIn) # PWM signals hal.Pin('%s.value' % pwmUp).link(sigUp) hal.Pin('%s.value' % pwmDown).link(sigDown) # Enable hal.Pin(enableUp).link(sigPwmEn) hal.Pin(enableDown).link(sigPwmEn) hal.Pin('%s.enable' % pwmUp).link(sigPwmEn) hal.Pin('%s.enable' % pwmDown).link(sigPwmEn) # prevent pid runup if disabled sigEnable.set(True) # 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)
def _init_jog(self): jog_vel_limited_sig = hal.newsig('ve%d.jog-vel-limited' % self.nr, hal.HAL_FLOAT) jog_vel_limit = rtapi.newinst('limit1', 've%d.limit1.jog-vel-limit' % self.nr) hal.addf(jog_vel_limit.name, SERVO_THREAD) jog_vel_limit.pin('in').link(self.jog_vel_sig) jog_vel_limit.pin('out').link(jog_vel_limited_sig) jog_vel_limit.pin('min').set(0.01) jog_vel_limit.pin('max').link(self.max_jog_vel_sig) jog_vel_limited_neg_sig = hal.newsig('ve%d.jog-vel-limited-neg' % self.nr, hal.HAL_FLOAT) jog_vel_limited_neg = rtapi.newinst('neg', 've%d.neg.jog-vel-limited-neg' % self.nr) hal.addf(jog_vel_limited_neg.name, SERVO_THREAD) jog_vel_limited_neg.pin('in').link(jog_vel_limited_sig) jog_vel_limited_neg.pin('out').link(jog_vel_limited_neg_sig) jog_vel_signed_sig = hal.newsig('ve%d.jog-vel-signed' % self.nr, hal.HAL_FLOAT) jog_vel_signed = rtapi.newinst('mux2', 've%d.mux2.jog-vel-signed' % self.nr) hal.addf(jog_vel_signed.name, SERVO_THREAD) jog_vel_signed.pin('in0').link(jog_vel_limited_sig) jog_vel_signed.pin('in1').link(jog_vel_limited_neg_sig) jog_vel_signed.pin('sel').link(self.jog_direction_sig) jog_vel_signed.pin('out').link(jog_vel_signed_sig) jog_time_sig = hal.newsig('ve%d.jog-time' % self.nr, hal.HAL_FLOAT) jog_time_left_sig = hal.newsig('ve%d.jog-time-left' % self.nr, hal.HAL_FLOAT) jog_time = rtapi.newinst('div2', 've%d.div2.jog-time' % self.nr) hal.addf(jog_time.name, SERVO_THREAD) jog_time.pin('in0').link(self.jog_distance_sig) jog_time.pin('in1').link(jog_vel_limited_sig) jog_time.pin('out').link(jog_time_sig) jog_single_sig = hal.newsig('ve%d.jog-single' % self.nr, hal.HAL_BIT) jog_single_oneshot = rtapi.newinst('oneshot', 've%d.oneshot.jog-single' % self.nr) hal.addf(jog_single_oneshot.name, SERVO_THREAD) jog_single_oneshot.pin('in').link(self.jog_trigger_sig) jog_single_oneshot.pin('width').link(jog_time_sig) jog_single_oneshot.pin('time-left').link(jog_time_left_sig) jog_single_oneshot.pin('rising').set(True) jog_single_oneshot.pin('falling').set(False) jog_single_oneshot.pin('retriggerable').set(1) jog_single_oneshot.pin('out').link(jog_single_sig) jog_reset_trigger = rtapi.newinst('reset', 've%d.reset.jog-trigger' % self.nr) hal.addf(jog_reset_trigger.name, SERVO_THREAD) jog_reset_trigger.pin('trigger').link(jog_single_sig) jog_reset_trigger.pin('rising').set(False) jog_reset_trigger.pin('falling').set(True) jog_reset_trigger.pin('reset-bit').set(False) jog_reset_trigger.pin('out-bit').link(self.jog_trigger_sig) jog_enable_sig = hal.newsig('ve%d.jog-enable' % self.nr, hal.HAL_BIT) jog_enable = rtapi.newinst('or2', 've%d.or2.jog-enable' % self.nr) hal.addf(jog_enable.name, SERVO_THREAD) jog_enable.pin('in0').link(self.jog_continuous_sig) jog_enable.pin('in1').link(jog_single_sig) jog_enable.pin('out').link(jog_enable_sig) jog_vel_output = rtapi.newinst('mux2', 've%d.mux2.jog-vel-output' % self.nr) hal.addf(jog_vel_output.name, SERVO_THREAD) jog_vel_output.pin('in0').set(0.0) jog_vel_output.pin('in1').link(jog_vel_signed_sig) jog_vel_output.pin('sel').link(jog_enable_sig) jog_vel_output.pin('out').link(self.jog_vel_output_sig) jog_dtg = rtapi.newinst('mult2', 've%d.mult2.jog-dtg' % self.nr) hal.addf(jog_dtg.name, SERVO_THREAD) jog_dtg.pin('in0').link(jog_vel_limited_sig) jog_dtg.pin('in1').link(jog_time_left_sig) jog_dtg.pin('out').link(self.jog_dtg_sig) disable1 = rtapi.newinst('reset', 've%d.reset.on-jog-single' % self.nr) hal.addf(disable1.name, SERVO_THREAD) disable1.pin('trigger').link(self.jog_trigger_sig) disable1.pin('rising').set(True) disable1.pin('falling').set(False) disable1.pin('retriggerable').set(True) disable1.pin('reset-bit').set(False) disable1.pin('out-bit').link(self.enable_sig) disable2 = rtapi.newinst('reset', 've%d.reset.on-jog-continuous' % self.nr) hal.addf(disable2.name, SERVO_THREAD) disable2.pin('trigger').link(self.jog_continuous_sig) disable2.pin('rising').set(True) disable2.pin('falling').set(False) disable2.pin('retriggerable').set(True) disable2.pin('reset-bit').set(False) disable2.pin('out-bit').link(self.enable_sig)