Skip to content

Instantly share code, notes, and snippets.

@stecman
Last active October 18, 2023 04:08
Show Gist options
  • Save stecman/4f578800d1fc6d7ec6c1c272c63a0b67 to your computer and use it in GitHub Desktop.
Save stecman/4f578800d1fc6d7ec6c1c272c63a0b67 to your computer and use it in GitHub Desktop.
Micropython example: controlling a motor with rotary encoder and PID

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.

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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment