PIO program for quadrature encoder and absolute addresses

[rkompass]

Hello,

I experimented with programming a quadrature encoder for RPI2040 PIO, inspired by
https://github.com/GitJer/Rotary_encode ... ncoder.pio and later
https://github.com/raspberrypi/pico-exa ... ncoder.pio.
I have two solutions now and a number of questions:
First solution:

Code: Select all

# Quadrature encoder for RPi 2040 Pio
# Has to be at address 0 of PIO programm space
#
# Original version (c) 2021 pmarques-dev @ github
# (https://github.com/raspberrypi/pico-examples/blob/master/pio/quadrature_encoder/quadrature_encoder.pio)
# Adapted and modified for micropython 2022 by rkompass
#
# SPDX-License-Identifier: BSD-3-Clause
#
# This program was reduced to take 'only' 24 of 32 available PIO instructions. 
# 
# Quadrature encoding uses a state table in form of a jump table
#   which is fast and has no interrupts.
# The counter x is permanently pushed nonblockingly to the FIFO.
# To read the actual value empty the FIFO then wait for and get the next pushed value.

# The worst case sampling loop takes 14 cycles, so this program is able to read step
#   rates up to sysclk / 14  (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec).

from machine import Pin
from rp2 import PIO, StateMachine, asm_pio
from time import sleep_ms

class PIO_QENC:
    def __init__(self, sm_id, pins, freq=10_000_000):
        if not isinstance(pins, (tuple, list)) or len(pins) != 2:
            raise ValueError('2 successive pins required')
        pinA = int(str(pins[0]).split(')')[0].split('(')[1].split(',')[0])
        pinB = int(str(pins[1]).split(')')[0].split('(')[1].split(',')[0])
        if abs(pinA-pinB) != 1:
            raise ValueError('2 successive pins required')
        in_base = pins[0] if pinA < pinB else pins[1]
        self.sm_qenc = StateMachine(sm_id, self.sm_qenc, freq=freq, in_base=in_base, out_base=in_base)
        self.sm_qenc.exec("set(x, 1)")  # we once decrement at the start
        self.sm_qenc.exec("in_(pins, 2)")
        self.sm_qenc.active(1)
    
    @staticmethod
    @rp2.asm_pio(in_shiftdir=PIO.SHIFT_LEFT, out_shiftdir=PIO.SHIFT_RIGHT)
    def sm_qenc():
        jmp("read")        # 0000 : from 00 to 00 = no change
        jmp("decr")        # 0001 : from 00 to 01 = backward
        jmp("incr")        # 0010 : from 00 to 10 = orward
        jmp("read")        # 0011 : from 00 to 11 = error
        jmp("incr")        # 0100 : from 01 to 00 = forward
        jmp("read")        # 0101 : from 01 to 01 = no change
        jmp("read")        # 0110 : from 01 to 10 = error
        jmp("decr")        # 0111 : from 01 to 11 = backward
        jmp("decr")        # 1000 : from 10 to 00 = backward
        jmp("read")        # 1001 : from 10 to 01 = error
        jmp("read")        # 1010 : from 10 to 10 = no change
        jmp("incr")        # 1011 : from 10 to 11 = forward
        jmp("read")        # 1100 : from 11 to 00 = error
        jmp("incr")        # 1101 : from 11 to 01 = forward

        label("decr")
        jmp(x_dec, "read") # 1110 : from 11 to 10 = backward

        label("read")      # 1111 : from 11 to 11 = no change
        mov(osr, isr)      # save last pin input in OSR
        mov(isr, x)
        push(noblock)
        out(isr, 2)        # 2 right bits of OSR into ISR, all other 0
        in_(pins, 2)       # combined with current reading of input pins
        mov(pc, isr)       # jump into jump-table at addr 0

        label("incr")      # increment x by inverting, decrementing and inverting
        mov(x, invert(x))
        jmp(x_dec, "here")
        label("here")
        mov(x, invert(x))
        jmp("read")
        
        nop()
        nop()
        nop()
        nop()
        nop()
        nop()
        nop()

    def read(self):
        for _ in range(self.sm_qenc.rx_fifo()):
            self.sm_qenc.get()
        n = self.sm_qenc.get()
        return n if n < (1<<31) else n - (1<<32)

pinA = Pin(15, Pin.IN, Pin.PULL_UP)
pinB = Pin(16, Pin.IN, Pin.PULL_UP)

qenc = PIO_QENC(0, (pinA, pinB))
print('starting....')
for i in range(120):
    print('x:', qenc.read())
    sleep_ms(500)
