Exemple #1
0
    def _setup_user_control(thread):
        conv_state_cmd = rt.newinst('conv_u32_bit', 'conv_u32_bit.state-cmd')
        hal.addf(conv_state_cmd.name, thread.name)
        conv_state_cmd.pin('in').link('state-cmd')
        conv_state_cmd.pin('out').link('power-on')

        conv_state_fb = rt.newinst('conv_bit_s32', 'conv_bit_s32.state-fb')
        hal.addf(conv_state_fb.name, thread.name)
        conv_state_fb.pin('in').link('lamp-yellow')
        conv_state_fb.pin('out').link('state-fb')

        reset = rt.newinst('reset', 'reset.state-cmd')
        hal.addf(reset.name, thread.name)
        reset.pin('trigger').link('estop-active')
        reset.pin('out-u32').link('state-cmd')
        reset.pin('rising').set(True)

        and2 = rt.newinst('and2', 'and2.reset-state-cmd')
        hal.addf(and2.name, thread.name)
        and2.pin('in0').link('reset')
        and2.pin('in1').link('estop-active')

        oneshot = rt.newinst('oneshot', 'oneshot.reset')
        hal.addf(oneshot.name, thread.name)
        oneshot.pin('in').link('reset-in')
        oneshot.pin('out-not').link('reset')
        oneshot.pin('width').set(BorunteConfig.RESET_DELAY_S)
        oneshot.pin('rising').set(True)
        oneshot.pin('falling').set(True)
def init_gantry(axisIndex, joints=2, latching=True):
    if latching:
        comp = 'lgantry'
    else:
        comp = 'gantry'
    rt.newinst(comp, 'gantry.%i' % axisIndex, pincount=joints)
    rcomps.create_gantry_rcomp(axisIndex=axisIndex)
def setup_stepper_multiplexer(stepgenIndex, sections, selSignal, thread):
    num = len(sections)
    sigBase = 'stepgen-%i' % stepgenIndex

    unsignedSignals = [['dirsetup', 'DIRSETUP'], ['dirhold', 'DIRHOLD'],
                       ['steplen', 'STEPLEN'], ['stepspace', 'STEPSPACE']]

    floatSignals = [['scale', 'SCALE'], ['max-vel', 'STEPGEN_MAX_VEL'],
                    ['max-acc', 'STEPGEN_MAX_ACC']]

    for item in unsignedSignals:
        signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_U32)
        mux = rt.newinst('muxn_u32',
                         'mux%i.%s' % (num, signal.name),
                         pincount=num)
        hal.addf(mux.name, thread)
        for n, section in enumerate(sections):
            mux.pin('in%i' % n).set(c.find(section, item[1]))
        mux.pin('sel').link(selSignal)
        mux.pin('out').link(signal)

    for item in floatSignals:
        signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_FLOAT)
        mux = rt.newinst('muxn', 'mux%i.%s' % (num, signal.name), pincount=num)
        hal.addf(mux.name, thread)
        for n, section in enumerate(sections):
            mux.pin('in%i' % n).set(c.find(section, item[1]))
        mux.pin('sel').link(selSignal)
        mux.pin('out').link(signal)
Exemple #4
0
    def _setup_joint_ferror(nr, thread):
        cmd_fb_pos = hal.Signal('joint-{}-cmd-fb-pos'.format(nr),
                                hal.HAL_FLOAT)
        fb_in_pos = hal.Signal('joint-{}-fb-in-pos'.format(nr), hal.HAL_FLOAT)
        ferror = hal.Signal('joint-{}-ferror'.format(nr), hal.HAL_FLOAT)
        ferror_abs = hal.Signal('joint-{}-ferror-abs'.format(nr),
                                hal.HAL_FLOAT)
        ferror_max = hal.Signal('joint-{}-ferror-max'.format(nr),
                                hal.HAL_FLOAT)
        ferror_active = hal.Signal('joint-{}-ferror-active'.format(nr),
                                   hal.HAL_BIT)

        sum_ferror = rt.newinst('sum2', 'sum2.joint-{}-ferror'.format(nr))
        sum_ferror.pin('in0').link(cmd_fb_pos)
        sum_ferror.pin('in1').link(fb_in_pos)
        sum_ferror.pin('gain1').set(-1.0)
        sum_ferror.pin('out').link(ferror)

        abs_ferror = rt.newinst('abs', 'abs.joint-{}-ferror'.format(nr))
        abs_ferror.pin('in').link(ferror)
        abs_ferror.pin('out').link(ferror_abs)

        comp = rt.newinst('comp', 'comp.joint-{}-ferror'.format(nr))
        comp.pin('in0').link(ferror_max)
        comp.pin('in1').link(ferror_abs)
        comp.pin('out').link(ferror_active)

        hal.addf(sum_ferror.name, thread.name)
        hal.addf(abs_ferror.name, thread.name)
        hal.addf(comp.name, thread.name)
