Self balancing robot with Pybricks and 51515

[BorisBrock]

Today I started playing with Pybricks. As a "hello world project" I chose to create a simple self balancing bot (using a PID controller).

The code for the bot is rather straight forward:

from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
from pybricks.parameters import Button, Color, Direction, Port, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

# Configure the hardware
hub = InventorHub()
hub.light.on(Color.RED)
motor_right = Motor(Port.A)
motor_left = Motor(Port.E)

# Set up the PID constants
target_angle = 89.0
kp = 800.0
ki = 40.0
kd = 400.0

# Perform the PID control algorithm
i_error = 0.0
d_error = 0.0
last_p_error = 0.0


while True:
    # Calculate P, I and D errors
    p_error = target_angle - hub.imu.tilt()[1]
    i_error = i_error + p_error * 0.25
    d_error = p_error - last_p_error

    # Store the error for delta calculation
    last_p_error = p_error

    # Limits
    if i_error > 100.0:
        i_error = 100.0
    if i_error < -100.0:
        i_error = -100.0

    # Compute the final output
    output = p_error * kp + i_error * ki + d_error * kd

    # Apply output to motors (motor speed ~ +/- 2000)
    motor_right.run(output)
    motor_left.run(-output)

    wait(10)

But be warned: this is still work in progress!
The bot is far from stable (I'm a total beginner regarding control algorithms).
If anyone wants to help out, I would be more than happy :-)

[laurensvalk]

Nice! For more inspiration, perhaps check this one too: https://github.com/pybricks/pybricks-projects/tree/master/sets/mindstorms-robot-inventor/other-models/balancer