Micropython example: controlling a motor with rotary encoder and PID

_README.md

MicroPython motor control on Raspberry Pi Pico

Getting started

See getting started with Raspberry Pi Pico. This walks you through installing Thonny, an editor that supports interacting with MicroPython running on the Pico. The board already has MicroPython installed on it, so you can skip that step.

pico_motor_pid_demo.py

from rp2 import PIO, StateMachine, asm_pio
from machine import Pin, PWM, Timer
import micropython
import utime
import math


class QuadratureDecoder:
    """
    Track rotary encoder position using programmable IO controller (PIO)
    """

    def __init__(self, pin_a, pin_b):
        self.sm = StateMachine(1, self._encoder, freq=125_000_000, in_base=pin_b, jmp_pin=pin_a)

        # Reset counter
        self.sm.put(0)
        self.sm.exec("pull()")
        self.sm.exec("out(x, 32)")

        # Start PIO
        self.sm.active(1)

    @micropython.viper
    def read(self) -> int:
        """
        Get the current position of the encoder
        """
        self.sm.exec("in_(x, 32)")
        x = self.sm.get()

        # Convert unsigned to signed
        # There doesn't seem to be a native way to get this from PIO
        if x & 0x80000000:
            x -= 0xFFFFFFFF
            
        return int(x)
        
    @asm_pio(autopush=True, push_thresh=32)
    def _encoder():
        """
        SPDX-FileCopyrightText: 2022 Jamon Terrell <github@jamonterrell.com>
        SPDX-License-Identifier: MIT
        https://github.com/jamon/pi-pico-pio-quadrature-encoder/
        """
        label("start")
        wait(0, pin, 0)         # Wait for CLK to go low
        jmp(pin, "WAIT_HIGH")   # if Data is low
        mov(x, invert(x))           # Increment X
        jmp(x_dec, "nop1")
        label("nop1")
        mov(x, invert(x))
        label("WAIT_HIGH")      # else
        jmp(x_dec, "nop2")          # Decrement X
        label("nop2")
        
        wait(1, pin, 0)         # Wait for CLK to go high
        jmp(pin, "WAIT_LOW")    # if Data is low
        jmp(x_dec, "nop3")          # Decrement X
        label("nop3")
        
        label("WAIT_LOW")       # else
        mov(x, invert(x))           # Increment X
        jmp(x_dec, "nop4")
        label("nop4")
        mov(x, invert(x))
        wrap()


class Motor():
    def __init__(self):
        self.encoder = QuadratureDecoder(Pin(2), Pin(3))

        self.pwm = PWM(Pin(13), freq=20000, duty_u16=0)
        self.fwd = Pin(15, mode=Pin.OUT, value=0)
        self.rev = Pin(14, mode=Pin.OUT, value=0)
        
        self.max_out = 65535
    
        self.read = self.encoder.read
        
    def brake(self):
        self.pwm.duty_u16(0)
        self.rev.off()
        self.fwd.off()
        
    def freewheel(self):
        self.pwm.duty_u16(0)
        self.rev.on()
        self.fwd.on()
                
    def set_speed(self, percent: float):
        clamped = max(min(100, percent), -100) / 100.0
        output = int(clamped * self.max_out)

        if output > -100 and output < 100:
            # Stop if the value is small enough to not do anything
            self.brake()
        elif output > 0:
            self.rev.off()
            self.fwd.on()
        else:
            self.fwd.off()
            self.rev.on()

        self.pwm.duty_u16(abs(output))


class PidMotor(Motor):
    def __init__(self, Kp, Ki, Kd):
        super().__init__()

        # PID settings
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd

        # PID state
        self.integral = 0
        self.last_error = 0
        self.last_update = utime.ticks_us()

        # Inputs
        self.target = 0
        self._smoothed_target = 0
        self.enable = False
        
        self._timer = Timer(period=10, mode=Timer.PERIODIC, callback=self._update)
        
    @micropython.native
    def _update(self, t):
        pos = self.encoder.read()

        now = utime.ticks_us()
        dt = (now - self.last_update)/1e6
        
        self._smoothed_target += (self.target - self._smoothed_target) * 0.02

        if self.enable:
            error = self._smoothed_target - pos
            self.integral = self.integral + dt * error
            derivative = (error - self.last_error)/dt
            output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        else:
            error = 0
            self.integral = 0
            output = 0
            
        if not self.enable:
            self.brake()
        else:
            self.set_speed(output)

        self.last_update = now
        self.last_error = error    


def cleanup_pios():
    """ Free any allocated PIOs from a previous interactive run """
    for i in range(6):
        try:
            rp2.PIO(i).remove_program()
        except:
            continue


# Motor sensing and control
#
# PID will need tuning based on the voltage you're running the motor at.
# Needs a decent integral component as the motor is a bit sticky. Currently
# doesn't have wind-up protection - probably need to add it for safety.
#
# Also might be worth adding an automatic cut-off if the encoder position
# goes out of some defined bounds, so if it runs away it will stop itself.
cleanup_pios()
motor = PidMotor(
    Kp=0.8, # Proportional
    Ki=1.8, # Integral
    Kd=0.0, # Derivative
)

# Turn this down if things are too spicy or you're hitting a current limit
motor.max_out = 65535

# Print what's going on periodically
def debug_print(t):
    print("Target:", motor.target, "Position:", motor.read(), "Output:", motor.pwm.duty_u16(), "Direction:", motor.fwd.value(), motor.rev.value())

debug = Timer(period=250, mode=Timer.PERIODIC, callback=debug_print)

# Example: move the set point continuously
motor.target = 0
motor.enable = True

while True:
    motor.target -= 100
    utime.sleep(0.1)