Exemple #5
0
def setup_estop(errorSignals, thread):
    # Create estop signal chain
    estopUser = hal.Signal('estop-user', hal.HAL_BIT)
    estopReset = hal.Signal('estop-reset', hal.HAL_BIT)
    estopOut = hal.Signal('estop-out', hal.HAL_BIT)
    estopIn = hal.Signal('estop-in', hal.HAL_BIT)
    estopError = hal.Signal('estop-error', hal.HAL_BIT)

    num = len(errorSignals)
    orComp = rt.newinst('orn', 'or%i.estop-error' % num, pincount=num)
    hal.addf(orComp.name, thread)
    for n, sig in enumerate(errorSignals):
        orComp.pin('in%i' % n).link(sig)
    orComp.pin('out').link(estopError)

    estopLatch = rt.newinst('estop_latch', 'estop-latch')
    hal.addf(estopLatch.name, thread)
    estopLatch.pin('ok-in').link(estopUser)
    estopLatch.pin('fault-in').link(estopError)
    estopLatch.pin('reset').link(estopReset)
    estopLatch.pin('ok-out').link(estopOut)

    estopUser.link('iocontrol.0.user-enable-out')
    estopReset.link('iocontrol.0.user-request-enable')

    # Monitor estop input from hardware
    estopIn.link('iocontrol.0.emc-enable-in')
Exemple #6
0
    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)
Exemple #7
0
def usrcomp_watchdog(comps,
                     enableSignal,
                     thread,
                     okSignal=None,
                     errorSignal=None):
    count = len(comps)
    watchdog = rt.newinst('watchdog', 'watchdog.usrcomp', pincount=count)
    hal.addf('%s.set-timeouts' % watchdog.name, thread)
    hal.addf('%s.process' % watchdog.name, 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-%02i' % n).link(sigIn)
        watchdog.pin('timeout-%02i' % 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_estop(errorSignals, thread):
    # Create estop signal chain
    estopUser = hal.Signal('estop-user', hal.HAL_BIT)
    estopReset = hal.Signal('estop-reset', hal.HAL_BIT)
    estopOut = hal.Signal('estop-out', hal.HAL_BIT)
    estopIn = hal.Signal('estop-in', hal.HAL_BIT)
    estopError = hal.Signal('estop-error', hal.HAL_BIT)

    num = len(errorSignals)
    orComp = rt.newinst('orn', 'or%i.estop-error' % num, pincount=num)
    hal.addf(orComp.name, thread)
    for n, sig in enumerate(errorSignals):
        orComp.pin('in%i' % n).link(sig)
    orComp.pin('out').link(estopError)

    estopLatch = rt.newinst('estop_latch', 'estop-latch')
    hal.addf(estopLatch.name, thread)
    estopLatch.pin('ok-in').link(estopUser)
    estopLatch.pin('fault-in').link(estopError)
    estopLatch.pin('reset').link(estopReset)
    estopLatch.pin('ok-out').link(estopOut)

    estopUser.link('iocontrol.0.user-enable-out')
    estopReset.link('iocontrol.0.user-request-enable')

    # Monitor estop input from hardware
    estopIn.link('iocontrol.0.emc-enable-in')
Exemple #9
0
    def _setup_gripper(self):
        if not self.tool.startswith('hand_e'):
            return

        open_close = hal.Signal('gripper-open-close', hal.HAL_BIT)
        opened = hal.Signal('gripper-opened', hal.HAL_BIT)
        cmd_active = hal.Signal('gripper-cmd-active', hal.HAL_BIT)

        mux2 = rt.newinst('mux2', 'mux2.gripper-open-close')
        hal.addf(mux2.name, self.thread.name)
        mux2.pin('sel').link(open_close)
        mux2.pin('in0').set(0xFF)
        mux2.pin('in1').set(0x00)

        float2u32 = rt.newinst('conv_float_u32', 'conv.gripper-pos')
        hal.addf(float2u32.name, self.thread.name)
        mux2.pin('out').link(float2u32.pin('in'))

        comp = rt.newinst('comp', 'comp.gripper-fb-pos')
        hal.addf(comp.name, self.thread.name)
        comp.pin('in0').set(254.5)
        comp.pin('out').link(opened)

        u32tofloat = rt.newinst('conv_u32_float', 'conv-gripper-pos-fb')
        hal.addf(u32tofloat.name, self.thread.name)
        u32tofloat.pin('out').link(comp.pin('in1'))

        robotiq = hal.components['robotiq-gripper']
        robotiq.pin('force').set(0xFF)
        robotiq.pin('velocity').set(0xFF)
        robotiq.pin('position-fb').link(u32tofloat.pin('in'))
        robotiq.pin('cmd-active').link(cmd_active)
        float2u32.pin('out').link(robotiq.pin('position'))

        open_close.set(True)  # start opened
Exemple #10
0
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)
Exemple #11
0
def init_gantry(axisIndex, joints=2, latching=True):
    if latching:
        comp = 'lgantry'
    else:
        comp = 'gantry'
    rt.newinst(comp, 'gantry.%i' % axisIndex, pincount=joints)
    rcomps.create_gantry_rcomp(axisIndex=axisIndex)
Exemple #12
0
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)
Exemple #13
0
def setup_stepper_multiplexer(stepgenIndex, sections, selSignal, thread):
    num = len(sections)
    sigBase = 'stepgen-%i' % stepgenIndex

    unsignedSignals = [['dirsetup', 'DIRSETUP'],
                       ['dirhold', 'DIRHOLD'],
                       ['steplen', 'STEPLEN'],
                       ['stepspace', 'STEPSPACE']]

    floatSignals = [['scale', 'SCALE'],
                    ['max-vel', 'STEPGEN_MAX_VEL'],
                    ['max-acc', 'STEPGEN_MAX_ACC']]

    for item in unsignedSignals:
        signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_U32)
        mux = rt.newinst('muxn_u32', 'mux%i.%s' % (num, signal.name), pincount=num)
        hal.addf(mux.name, thread)
        for n, section in enumerate(sections):
            mux.pin('in%i' % n).set(c.find(section, item[1]))
        mux.pin('sel').link(selSignal)
        mux.pin('out').link(signal)

    for item in floatSignals:
        signal = hal.Signal('%s-%s' % (sigBase, item[0]), hal.HAL_FLOAT)
        mux = rt.newinst('muxn', 'mux%i.%s' % (num, signal.name), pincount=num)
        hal.addf(mux.name, thread)
        for n, section in enumerate(sections):
            mux.pin('in%i' % n).set(c.find(section, item[1]))
        mux.pin('sel').link(selSignal)
        mux.pin('out').link(signal)
