PIO Module

The PIO module is a pretty unique feature of the RP2040 that we used that allows you to basically run mini programs on two smaller processors built into the chip that cut down the amount of processing the main processor has to do. It also helps with reducing ‘bit-banging’ and can make the timing of the programs run much more accurately. Below is an good block chart from the data sheet that can help with understanding how it works.

PIO Code

The baseline PIO code that was used in this project was taken from a sample project we found online to simply turn a servo. It does this by generating PWM signals on the desired GPIO pins. We then converted it to work for both servo motors and the PWM signal to be based on the reading from the sensor. Below is that PIO code.

; Side-set pin 0 is used for PWM output

.program pico_servo_pio
.side_set 1 opt

    pull noblock    side 0 ; Pull from FIFO to OSR if available, else copy X to OSR.
    mov x, osr             ; Copy most-recently-pulled value back to scratch X
    mov y, isr             ; ISR contains PWM period. Y used as counter.
countloop:
    jmp x!=y noset         ; Set pin high if X == Y, keep the two paths length matched
    jmp skip        side 1
noset:
    nop                    ; Single dummy cycle to keep the two paths the same length
skip:
    jmp y-- countloop      ; Loop until Y hits 0, then pull a fresh PWM value from FIFO

% c-sdk {

static inline void pico_servo_pio_program_init(PIO pio, uint sm, uint offset, uint clk_div, uint pin) {
   pio_gpio_init(pio, pin);
   pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
   pio_sm_config c = pico_servo_pio_program_get_default_config(offset);
   sm_config_set_sideset_pins(&c, pin);
   sm_config_set_clkdiv(&c, clk_div);
   pio_sm_init(pio, sm, offset, &c);
}
%}

The nice thing about the code is that it is very simple and designed to generate PWM based on the data from the C code so we barely had to mess around with the PIO language. All we needed to do was add more to the C code to setup two state machines, one for each servo. We could then get the data from the C code and send it into the FIFOs to generate the PWM on the pins. This made programming it much simpler and easier to troubleshoot.