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.
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
self.clear()
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 (test_pid.py)
"""
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)
setScreenColor(0x222222)
def buttonA_wasPressed():
global speed
lego_motor.M1.stop()
pass
btnA.wasPressed(buttonA_wasPressed)
def buttonC_wasPressed():
global speed
lego_motor.M1.encode_clear()
pass
btnC.wasPressed(buttonC_wasPressed)
speed = 0
lego_motor.M1.set_pwm(speed)
# 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
pid.SetPoint=360*6
lego_motor.M1.encode_clear();
# If motor signal is less than about 50
# the motor won't turn. So stop then
too_small=50
too_small_count_max=20
too_small_count=0
for count in range(1000):
pid.update(lego_motor.M1.encoder_read())
speed=pid.output;
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
lego_motor.M1.set_pwm(speed)
if speed < too_small:
too_small_count += 1
else:
too_small_count = 0
if too_small_count > too_small_count_max:
break;
time.sleep(0.2)
lego_motor.M1.stop()
It was interesting to find usable PID parameters. In the end https://en.wikipedia.org/wiki/PID_controller#Ziegler%E2%80%93Nichols_method 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.