Exemple #14
0
    def _setup_joint_offset(nr, thread):
        home_pos = hal.Signal('joint-{}-home-pos'.format(nr), hal.HAL_FLOAT)
        abs_pos = hal.Signal('joint-{}-abs-pos'.format(nr), hal.HAL_FLOAT)
        fb_out_pos = hal.Signal('joint-{}-fb-out-pos'.format(nr),
                                hal.HAL_FLOAT)
        fb_in_pos = hal.Signal('joint-{}-fb-in-pos'.format(nr), hal.HAL_FLOAT)
        cmd_pos = hal.Signal('joint-{}-cmd-pos'.format(nr), hal.HAL_FLOAT)
        real_pos = hal.Signal('joint-{}-real-pos'.format(nr), hal.HAL_FLOAT)
        cmd_in_pos = hal.Signal('joint-{}-cmd-in-pos'.format(nr),
                                hal.HAL_FLOAT)
        cmd_out_pos = hal.Signal('joint-{}-cmd-out-pos'.format(nr),
                                 hal.HAL_FLOAT)
        pos_offset = hal.Signal('joint-{}-pos-offset'.format(nr),
                                hal.HAL_FLOAT)
        limit_min = hal.Signal('joint-{}-limit-min'.format(nr), hal.HAL_FLOAT)
        limit_max = hal.Signal('joint-{}-limit-max'.format(nr), hal.HAL_FLOAT)
        son = hal.Signal('son-{}'.format(nr), hal.HAL_BIT)
        son_not = hal.Signal('son-{}-not'.format(nr), hal.HAL_BIT)
        set_home = hal.Signal('joint-{}-set-home'.format(nr), hal.HAL_BIT)

        offset = rt.newinst('offset', 'offset.joint-{}'.format(nr))
        offset.pin('offset').link(pos_offset)
        offset.pin('fb-in').link(fb_in_pos)
        offset.pin('fb-out').link(fb_out_pos)
        offset.pin('in').link(cmd_in_pos)
        offset.pin('out').link(cmd_out_pos)

        abs_joint = rt.newinst('absolute_joint', 'abs-joint.{}'.format(nr))
        abs_joint.pin('home-pos').link(home_pos)
        abs_joint.pin('abs-pos').link(abs_pos)
        abs_joint.pin('real-pos').link(real_pos)
        abs_joint.pin('fb-pos').link(fb_in_pos)
        abs_joint.pin('offset').link(pos_offset)
        abs_joint.pin('set-abs').link(son_not)
        abs_joint.pin('set-home').link(set_home)

        not_son = rt.newinst('not', 'not.son-{}'.format(nr))
        not_son.pin('in').link(son)
        not_son.pin('out').link(son_not)

        # setup min/max joint limits
        limit = rt.newinst('limit1', 'limit1.joint-{}'.format(nr))
        limit.pin('min').link(limit_min)
        limit.pin('max').link(limit_max)
        limit.pin('in').link(cmd_pos)
        limit.pin('out').link(cmd_in_pos)

        # connect functions in correct order
        hal.addf(not_son.name, thread.name)
        hal.addf(abs_joint.name, thread.name)
        hal.addf(limit.name, thread.name)
        hal.addf('{}.update-feedback'.format(offset.name), thread.name)
        hal.addf('{}.update-output'.format(offset.name), thread.name)
