Skip to content

Instantly share code, notes, and snippets.

@machinekoder

machinekoder/rc_servo_machinekit.py

Last active August 22, 2018 21:41
Download ZIP

Revisions

  1. machinekoder revised this gist Aug 22, 2018. 1 changed file with 2 additions and 2 deletions.
    4 changes: 2 additions & 2 deletions rc_servo_machinekit.py
    View file
    Original file line number Diff line number Diff line change
    @@ -8,8 +8,8 @@
    def rc_servo(thread, name, range_ms, offset_ms, period_ms, min_step, max_step, in_signal, out_signal):
    gain = range_ms / pwm_ms / (max_step - min_step)
    offset = value_ms / pwm_ms
    min_limit = offset - (gain * min_step)
    max_limit = offset - (gain * max_step)
    min_limit = offset + (gain * min_step)
    max_limit = offset + (gain * max_step)

    scale = rt.newinst('scale', 'scale_{}'.format(name))
    hal.addf(scale.name, thread)
  2. machinekoder created this gist Aug 22, 2018.
    44 changes: 44 additions & 0 deletions rc_servo_machinekit.py
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,44 @@

    # Turn commanded position -5 to +5 into 1-2 mS pulse for RC (hobby) servo
    # gain = 1 mS (range) / 50 mS (PWM period) / 10 (units)= 0.002
    # offset = 1.5 mS (value) / 50 mS (PWM period) = 0.03
    # Resulting PWM values should be between 0.02 and 0.04, representing a
    # 1 mS to 2 mS wide pulse

    def rc_servo(thread, name, range_ms, offset_ms, period_ms, min_step, max_step, in_signal, out_signal):
    gain = range_ms / pwm_ms / (max_step - min_step)
    offset = value_ms / pwm_ms
    min_limit = offset - (gain * min_step)
    max_limit = offset - (gain * max_step)

    scale = rt.newinst('scale', 'scale_{}'.format(name))
    hal.addf(scale.name, thread)
    limit = rt.newinst('limit', 'limit_{}'.format(name))
    hal.addf(limit.name, thread)

    scale.pin('gain').set(gain)
    scale.pin('offset').set(offset)
    scale.pin('in').link(in_signal)
    scale.pin('out').link(limit.pin('in'))

    limit.pin('min').set(0.02)
    limit.pin('max').set(0.04)
    limit.pin('out').link(out_signal)


    z_pos_cmd = hal.Signal('z-pos-cmd', hal.HAL_FLOAT)
    pins['emcmot.02.pos-cmd'].link(z_pos_cmd)
    z_pwm_out = hal.Signal('z-pwm-out', hal.HAL_FLOAT)
    pins['hpg.pwmgen.00.out.00.value'].link(z_pwm_out)

    rc_servo(
    thread='servo-thread',
    name='z',
    range_ms=1,
    offset_ms=1.5,
    period_ms=50,
    min_step=-5,
    max_step=5,
    in_signal=z_pos_cmd,
    out_signal=z_pwm_out,
    )