Testing a 2-wheel balancing robot

This time, a small demo showing a balancing robot platform using a gyro to measure tilt in one direction and two wheels that try to balance it upright.

The computer code (see at the bottom of this post if you’re interested) is basically doing the same thing as all “balancing robot” projects you can find on the net built around Arduinos and other platforms (for example see this discussion thread). It’s pretty cool that the same pitfalls and principles apply in the game here as in the construction of real robots!

In the video above, the goal of the control algorithm was to keep the robot upright and if it moves, try to stop it. It’s really difficult to tune so it doesn’t oscillate, like it does in the video here and there.

You can actually ride it just like a Segway, by jumping up on the top and pushing (hit the F-key) slightly in front of the center line to tilt it forward which forces the control loop to roll the wheels forward. There is no control to make it rotate to steer it though.

UPDATE: after having written this post I went back to review some of the PID-controller basics again and found this well written piece about a PID controller in an Arduino. I realized the I-part might be a good thing to have.

Just to relate to the real world, here are some videos in no particular order of real balancing robot projects I looked at:

Here is the current test-code, if you are curious:


-- Mekside 2-wheel balancer demo code v1.1
-- By Realitylabs Game Studios
-- Use/adapt however you want.

log("Balancer starting up")

-- First some constants:
---------------------------------

enable = 1

desired_lspeed = 0.0
desired_rspeed = 0.0

yfilter = 0.2
yfiltstate = 0.0

spfilter = 0.05
spfiltstate = 0.0

-- Proportional coefficient of the angle PD-regulator
-- (how much we try to reduce errors in angle)
ytiltP = 15.0

-- Derivative coefficient of the angle PD-regulator
-- (how much we dampen quick angular changes)
ytiltD = 3.0

-- Proportional coefficient of the speed PD-regulator
speedP = 0.1

-- Derivative coefficient of the speed PD-regulator
speedD = 0.0005

tiltcap = 0.4

-- Amount of tilt the control panel input can add
tiltcontrol = 0.2

-- I/O doc
------------------------------------------

-- Outputs (wheel motors):
-- 1: left speed
-- 2: right speed

-- Inputs:
-- 1: Y tilt (positive if the vehicle tilts "backwards")
-- 2: Angular speed from left motor
-- 3: Angular speed from right motor

collective = 0

-- Start with -200 so we know it's invalid below on the first iteration
last_angle = -200
last_xtilt = -200
last_ytilt = -200

-- This function is called each physics tick, that is 100 times per second
-----------------------------------------------------------

function tick()

    -- Filter the inputs
    yfiltstate = yfiltstate * (1.0 - yfilter) + inputs[1] * yfilter
    ytilt = yfiltstate

    spfiltstate = spfiltstate * (1.0 - spfilter) + inputs[2] * spfilter

    lspeed = -spfiltstate
    rspeed = inputs[3]  -- right wheel speed (not used currently)

    -- The first iteration we have to set some last-variables
    if last_ytilt < -100 then
        last_ytilt = ytilt
        last_lspeed = lspeed
        last_rspeed = rspeed
    end

    -- Speed derivatives
    lspeed_d = 100 * (lspeed - last_lspeed)
    rspeed_d = 100 * (rspeed - last_rspeed)

    -- Differences from set points
    dlspeed = desired_lspeed - lspeed
    drspeed = desired_rspeed - rspeed

    if dlspeed > 0.3 then
       dlspeed = 0.3
    elseif dlspeed < -0.3 then
       dlspeed = -0.3
    end

    -- PD regulator for speed
    reg_tilt = speedP * dlspeed - speedD * lspeed_d
    reg_rspeed = speedP * drspeed - speedD * rspeed_d

    -- Y tilt derivative
    ytiltspeed = 100 * (ytilt - last_ytilt)

    if reg_tilt > 0.005 then
        reg_tilt = 0.005
    elseif reg_tilt < -0.005 then
        reg_tilt = -0.005
    end

    -- Differences from set points
    dytilt = reg_tilt - ytilt

    if dytilt > tiltcap then
        dytilt = tiltcap
    elseif dytilt < -tiltcap then
        dytilt = -tiltcap
    end

    -- PD regulator for y tilt
    yrot = ytiltP * dytilt - ytiltD * ytiltspeed

    if yrot < -0.5 then
       yrot = -0.5
    elseif yrot > 0.5 then
       yrot = 0.5
    end

    log(string.format("Balancer: ytilt %f, lspeed %f, dlspeed %f, reg_tilt %f, yrot %f",
                      ytilt, lspeed, dlspeed, reg_tilt, yrot))

    -- Calculate the actual motor speeds for the 2 motors

    left = math.min(1.0, math.max(-1.0, yrot))
    right = math.min(1.0, math.max(-1.0, yrot))

    -- Write to the outputs

    outputs[1] = -left * enable
    outputs[2] = right * enable

    -- Prepare for the next tick so we can calculate deltas
    last_ytilt = ytilt
    last_lspeed = lspeed
    last_rspeed = rspeed
    
    -- End of physics tick
end
Share Button