Exemple #15
0
    def _setup_drive_safety_signals(thread):
        for i in range(1, NUM_JOINTS + 1):
            and2_son = rt.newinst('and2', 'and2.son-{}'.format(i))
            hal.addf(and2_son.name, thread.name)
            and2_son.pin('in0').link('son-{}'.format(i))
            and2_son.pin('in1').link('ok')
            and2_son.pin('out').link('son-{}-out'.format(i))

            and2_brake = rt.newinst('and2', 'and2.brake-release-{}'.format(i))
            hal.addf(and2_brake.name, thread.name)
            and2_brake.pin('in0').link('brake-release-{}'.format(i))
            and2_brake.pin('in1').link('ok')
            and2_brake.pin('out').link('brake-release-{}-out'.format(i))
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)
Exemple #17
0
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)
Exemple #18
0
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)
Exemple #19
0
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])
Exemple #20
0
    def _init_hm2(self):
        mesahandler = MesaHandler(
            device='7I80', address=MESA_BOARD_IP, firmware=MESA_FIRMWARE_FILE
        )
        mesahandler.load_mesacard()

        rt.loadrt('hostmot2')
        rt.loadrt(
            'hm2_eth',
            board_ip=MESA_BOARD_IP,
            config='"num_encoders={count},num_stepgens={count}"'.format(
                count=NUM_JOINTS
            ),
        )
        hal.Pin('hm2_7i80.0.watchdog.timeout_ns').set(int(self.thread.period_ns * 2))

        hw_watchdog_signal = hal.Signal('hardware-watchdog', hal.HAL_BIT)
        hal.Pin('hm2_7i80.0.watchdog.has_bit').link(hw_watchdog_signal)
        self.error_signals.append(hw_watchdog_signal)

        reset = rt.newinst('reset', 'reset.watchdog')
        hal.addf(reset.name, self.thread.name)
        reset.pin('trigger').link('power-on')
        reset.pin('reset-bit').set(False)
        reset.pin('out-bit').link(hw_watchdog_signal)
 def _setup_brakes(self):
     # pass through SON to brake disable signal
     for i in range(1, NUM_JOINTS + 1):
         or2 = rt.newinst('or2', 'or2.brake-release-{}'.format(i))
         hal.addf(or2.name, self.thread.name)
         or2.pin('in0').link('son-{}-out'.format(i))
         or2.pin('out').link('brake-release-{}'.format(i))
Exemple #22
0
        def __init__(self):
            BorunteConfig._setup_joint_offset(nr, hal_config.thread)

            # feed back the output for now
            sum2 = rt.newinst('sum2', 'sum2-{}-test'.format(nr))
            hal.addf(sum2.name, hal_config.thread.name)
            sum2.pin('in0').link(self.cmd_out_pos)
            sum2.pin('out').link(self.fb_in_pos)
Exemple #23
0
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_gripper(self):
        open_close = hal.Signal('gripper-open-close', hal.HAL_BIT)
        opened = hal.Signal('gripper-opened', hal.HAL_BIT)

        or2 = rt.newinst('or2', 'or2.gripper-open-close')
        hal.addf(or2.name, self.thread.name)
        or2.pin('in0').link(open_close)
        or2.pin('out').link(opened)
Exemple #25
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)
Exemple #26
0
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)
Exemple #27
0
def init_hardware():
    watchList = []
    #rt=rtapi.RTAPIcommand()

    rt.newinst('pp_gpio', 'hpg', 'pincount=6')
    rt.newinst('pp_stepgen', 'hps', 'pincount=5')

    #print(c.find('HBP', 'THERMISTOR', defaultThermistor))
    #raw_input()

    # load low-level drivers
    if False:
        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 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)
Exemple #29
0
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)
Exemple #30
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)
Exemple #31
0
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)
Exemple #32
0
def setup_extruder_multiplexer(extruders, thread):
    extruderSel = hal.Signal('extruder-sel', hal.HAL_S32)

    select = rt.newinst('selectn', 'select%i.extruder-sel' % extruders,
                        pincount=extruders)
    hal.addf(select.name, thread)
    for n in range(0, extruders):
        select.pin('out%i' % n).link('e%i-enable' % n)
    select.pin('sel').link(extruderSel)

    extruderSel.link('iocontrol.0.tool-prep-number')  # driven by T code
Exemple #33
0
def setup_extruder_multiplexer(extruders, thread):
    extruderSel = hal.Signal('extruder-sel', hal.HAL_S32)

    select = rt.newinst('selectn', 'select%i.extruder-sel' % extruders,
                        pincount=extruders)
    hal.addf(select.name, thread)
    for n in range(0, extruders):
        select.pin('out%i' % n).link('e%i-enable' % n)
    select.pin('sel').link(extruderSel)

    extruderSel.link('iocontrol.0.tool-prep-number')  # driven by T code
Exemple #34
0
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)
Exemple #35
0
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)
Exemple #37
0
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()
Exemple #38
0
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)
Exemple #39
0
    def _setup_estop_chain(error_signals, thread):
        reset = hal.Signal('reset', hal.HAL_BIT)
        estop_active = hal.Signal('estop-active', hal.HAL_BIT)
        estop_error = hal.Signal('estop-error', hal.HAL_BIT)
        estop_in = hal.Signal('estop-in', hal.HAL_BIT)
        ok = hal.Signal('ok', hal.HAL_BIT)

        num = len(error_signals)
        or_comp = rt.newinst('orn',
                             'or{}.estop-error'.format(num),
                             pincount=num)
        hal.addf(or_comp.name, thread.name)
        for n, sig in enumerate(error_signals):
            or_comp.pin('in{}'.format(n)).link(sig)
        or_comp.pin('out').link(estop_error)

        estop_latch = rt.newinst('estop_latch', 'estop-latch')
        hal.addf(estop_latch.name, thread.name)
        estop_latch.pin('ok-in').link(estop_in)
        estop_latch.pin('fault-in').link(estop_error)
        estop_latch.pin('fault-out').link(estop_active)
        estop_latch.pin('reset').link(reset)
        estop_latch.pin('ok-out').link(ok)
Exemple #40
0
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_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()
Exemple #42
0
def usrcomp_watchdog(comps, enableSignal, thread,
                     okSignal=None, errorSignal=None):
    count = len(comps)
    watchdog = rt.newinst('watchdog', 'watchdog.usrcomp', pincount=count)
    hal.addf('%s.set-timeouts' % watchdog.name, thread)
    hal.addf('%s.process' % watchdog.name, 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-%02i' % n).link(sigIn)
        watchdog.pin('timeout-%02i' % 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)
Exemple #43
0
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)
Exemple #44
0
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 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()
Exemple #46
0
# setup axis feedback
limits = [
    [c.find('AXIS_0', 'MAX_LIMIT') - 0.2, c.find('AXIS_0', 'MAX_LIMIT')],
    [c.find('AXIS_1', 'MAX_LIMIT') - 0.2, c.find('AXIS_1', 'MAX_LIMIT')],
    [c.find('AXIS_2', 'MIN_LIMIT'), 0.0]
]

for n in range(c.find('TRAJ', 'AXES')):
    axisPos = hal.newsig('axis-%i-pos' % n, hal.HAL_FLOAT)
    hal.Pin('axis.%i.motor-pos-cmd' % n).link(axisPos)
    hal.Pin('axis.%i.motor-pos-fb' % n).link(axisPos)

    # fake limits
    limitHome = hal.newsig('limit-%i-home' % n, hal.HAL_BIT)
    hal.Pin('axis.%i.home-sw-in' % n).link(limitHome)
    wcomp = rt.newinst('wcomp', 'wcomp.axis-%i-home' % n)
    hal.addf(wcomp.name, 'servo-thread')
    wcomp.pin('in').link(axisPos)
    wcomp.pin('out').link(limitHome)
    wcomp.pin('min').set(limits[n][0])
    wcomp.pin('max').set(limits[n][1])

numFans = int(c.find('FDM', 'NUM_FANS'))
numExtruders = int(c.find('FDM', 'NUM_EXTRUDERS'))
numLights = int(c.find('FDM', 'NUM_LIGHTS'))

ve.velocity_extrusion(extruders=numExtruders, thread='servo-thread')