qenc.sm_qenc.active(0)
print('stop')

Here a state table is employed which actually is a jump table.
The jump is

Code: Select all

mov(pc, isr)

I had to fill up the program with nop()s to reach 32 instructions. Only then the program works. All my efforts to reduce program size to allow for a coexistence of more than one programs in PIO apparently are useless.
Apparently the programm is not placed at address 0 by default.

First question: Is there a way in micropython to enforce that the program is placed at address 0? In PIO-code there is ".origin 0" which seems to do that.

A shorter version:

Code: Select all

@rp2.asm_pio(in_shiftdir=PIO.SHIFT_LEFT, out_shiftdir=PIO.SHIFT_RIGHT)
def sm_qenc():
    jmp("read")        # 000 : B from 0 to 0         => no change
    jmp("decr")        # 001 : B from 0 to 1, A = 0  => backward
    jmp("read")        # 010 : B from 0 to 0         => no change
    jmp("incr")        # 011 : B from 0 to 1, A = 1  => forward
    jmp("incr")        # 100 : B from 1 to 0, A = 0  => forward
    jmp("read")        # 101 : B from 0 to 0         => no change

    label("decr")
    jmp(x_dec, "read") # 110 : B from 1 to 0, A = 1  => backward

    label("read")      # 111 : B from 0 to 0         => no change
    mov(osr, isr)      # save last pin input in OSR
    mov(isr, x)
    push(noblock)
    out(isr, 1)        # right bit B' of OSR into ISR, all other 0
    in_(pins, 2)       # combined with current reading A B of input pins

    mov(pc, isr)       # jump into jump-table at addr 0

    label("incr")      # increment x by inverting, decrementing and inverting
    mov(x, invert(x))
    jmp(x_dec, "here")
    label("here")
    mov(x, invert(x))  # we rely on implicit .wrap with micropython
    jmp("read")

takes only 16 instructions at the cost of having only a resolution of 2 in/decrements per rotation cycle. But it also does not work as long as the origin 0 cannot be enforced.

Code: Select all

    @rp2.asm_pio(in_shiftdir=PIO.SHIFT_LEFT, out_shiftdir=PIO.SHIFT_RIGHT)
    def sm_qenc():
        label("decr")
        jmp(x_dec, "read") # 110 : B from 1 to 0, A = 1  => backward
        # ---------------
        label("read")      # 111 : B from 0 to 0         => no change
        mov(osr, isr)      # save last pin input in OSR
        mov(isr, x)
        push(noblock)
        out(isr, 1)        # right bit B' of OSR into ISR, all other 0; needs out_shiftdir=PIO.SHIFT_RIGHT
        in_(pins, 2)       # combined with current reading A B of input pins
        # ---------------
        mov(y, isr)         # use y to perform different jumps
        jmp(y_dec, "next1")
        label("next1")
        jmp(not_y, "decr")  # isr = 001 : B from 0 to 1, A = 0  => backward
        jmp(y_dec, "next2")
        label("next2")
        jmp(y_dec, "next3")
        label("next3")
        jmp(not_y, "incr")  # isr = 011 : B from 0 to 1, A = 1  => forward
        jmp(y_dec, "next4")
        label("next4")
        jmp(not_y, "incr")  # isr = 100 : B from 1 to 0, A = 0  => forward
        jmp(y_dec, "next5")
        label("next5")
        jmp(y_dec, "next6")
        label("next6")
        jmp(not_y, "decr")  # isr = 110 : B from 1 to 0, A = 1  => backward
        jmp("read")
        # ---------------
        label("incr")      # increment x by inverting, decrementing and inverting
        mov(x, invert(x))
        jmp(x_dec, "here")
        label("here")
        mov(x, invert(x))  # we rely on implicit .wrap with micropython
        jmp("read")

again works. The jump table is now replaced by a sequence of decrement-and 0-comparison-jump's.

More questions:
When I print the state machine I get:

Code: Select all

[array('H', [65, 41190, 41153, 32768, 24769, 16386, 41030, 136, 96, 138, 139, 114, 141, 114, 143, 144, 96, 1, 41001, 84, 41001, 1]), 10, -1, 86016, 524288, None, None, None]

The array obviously is the PIO inscruction code. What are the other elements?
Has anybody used

Code: Select all

PIO.add_program(program)

so far? What has to be supplied? The above list?
Is there a way to read out the program from the PIO?
If I add two programms, how do I control which program to activate?
Does this automatically happen when I instantiate two state machines with rp2.StateMachine and PIO id.s of the same block e.g. 0 and 1?

TLDR: We now have two micropython quadrature (rotary) encoders in almost-hardware on RPI2040.
And many questions:-) Thank you for answers and hints. Raul

[pythoncoder]

I don't know the answer to these questions but you might be interested in this solution. The PIO code was developed by Sandor Attila Gerendi (@sanyi). This link describes the development of the class.

[Roberthh]

The array obviously is the PIO inscruction code. What are the other elements?

See modules/rp2.py, line 52:

Code: Select all

        self.prog = [array("H"), -1, -1, execctrl, shiftctrl, out_init, set_init, sideset_init]

As far as I remember, the PIO RAM is filled top-down when you load state machines. That could be the reason why you need it to be 32 words to get the jump table at address 0.

[Roberthh]

There are quite a few quadrature encoder/counter implementations around, and there is no clear direction which way to go. There was a suggestion for a python level API, written down by IhorNehrutsa (see. https://github.com/micropython/micropython/pull/8072), and there are 3 PRs for ESP32, a few generic Python implementations, one for MIMXRT and one for STM32 (hidden in the Timer class). Only the pure Python and STM32 implementations are available. So your code joins the Python group. You did not show the API of your implementation. But maybe it's still under construction.

[rkompass]

Peter and Robert,

thank you very much for code and explanations. In Peters encoder code the changed X,Y values
which lead to the interrupt are directly pushed and evaluated in the callback.
So the problem which I mentioned last time (taking new values of X and Y in the callback)
should not exist here. Every change of X or Y leads to an interrupt though. As a consequence the
maximum change rate is limited by the processing speed of interrupts.
Question: Does the RPi2040 under uPy have hard interrupts?
The PIO codes presented here are much longer but have the following advantage:
The up/down counting is done in the PIO (in x register).
The value() class member function just reads the last counter value out.
It has to read any previously (nonblockingly) pushed old counter value before reading
the actual value, thats all. No interrupt.
The maximum possible X,Y change rate should be several MHz.

Robert, thank you for pointing to Ihor Nehrutsas pull request.
I was not able yet to study it in full, but the idea to collaborate here I can only support.
I will try to study the ESP32 solutions, which I assume are using the hardware encoders.
Question: Does the MIMXRT have a hardware quadrature encoder?
If yes then only the ESP8266 remains having no fast (hardware or PIO) quadrature encoder.
I would suggest to equip it with a fast interrupt driven encoder in firmware.
I could contribute to development of the core of it in C, but have no oversight of
the intrinsics of uPy firmware. Even do not master Git yet, which should be my next learning step.
Only nice idea for a state table interrupt routine which even allows for dropping single bit
changes whithout miscounting.

Question: Is it possible to have a transparent coexistence of firmware and python code for the encoder in the
different ports?
A uniform API of course is desirable because what we want is to enable the kids just to import
the encoder, plug in the pins and then concentrate on the car.
Perhaps us concentrate on PID controllers in uPy.

My Python-API is in the first code block:

Code: Select all

class PIO_QENC:
    def __init__(self, sm_id, pins, freq=10_000_000):
        if not isinstance(pins, (tuple, list)) or len(pins) != 2:
            raise ValueError('tuple of 2 successive pins required')
        pinA = int(str(pins[0]).split(')')[0].split('(')[1].split(',')[0])
        ....

You have to deliver both pins as PIN objects.
qenc = PIO_QENC(0, (pinA, pinB)) instantiates the class, qenc.read() reads the value.
At present there is no function for releasing the PIO, but I may program one.
qenc.sm_qenc.active(0) stops the counter.
I will try to refit the API as soon as there is an agreement of how the uniform API has to look.
Perhaps a very simple management of Timer, Counter, PIO ressources will be necessary?.

[rkompass]

I only now see https://github.com/micropython/micropython/pull/6894. Shall I repeat the above and discuss there?

[Roberthh]

Robert, thank you for pointing to Ihor Nehrutsas pull request.

That was the result of a discussion about a common basic API. Each port may extend that with port specific properties.

Does the MIMXRT have a hardware quadrature encoder?

Yes. The encoder/counter are implemented in hardware. Encoder speed ranges up to 25MHz, counter up to 50 MHz.

About the ESP8266: for hand operated encoders the Python Encoder version of @pythoncoder is sufficient.

[pythoncoder]

...Every change of X or Y leads to an interrupt though...

This is necessary to achieve correct decoding. See this doc where I discuss the algorithm and how it handles contact bounce/vibration.

On most platforms processing speed is actually limited by the interrupt latency rather than the duration of the ISR.