Encoder Readout#
Rotational speed can be measured with quadrature encoders by either counting pulses or measuring the width or period of the pulses at the output from the quadrature encoder. Counting pulses is simple and works well for high rotation speed, but gives inaccurate results at low speed when only few pulses are detected.
By contrast, measuring pulse width works well at low speeds (as long as at least one pulse is detected in the measurement interval), but requires a very fast clock to measure rotational speed at high rpm.
Below we compare the two approaches.
%connect serial:///dev/ttyAMA1
Connected to robot-stm32 @ serial:///dev/ttyAMA1
Setup motors:
from tb6612 import TB6612
from pyb import Pin, Timer
# motor power control
nstby = Pin('NSTBY', mode=Pin.OUT_PP)
nstby.value(0)
# motors
pwm_timer = Timer(8, freq=10_000)
scale = (pwm_timer.period()+1)/100
motor1 = TB6612(
pwm_timer.channel(3, Timer.PWM_INVERTED, pin=Pin('PWM_A')),
scale,
Pin('AIN1', mode=Pin.OUT_PP),
Pin('AIN2', mode=Pin.OUT_PP)
)
motor2 = TB6612(
pwm_timer.channel(1, Timer.PWM_INVERTED, pin=Pin('PWM_B')),
scale,
Pin('BIN1', mode=Pin.OUT_PP),
Pin('BIN2', mode=Pin.OUT_PP)
)
Pulse width measurement based on dhylands/upy-examples.
import pyb, micropython
from encoder import init_encoder, c2
# quadrature counter (motor2)
enc = init_encoder(3, 'ENC_A1', 'ENC_A2', Pin.AF2_TIM4)
# interval counter (motor1)
# increase prescaler value if width overflows at low speeds
ic_tim = pyb.Timer(4, prescaler=32, period=0x0ffff)
ic_pin = pyb.Pin('ENC_B1')
ic_dir = pyb.Pin('ENC_B2')
ic_cha = ic_tim.channel(1, pyb.Timer.IC, pin=ic_pin, polarity=pyb.Timer.BOTH)
ic_start = 0
ic_width = 0
ic_period= 0
def ic_cb(tim):
global ic_start
global ic_width
global ic_period
# Read the GPIO pin to figure out if this was a rising or falling edge
if ic_pin.value():
ic_period = ic_cha.capture() - ic_start
ic_start = ic_cha.capture()
# counter rolled over
if ic_period < 0:
ic_period += 0x10000
# use quadrature pin to determine direction
if not ic_dir.value():
ic_period = -ic_period
else:
# Falling edge - end of the pulse
ic_width = ic_cha.capture() - ic_start
# counter rolled over
if ic_width < 0:
ic_width += 0x10000
# use quadrature pin to determine direction
if ic_dir.value():
ic_width = -ic_width
micropython.alloc_emergency_exception_buf(100)
ic_cha.callback(ic_cb)
from time import sleep
# enable TB6612
nstby.value(1)
# speed vector
a = list(range(8, 20))
a.extend([50, 100])
b = [ -i for i in a ]
b.reverse()
b.extend(a)
for speed in b:
motor1.speed(speed)
motor2.speed(speed)
# let motor speed stabilize
sleep(0.5)
# measure speed in only 10ms
for i in range(2):
enc.counter(0)
ic_width = 0
sleep(0.01)
cnt = 2*c2(enc.counter())
enc.counter(0)
if ic_width != 0:
print(f"duty: {speed:4d} encoder: {cnt:8.2f} interval: {100_000/ic_width:8.2f} period: {200_000/ic_period:8.2f} pw: {ic_width:6d}")
else:
print(f"duty: {speed:4d} encoder: {cnt:8.2f}")
nstby.value(0)
duty: -100 encoder: -100.00 interval: -102.77 period: -104.66 pw: -973
duty: -100 encoder: -92.00 interval: -102.77 period: -106.61 pw: -973
duty: -50 encoder: -46.00 interval: -50.53 period: -49.32 pw: -1979
duty: -50 encoder: -42.00 interval: -50.45 period: -49.16 pw: -1982
duty: -19 encoder: -14.00 interval: -13.31 period: -13.81 pw: -7512
duty: -19 encoder: -14.00 interval: -13.33 period: -13.83 pw: -7501
duty: -18 encoder: -12.00 interval: -12.46 period: -12.51 pw: -8027
duty: -18 encoder: -12.00 interval: -12.42 period: -12.69 pw: -8054
duty: -17 encoder: -12.00 interval: -11.20 period: -11.27 pw: -8929
duty: -17 encoder: -10.00 interval: -11.16 period: -11.41 pw: -8963
duty: -16 encoder: -10.00 interval: -9.76 period: -9.99 pw: -10246
duty: -16 encoder: -10.00 interval: -9.75 period: -10.07 pw: -10255
duty: -15 encoder: -8.00 interval: -8.55 period: -8.85 pw: -11695
duty: -15 encoder: -8.00 interval: -8.66 period: -8.76 pw: -11543
duty: -14 encoder: -8.00 interval: -7.56 period: -7.66 pw: -13229
duty: -14 encoder: -8.00 interval: -7.94 period: -7.71 pw: -12593
duty: -13 encoder: -8.00 interval: -6.39 period: -6.49 pw: -15643
duty: -13 encoder: -8.00
duty: -12 encoder: -6.00 interval: -5.25 period: -5.41 pw: -19043
duty: -12 encoder: -6.00 interval: -5.71 period: -5.49 pw: -17500
duty: -11 encoder: -6.00
duty: -11 encoder: -6.00
duty: -10 encoder: -4.00
duty: -10 encoder: -4.00 interval: -2.87 period: -3.08 pw: -34892
duty: -9 encoder: -4.00 interval: -20.92 period: -18.73 pw: -4781
duty: -9 encoder: -4.00
duty: -8 encoder: 0.00
duty: -8 encoder: 0.00
duty: 8 encoder: 0.00
duty: 8 encoder: 0.00
duty: 9 encoder: 0.00
duty: 9 encoder: 0.00
duty: 10 encoder: 0.00
duty: 10 encoder: 0.00
duty: 11 encoder: 0.00 interval: 4.05 period: 3.73 pw: 24698
duty: 11 encoder: 0.00 interval: 3.46 period: 3.76 pw: 28923
duty: 12 encoder: 0.00 interval: 5.47 period: 5.09 pw: 18289
duty: 12 encoder: 0.00 interval: 4.78 period: 5.24 pw: 20931
duty: 13 encoder: 6.00 interval: 6.66 period: 6.31 pw: 15012
duty: 13 encoder: 6.00
duty: 14 encoder: 8.00 interval: 7.57 period: 7.38 pw: 13212
duty: 14 encoder: 8.00
duty: 15 encoder: 8.00 interval: 9.19 period: 8.65 pw: 10878
duty: 15 encoder: 8.00 interval: 8.72 period: 8.53 pw: 11473
duty: 16 encoder: 10.00 interval: 10.39 period: 10.23 pw: 9626
duty: 16 encoder: 10.00 interval: 9.85 period: 9.77 pw: 10155
duty: 17 encoder: 12.00 interval: 10.99 period: 10.77 pw: 9101
duty: 17 encoder: 12.00 interval: 10.60 period: 10.75 pw: 9432
duty: 18 encoder: 12.00 interval: 12.71 period: 12.15 pw: 7866
duty: 18 encoder: 10.00 interval: 12.08 period: 12.04 pw: 8275
duty: 19 encoder: 14.00 interval: 13.98 period: 13.20 pw: 7152
duty: 19 encoder: 12.00 interval: 13.32 period: 13.24 pw: 7508
duty: 50 encoder: 44.00 interval: 48.38 period: 51.59 pw: 2067
duty: 50 encoder: 40.00 interval: 49.16 period: 48.94 pw: 2034
duty: 100 encoder: 98.00 interval: 114.16 period: 114.03 pw: 876
duty: 100 encoder: 88.00 interval: 107.07 period: 107.87 pw: 934
As expected, pulse width measurement gives more consistent results.