# Fans
for i in range(0, numFans):
    base.setup_fan('f%i' % i, thread='servo-thread')
    def __init__(self, temp_hal, pwm_pin, on_pin, 
                 enable_sig, estop_reset_sig, 
                 config_name, comp_name, hal_name, motion_pin_number):
        self.temp_hal = temp_hal

        comp = hal.RemoteComponent(comp_name, timer=100)
        comp_limit_min_pin = comp.newpin('temp.limit.min', hal.HAL_FLOAT, hal.HAL_IN)
        comp_limit_max_pin = comp.newpin('temp.limit.max', hal.HAL_FLOAT, hal.HAL_IN)
        comp_standby_pin = comp.newpin('temp.standby', hal.HAL_FLOAT, hal.HAL_IN)
        comp_meas_pin = comp.newpin('temp.meas', hal.HAL_FLOAT, hal.HAL_IN)
        comp_set_pin = comp.newpin('temp.set', hal.HAL_FLOAT, hal.HAL_IO)
        comp_in_range_pin = comp.newpin('temp.in-range', hal.HAL_BIT, hal.HAL_IN)
        comp_active_pin = comp.newpin('active', hal.HAL_BIT, hal.HAL_IN)
        comp_error_pin = comp.newpin('error', hal.HAL_BIT, hal.HAL_IN)
        comp.ready()

        # -- General Signals --
        meas_sig = hal.newsig('%s.meas' % hal_name, hal.HAL_FLOAT)
        meas_sig.link(temp_hal.pin('value'))
        meas_sig.link(comp_meas_pin)
        meas_sig.link('motion.analog-in-%02d' % motion_pin_number)

        set_sig = hal.newsig('%s.set' % hal_name, hal.HAL_FLOAT)
        set_sig.link(comp_set_pin)
        set_sig.link('motion.analog-out-io-%02d' % motion_pin_number)

        limit_min_sig = hal.newsig('%s.limit.min' % hal_name, hal.HAL_FLOAT)
        limit_min_sig.link(comp_limit_min_pin)
        limit_min_sig.set(config.find(config_name, 'TEMP_LIMIT_MIN', 0.0))
        
        limit_max_sig = hal.newsig('%s.limit.max' % hal_name, hal.HAL_FLOAT)
        limit_max_sig.link(comp_limit_max_pin)
        limit_max_sig.set(config.find(config_name, 'TEMP_LIMIT_MAX', 30.0))

        standby_sig = hal.newsig('%s.standby' % hal_name, hal.HAL_FLOAT)
        standby_sig.link(comp_standby_pin)
        standby_sig.set(config.find(config_name, 'TEMP_STANDBY', 15.0))

        on_sig = hal.newsig('%s.on' % hal_name, hal.HAL_BIT)
        on_sig.link(on_pin)
        on_sig.link(comp_active_pin)

        error_sig = hal.newsig('%s.error' % hal_name, hal.HAL_BIT)
        self.error_sig = error_sig
        error_sig.link(comp_error_pin)

        self.temp_watchdog_sig = hal.newsig('%s.watchdog' % hal_name, hal.HAL_BIT)
        temp_hal.pin('watchdog').link(self.temp_watchdog_sig)

        # -- Measurement --

        # In-Range Signal
        in_range_sig = hal.newsig('%s.in-range' % hal_name, hal.HAL_BIT)
        in_range_sig.link(comp_in_range_pin)
        in_range_sig.link('motion.digital-in-%02d' % motion_pin_number)

        range_lb_sum = rtapi.newinst('sum2', '%s.range.lb.sum2' % hal_name)
        hal.addf(range_lb_sum.name, SERVO_THREAD)
        range_ub_sum = rtapi.newinst('sum2', '%s.range.ub.sum2' % hal_name)
        hal.addf(range_ub_sum.name, SERVO_THREAD)
        range_lb_sig = hal.newsig('%s.range.lb' % hal_name, hal.HAL_FLOAT)
        range_ub_sig = hal.newsig('%s.range.ub' % hal_name, hal.HAL_FLOAT)

        range_lb_sum.pin('in0').link(set_sig)
        range_lb_sum.pin('in1').set(float(config.find(config_name, 'TEMP_RANGE_NEG_ERROR', -1.0)))
        range_lb_sum.pin('out').link(range_lb_sig)
        range_ub_sum.pin('in0').link(set_sig)
        range_ub_sum.pin('in1').set(float(config.find(config_name, 'TEMP_RANGE_POS_ERROR', 1.0)))
        range_ub_sum.pin('out').link(range_ub_sig)

        range_wcomp = rtapi.newinst('wcomp', '%s.range.wcomp' % hal_name)
        hal.addf(range_wcomp.name, SERVO_THREAD)
        range_wcomp.pin('min').link(range_lb_sig)
        range_wcomp.pin('max').link(range_ub_sig)
        range_wcomp.pin('in').link(meas_sig)
        range_wcomp.pin('out').link(in_range_sig)

        # -- Output --
        pwm_raw_sig = hal.newsig('%s.pwm_raw' % hal_name, hal.HAL_FLOAT)
        pwm_sig = hal.newsig('%s.pwm' % hal_name, hal.HAL_FLOAT)
        pwm_max = float(config.find(config_name, 'PWM_MAX', 1.0))

        # PID
        pid = rtapi.newinst('at_pid', '%s.pid' % hal_name)
        hal.addf(pid.name + '.do-pid-calcs', SERVO_THREAD)
        pid.pin('enable').link(enable_sig)
        pid.pin('feedback').link(meas_sig)
        pid.pin('command').link(set_sig)
        pid.pin('output').link(pwm_raw_sig)
        pid.pin('maxoutput').set(pwm_max)
        pid.pin('Pgain').set(float(config.find(config_name, 'PID_PGAIN', 0)))
        pid.pin('Igain').set(float(config.find(config_name, 'PID_IGAIN', 0)))
        pid.pin('Dgain').set(float(config.find(config_name, 'PID_DGAIN', 0)))
        pid.pin('maxerrorI').set(float(config.find(config_name, 'PID_MAXERRORI', 1.0)))
        pid.pin('bias').set(float(config.find(config_name, 'PID_BIAS', 0.0)))

        # PWM Limit (PID can output negative values)
        pwm_limit = rtapi.newinst('limit1', '%s.limit1.pwm' % hal_name)
        hal.addf(pwm_limit.name, SERVO_THREAD)
        pwm_limit.pin('min').set(0.0)
        pwm_limit.pin('max').set(pwm_max)
        pwm_limit.pin('in').link(pwm_raw_sig)
        pwm_limit.pin('out').link(pwm_sig)

        pwm_pin.link(pwm_sig)

        # -- Safety Check --
        check_limit_ok_sig = hal.newsig('%s.check.limit-ok' % hal_name, hal.HAL_BIT)

        limit_wcomp = rtapi.newinst('wcomp', '%s.check.limit.wcomp' % hal_name)
        hal.addf(limit_wcomp.name, SERVO_THREAD)
        limit_wcomp.pin('min').link(limit_min_sig)
        limit_wcomp.pin('max').link(limit_max_sig)
        limit_wcomp.pin('in').link(meas_sig)
        limit_wcomp.pin('out').link(check_limit_ok_sig)

        out_range_sig = hal.newsig('%s.out-range' % hal_name, hal.HAL_BIT)
        out_range_not = rtapi.newinst('not', '%s.out-range.not' % hal_name)
        hal.addf(out_range_not.name, SERVO_THREAD)
        out_range_not.pin('in').link(in_range_sig)
        out_range_not.pin('out').link(out_range_sig)

        thermistor_check_enable_sig = hal.newsig('%s.check.therm.enable' % hal_name, hal.HAL_BIT)
        thermistor_check_enable_and = rtapi.newinst('and2', '%s.check.therm.enable.and2' % hal_name)
        hal.addf(thermistor_check_enable_and.name, SERVO_THREAD)
        thermistor_check_enable_and.pin('in0').link(enable_sig)
        thermistor_check_enable_and.pin('in1').link(out_range_sig)
        thermistor_check_enable_and.pin('out').link(thermistor_check_enable_sig)

        check_thermistor_ok_sig = hal.newsig('%s.check.therm-ok' % hal_name, hal.HAL_BIT)
        thermistor_check = rtapi.newinst('thermistor_check', '%s.check.therm-check' % hal_name)
        hal.addf(thermistor_check.name, SERVO_THREAD)
        thermistor_check.pin('enable').link(thermistor_check_enable_sig)
        thermistor_check.pin('pid').link(pwm_sig)
        thermistor_check.pin('temp').link(meas_sig)
        thermistor_check.pin('min-pid').set(float(config.find(config_name, 'CHECK_MIN_PID', 0.25)))
        thermistor_check.pin('min-temp').set(float(config.find(config_name, 'CHECK_MIN_TEMP', 2.0)))
        thermistor_check.pin('wait').set(float(config.find(config_name, 'CHECK_WAIT', 30.0)))
        thermistor_check.pin('no-error').link(check_thermistor_ok_sig)

        check_all_ok_sig = hal.newsig('%s.check.all-ok' % hal_name, hal.HAL_BIT)
        check_all_ok = rtapi.newinst('and2', '%s.check.all.and2' % hal_name)
        hal.addf(check_all_ok.name, SERVO_THREAD)
        check_all_ok.pin('in0').link(check_limit_ok_sig)
        check_all_ok.pin('in1').link(check_thermistor_ok_sig)
        check_all_ok.pin('out').link(check_all_ok_sig)

        check_error_sig = hal.newsig('%s.check.error' % hal_name, hal.HAL_BIT)
        check_error = rtapi.newinst('not', '%s.check.error.not' % hal_name)
        hal.addf(check_error.name, SERVO_THREAD)
        check_error.pin('in').link(check_all_ok_sig)
        check_error.pin('out').link(check_error_sig)

        error_latch = rtapi.newinst('flipflop', '%s.check.error.flipflop' % hal_name)
        hal.addf(error_latch.name, SERVO_THREAD)
        error_latch.pin('set').link(check_error_sig)
        error_latch.pin('reset').link(estop_reset_sig)
        error_latch.pin('out').link(error_sig)
