## 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

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 * yfilter
ytilt = yfiltstate

spfiltstate = spfiltstate * (1.0 - spfilter) + inputs * spfilter

lspeed = -spfiltstate
rspeed = inputs  -- 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 = -left * enable
outputs = 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
``````