LEGO+ and M5Stack

When the Lego Mindstorm NXT came out, I bought one. Good fun, but I realized quite quickly that programming via GUI is…not fun at all. Attempts to program in NXC have been made, but it wasn’t as much fun as I hoped.

Enter the M5Stack with its LEGO+ module (now renamed to DC Motor) which can connect to the motors. Not the sensors, but there’s of course the way more useful M5Stack Units.

And since the M5Stack Core unit has WiFi, a display, buttons and can be programmed in microPython, it’s fun again!

However the latest firmware (v.1.7.2) misses the module for the LEGO+ module. v1.4.5 has it though.

Preview(opens in a new tab)

Task 1: Since you can only tell the motor to turn with a given direction and speed, make it so reach a certain amount of rotations. Classical PID control loop:

This rather simple program does the trick:

from m5stack import *
from m5ui import *
from uiflow import *
import module
import time

class PID:

    def __init__(self, P=0.2, I=0.0, D=0.0):
        self.Kp = P
        self.Ki = I
        self.Kd = D

        self.sample_time = 0.00
        self.current_time = time.ticks_ms()
        self.last_time = self.current_time

    def clear(self):
        self.SetPoint = 0.0
        self.PTerm = 0.0
        self.ITerm = 0.0
        self.DTerm = 0.0
        self.last_error = 0.0

        # Windup Guard
        self.int_error = 0.0
        self.windup_guard = 10000.0

        self.output = 0.0

    def update(self, feedback_value):
        """Calculates PID value for given reference feedback
        .. math::
            u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt}
        .. figure:: images/pid_1.png
           :align:   center
           Test PID with Kp=1.2, Ki=1, Kd=0.001 (
        error = self.SetPoint - feedback_value

        self.current_time = time.ticks_ms()
        delta_time = self.current_time - self.last_time
        delta_error = error - self.last_error

        if (delta_time >= self.sample_time):
            self.PTerm = self.Kp * error
            self.ITerm += error * delta_time

            if (self.ITerm < -self.windup_guard):
                self.ITerm = -self.windup_guard
            elif (self.ITerm > self.windup_guard):
                self.ITerm = self.windup_guard

            self.DTerm = 0.0
            if delta_time > 0:
                self.DTerm = delta_error / delta_time

            # Remember last time and last error for next calculation
            self.last_time = self.current_time
            self.last_error = error

            self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)
            lcd.print("PTerm: "+str(self.PTerm)+"    ", 0, 80, 0xffffff)
            lcd.print("ITerm: "+str(self.ITerm)+"    ", 0, 100, 0xffffff)
            lcd.print("DTerm: "+str(self.DTerm)+"    ", 0, 120, 0xffffff)

    def setWindup(self, windup):
        self.windup_guard = windup

lego_motor = module.get(module.LEGO)

def buttonA_wasPressed():
  global speed

def buttonC_wasPressed():
  global speed

speed = 0

# Parameter tuned for motor@9V
pid=PID(0.3, 0.0, 40.0)

# Turn 1/8th of circle, that's 6 full rotations for the motor

# If motor signal is less than about 50
# the motor won't turn. So stop then


for count in range(1000):
    lcd.print("Encoder: "+str(lego_motor.M1.encoder_read())+"      ", 0, 0, 0xffffff)
    lcd.print("Speed: "+str(speed)+" ", 0, 20, 0xffffff)
    lcd.print("Time: "+str(time.ticks_ms()), 0, 40, 0xffffff)
    if speed < -255:
        speed = -255
    if speed > 255:
        speed = 255
    if speed < too_small:
        too_small_count += 1
        too_small_count = 0
    if too_small_count > too_small_count_max:


It was interesting to find usable PID parameters. In the end worked quite well. The motor has a lot of play and does not work at lowest speeds. I’ll instead use some servos I still have and see what I can PID with those.

Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Create your website with
Get started
%d bloggers like this: