"""Wheel encoder driver, using Timer Input Capture
Manage the wheel encoders using 'Input Capture' for microsecond level measurement.
..
TODO: Add to this module or separate module calculations of floating-point fractional-tick values:
current velocity (ticks/s)
distance traveled over specified interval (ticks)
current acceleration (ticks/s/s)
"""
import botcore
import pyb
import time
import array
import stm
#import micropython
#micropython.alloc_emergency_exception_buf(100)
[docs]class CaptureEncoder:
"""Input-capture based handler for single encoder. Maintains circular buffer of 32-bit samples."""
def __init__(self, tmr_channel, max_samples):
self.ch = tmr_channel
self.ch.callback(self.cb)
self.max_samples = max_samples
self.samples = array.array('L', range(max_samples)) # Fast array of 32-bit unsigned ints
self.i_next = 0
self.tick_count = 0
def cb(self, tmr):
self.samples[self.i_next] = self.ch.capture()
self.i_next = (self.i_next + 1) % self.max_samples
self.tick_count += 1
[docs]class TimedEncoders:
"""Manage the wheel encoders using 'Input Capture' for microsecond level measurement.
- Call 'snapshot()' then grab 'data(n)' to get historical (ticks,timestamps) for an encoder.
- The 'max_samples' value just needs to be large enough to accommodate the longest interval over
which you'll want to calculate distance. For example, if you're updating a PID control every 100ms
you'd ideally want enough samples to cover that interval. At that rate, 20 samples covers a full
revolution (100ms) at theoretical top-speed of 10rev/s.
- The 'ticks' value is an ever-increasing integer. Starting at zero, I'm pretty sure it won't rollover
for the life of the motors. (a 32 bit integer would cover 532,380 miles)
Implementation details:
* Buffers last 'max_samples' timestamps of each wheel encoder. (circular buffer)
* Running 'tick_count' for each encoder.
* Index 'i_next' locates tail (oldest entry) of circular buffer.
Notes:
* There are min(tick_count, max_samples) of valid data in buffers.
(Don't just assume you have 'max_samples' valid data in buffer!)
"""
def __init__(self, max_samples=20):
self.max_samples = max_samples
# Snapshot data
self.samples = (0,0)
self.ticks = ()
self.i_next = ()
# Encoders capture Timer5 counts. Run at 1MHz, with max ARR value this function seems to handle.
tim5 = pyb.Timer(5, prescaler=79, period=0x3FFFFFFF)
# TODO: Use stm module to set ARR directly to max count
#stm.mem32[stm.TIM5 + stm.TIM_ARR] = 0xffffffff
enc_l_pin = pyb.Pin(pyb.Pin.cpu.A1, pyb.Pin.IN, af=pyb.Pin.AF2_TIM5)
enc_l_ch = tim5.channel(2, mode=pyb.Timer.IC, pin=enc_l_pin, polarity=pyb.Timer.RISING)
self.enc_l = CaptureEncoder(enc_l_ch, max_samples)
enc_r_pin = pyb.Pin(pyb.Pin.cpu.A2, pyb.Pin.IN, af=pyb.Pin.AF2_TIM5)
enc_r_ch = tim5.channel(3, mode=pyb.Timer.IC, pin=enc_r_pin, polarity=pyb.Timer.RISING)
self.enc_r = CaptureEncoder(enc_r_ch, max_samples)
# Enable digital filtering for input capture channels. On the fringes of encoder slots there
# can be noise on the signal. Worse if encoder disc isn't fully seated, causing marginal signal.
stm.mem32[stm.TIM5 + stm.TIM_CCMR1] |= 0x0000F000 # IC2F (CH2 filter)
stm.mem32[stm.TIM5 + stm.TIM_CCMR2] |= 0x000000F0 # IC3F (CH3 filter)
#@micropython.native
[docs] def snapshot(self):
"""Copy sample buffers from encoders, so we can work with them outside interrupt context"""
q = pyb.disable_irq()
self.samples = (self.enc_l.samples[:], self.enc_r.samples[:])
self.ticks = (self.enc_l.tick_count, self.enc_r.tick_count)
self.i_next = (self.enc_l.i_next, self.enc_r.i_next)
pyb.enable_irq(q)
[docs] def data(self, n):
"""Get encoder data lists: oldest to newest. Requires a `snapshot` prior to calling.
Returns two equal length lists: tick counts and timestamps of each sample captured respectively.
Args:
n (int): Which encoder (0=LEFT, 1=RIGHT)
Returns:
tuple (ticks, samples) containing latest snapshot data lists for given encoder.
"""
# Requires snapshot() data
if not self.samples[n]:
return None
samp = self.samples[n]
ticks = self.ticks[n]
i_next = self.i_next[n] if ticks > self.max_samples else 0
hist = []
for i in range(min(ticks, self.max_samples)):
hist.append(samp[i_next])
i_next = (i_next + 1) % self.max_samples
return (ticks, hist)
[docs] def t_diff(self, sample_new, sample_old):
"""Calculate time difference (uS) between samples, accounting for rollover."""
diff = sample_new - sample_old
return diff if diff >= 0 else (0x3FFFFFFF + diff)
[docs] def t_cur(self):
"""Get the current microsecond count"""
return self.tim5.counter()
[docs] def dump(self):
"""Take a snapshot and print both encoder data-capture records to the console."""
self.snapshot()
(ticks, samples) = self.data(0)
print("LEFT: ticks={}, samples={}".format(ticks, samples))
(ticks, samples) = self.data(1)
print("RIGHT: ticks={}, samples={}".format(ticks, samples))
[docs] def dump_raw(self):
"""Print raw encoder data-capture values without taking a snapshot."""
print("LEFT: ticks={}".format(self.enc_l.tick_count))
print(" Samples: {}".format(self.enc_l.samples))
print("RIGHT: ticks={}".format(self.enc_r.tick_count))
print(" Samples: {}".format(self.enc_r.samples))
if __name__ == '__main__':
# Test Encoders
botcore.leds.enc_emit(0x03)
te = TimedEncoders()
while True:
if botcore.buttons.is_pressed(0):
break
if botcore.buttons.is_pressed(1):
te.dump()
time.sleep(0.1)