Exemple #48
0
def setup_joints():
    """
    Create joints commands and feedbacks signal for the system
    """
    commands = [None] * AXIS_TOTAL
    feedbacks = [None] * AXIS_TOTAL
    for i in xrange(AXIS_TOTAL):
        commands[i] = hal.newsig('machine.joint.%d.command' % i, hal.HAL_FLOAT)
        feedbacks[i] = hal.newsig('machine.joint.%d.feedback' % i, hal.HAL_FLOAT)

    core_xy = config.find('FDM','CORE_XY')
    if core_xy is not None and int(core_xy) > 0:
        sum_cmd_a = rtapi.newinst('sum2', 'corexy.sum2.cmd.a')
        sum_fb_x = rtapi.newinst('sum2', 'corexy.sum2.fb.x')
        sum_cmd_b = rtapi.newinst('sum2', 'corexy.sum2.cmd.b')
        sum_fb_y = rtapi.newinst('sum2', 'corexy.sum2.fb.y')
        hal.addf(sum_cmd_a.name, SERVO_THREAD)
        hal.addf(sum_cmd_b.name, SERVO_THREAD)
        hal.addf(sum_fb_x.name, SERVO_THREAD)
        hal.addf(sum_fb_y.name, SERVO_THREAD)

        sum_cmd_a.pin('gain0').set(1)
        sum_cmd_a.pin('gain1').set(1)
        sum_cmd_b.pin('gain0').set(1)
        sum_cmd_b.pin('gain1').set(-1)

        sum_fb_x.pin('gain0').set(0.5)
        sum_fb_x.pin('gain1').set(0.5)
        sum_fb_y.pin('gain0').set(0.5)
        sum_fb_y.pin('gain1').set(-0.5)

        corex_cmd = hal.newsig('machine.joint.corex.command', hal.HAL_FLOAT)
        corey_cmd = hal.newsig('machine.joint.corey.command', hal.HAL_FLOAT)

        corex_cmd.link('axis.0.motor-pos-cmd')
        corey_cmd.link('axis.1.motor-pos-cmd')

        sum_cmd_a.pin('in0').link(corex_cmd)
        sum_cmd_a.pin('in1').link(corey_cmd)
        sum_cmd_b.pin('in0').link(corex_cmd)
        sum_cmd_b.pin('in1').link(corey_cmd)
        sum_cmd_a.pin('out').link(commands[0])
        sum_cmd_b.pin('out').link(commands[1])

        sum_fb_x.pin('in0').link(feedbacks[0])
        sum_fb_x.pin('in1').link(feedbacks[1])
        sum_fb_y.pin('in0').link(feedbacks[0])
        sum_fb_y.pin('in1').link(feedbacks[1])
        sum_fb_x.pin('out').link('axis.0.motor-pos-fb')
        sum_fb_y.pin('out').link('axis.1.motor-pos-fb')
    else:
        commands[0].link('axis.0.motor-pos-cmd')
        feedbacks[0].link('axis.0.motor-pos-fb')
        commands[1].link('axis.1.motor-pos-cmd')
        feedbacks[1].link('axis.1.motor-pos-fb')

    for i in xrange(AXIS_TOTAL):
        if i >= 2:
            commands[i].link('axis.%d.motor-pos-cmd' % i)
            feedbacks[i].link('axis.%d.motor-pos-fb' % i)

    return (commands, feedbacks)
