Self balancing robot

Showroom for MicroPython related hardware projects.
Target audience: Users wanting to show off their project!
User avatar
pythoncoder
Posts: 4645
Joined: Fri Jul 18, 2014 8:01 am
Location: UK
Contact:

Re: Self balancing robot

Post by pythoncoder » Mon Mar 23, 2020 7:02 am

How are you combining the gyro and accelerometer values? Many users recommend Kalman filters, but I found a complementary filter worked equally well and is simple and fast. My main loop is quite simple, but does have a couple of wrinkles. I maintain a constant loop period of 5ms. To achieve this on a Pyboard 1.0 I tweak the garbage collector to ensure it runs exactly once per iteration.

In my testing the main effort was in adjusting the PID constants so that the thing balanced when stationary and recovered well if I applied a brief push to the top. Then getting it to move or turn is just a matter of applying speed deltas. Hopefully the following is self explanatory. The PID constants are Kphi, Kdphi, and Kd

Code: Select all

def b():
    gc.disable()
    controlspeed = 0  # integrator output
    fspeed = 0  # mean forward speed
    fangle = 0.0  # Angle from complementary filter
    csd = 0  # Differentiator output

    stats = statgen() # TEST
    led = pyb.LED(1)
    while abs(fangle) < 45:  # Topple detect: 45 degs
        start = ticks_us()
        demandspeed, turnspeed = getdemand()  # Commands from radio
        angle = imu.pitch()
        rate = imu.gyro.x  # Rotates about x axis. Rate in deg/sec
        fangle = compf(fangle, angle, rate, USLOOPTIME, 0.99)  # filtered angle
        fspeed = (motor1.speed + motor2.speed)/2  # Forward speed
        accel = fangle * Kphi + rate * Kdphi + (demandspeed - fspeed) * Kv
        controlspeed += accel * MSLOOPTIME
        motor1.speed = -controlspeed + turnspeed   # set motor speed -100 <= speed <= 100
        motor2.speed = -controlspeed - turnspeed
        gc.collect()  # Reduce GC delay from mS to ~26uS
        dT = ticks_diff(ticks_us(), start)
        if dT < USLOOPTIME:
            sleep_us(USLOOPTIME - dT)  # Maintain constant loop time
        else:
            led.on()

    motor1.speed = 0  # Crash! Stop and turn off motors
    motor2.speed = 0
    gc.enable()
Peter Hinch

Post Reply