Exemple #49
0
def setup_extruders(replicape, extruder_sel_sig):
    motion_vel_sig = hal.newsig('ve-motion-vel', hal.HAL_FLOAT)
    motion_vel_sig.link('motion.current-vel')
        
    extruders = [None] * EXTRUDER_TOTAL
    for i in xrange(EXTRUDER_TOTAL):
        extruders[i] = Extruder(i, motion_vel_sig, config)
        setup_extruder(replicape, extruders[i], i)

    shared_signals = [
        # Shared Signal   Type           Attribute            fdm-ve-jog     fdm-ve-params      analog-io
        ('enable',        hal.HAL_BIT,   'enable_sig',        None,          None,              None),
        ('cross-section', hal.HAL_FLOAT, 'cross_section_sig', None,          None,              41),
        ('jog-vel',       hal.HAL_FLOAT, 'jog_vel_sig',       'velocity',    None,              None),
        ('jog-direction', hal.HAL_BIT,   'jog_direction_sig', 'direction',   None,              None),
        ('jog-distance',  hal.HAL_FLOAT, 'jog_distance_sig',  'distance',    None,              None),
        ('jog-trigger',   hal.HAL_BIT,   'jog_trigger_sig',   'trigger',     None,              None),
        ('jog-continuous',hal.HAL_BIT,   'jog_continuous_sig','continuous',  None,              None),
        ('jog-dtg',       hal.HAL_FLOAT, 'jog_dtg_sig',       'dtg',         None,              None),
        ('max-jog-vel',   hal.HAL_FLOAT, 'max_jog_vel_sig',   'max-velocity',None,              None),
        ('filament-dia',  hal.HAL_FLOAT, 'filament_dia_sig',  None,          'filament-dia',    44),
        ('extrude-scale', hal.HAL_FLOAT, 'extrude_scale_sig', None,          'extrude-scale',   None),
        ('accel-gain',    hal.HAL_FLOAT, 'accel_gain_sig',    None,          'accel-adj-gain',  None),
        ('retract-vel',   hal.HAL_FLOAT, 'retract_vel_sig',   None,          'retract-vel',     None),
        ('retract-len',   hal.HAL_FLOAT, 'retract_len_sig',   None,          'retract-len',     None),

        ('extrude-vel',   hal.HAL_FLOAT, 'extrude_vel_sig',   None,          None,              None),
        ('retracting',    hal.HAL_BIT  , 'retracting_sig',    None,          None,              None),
    ]

    comp_jog = hal.RemoteComponent('fdm-ve-jog', timer=100)
    comp_params = hal.RemoteComponent('fdm-ve-params', timer=100)

    for (signal_name, signal_type, attr_name, comp_jog_name, comp_params_name, analog_io_pin) in shared_signals:
        signal = hal.newsig('ve.%s' % signal_name, signal_type)
        comp_dir_type = hal.HAL_IO

        if signal_name == 'jog-dtg' or signal_name == 'max-jog-vel' or signal_name == 'extrude-vel':
            mux_type = 'muxn' 
            comp_dir_type = hal.HAL_IN
        elif signal_name == 'retracting':
            mux_type = 'muxn_bit' 
            comp_dir_type = hal.HAL_IN
        else:
            mux_type = 'io_muxn' if signal_type == hal.HAL_FLOAT else 'io_muxn_bit'

        mux = rtapi.newinst(
            mux_type,
            'ex.mux%d.%s' % (EXTRUDER_TOTAL, signal_name), 
            pincount=EXTRUDER_TOTAL)
        hal.addf(mux.name, SERVO_THREAD)
        mux.pin('out').link(signal)
        mux.pin('sel').link(extruder_sel_sig)
        for j in xrange(EXTRUDER_TOTAL):
            mux.pin('in%i' % j).link(getattr(extruders[j], attr_name))

        if comp_jog_name is not None:
            pin = comp_jog.newpin(comp_jog_name, signal_type, comp_dir_type)
            pin.link(signal)
        if comp_params_name is not None:
            pin = comp_params.newpin(comp_params_name, signal_type, comp_dir_type)
            pin.link(signal)
        if analog_io_pin is not None:
            signal.link('motion.analog-out-io-%d' % analog_io_pin)
        if signal_name == 'enable':
            signal.link('motion.digital-out-io-01')
            
    hal.Signal('ve.retracting').link('motion.feed-hold')

    extruder_sel_sig.link('iocontrol.0.tool-prep-number')
    comp_jog.newpin('extruder-sel', hal.HAL_S32, hal.HAL_IN).link(extruder_sel_sig)
    comp_jog.newpin('extruder-count', hal.HAL_U32, hal.HAL_IN).set(EXTRUDER_TOTAL)

    comp_jog.ready()
    comp_params.ready()
Exemple #50
0
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_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)
    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)
Exemple #53
0
    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 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)
Exemple #55
0
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
Exemple #56
0
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'))