The Algorithm for two-wheel balancing robot nBot.

The balancing algorithm measures two outputs from the
robot and calculates the torque forces from the motors
needed for balance.  Here is an ascii-art block diagram:


               +---------------------+--------------+
    +--------< | Motor shaft encoder |              |
    |          +---------------------+   Motor PWM  | <-----+
    |    +---< | Angle sensor        |              |       |
    |    |     +---------------------+--------------+       |
    |    |                                                  |
    |    |     +-------+      |\ angle                      |
    |    |     |       |      | \ velocity                  |
    |    +-----| Deriv |------|K1-----+                     |
    |    |     |       |      | /      \                    |
    |    |     +-------+      |/        \                   |
    |    |                               +-----+            |
    |    |                    |\ angle   |      \           |
    |    |                    | \ pos    |       \          |
    |    +--------------------|K2--------+        \         |
    |                         | /        |         \        |
    |                         |/         |          \       |
    |                                    |  SUM      +------+
    |          +-------+      |\ wheel   |          /
    |          |       |      | \velocity|         /
    +----------| Deriv |------|K3--------+        /
    |          |       |      | /        |       /
    |          +-------+      |/         |      /
    |                                    +-----+
    |                         |\ wheel  / 
    |                         | \ pos  /
    +-------------------------|K4-----+
                              | /
                              |/


The boxes labled "Deriv" calculate the derivative of the inputs
by subtracting the last sample from the current sample.  For the
shaft encoders this gives the wheel velocity, and for the angle
sensor this gives the angle velocity.  The four triangles labeled
K1 through K4 are the "knobs" that apply gain to the four feedback
signals.  They are then summed together and fed back to the robot
as the PWM motor voltage. Here is the same thing in C code:

/* define constants.  In the demo these were set with a knob */

#define K1 20
#define K2 50
#define K3 10
#define K4 22

/* HC11 16 bit integers for tilt sensor and shaft encoder */

int angle, angle_velocity, last_angle;
int wheel, wheel_velocity, last_wheel;
int torque;

/* the code */

void balance()
{
    while(1) {		/* loop forever */

	angle = read_analog(TILT_SENSOR);
	angle_velocity = angle - last_angle;
	last_angle = angle;

	wheel = read_wheel_encoder();  /* wheel = read_encoder()-profiler() */
	velocity = wheel - last_wheel;
	last_wheel = wheel;

	torque = (angle_velocity * K1) + (angle * K2)
			+ (wheel_velocity * K3) + (wheel * K4);

	pwm(torque);

	msleep(40);	/* sleep 40 milliseconds */
    }
}



This loop runs 25 times per second, measuring the wheel position and
tilt angle and updating the motor voltage to achieve balance.  The
constant for the wheel location, K4, is set equal to 0 for the
robot to travel, and to non zero (10 in this case) for the robot
to hold its location.

For driving the robot, the desired X offset is summed in with the
velocity term.  Steering is accomplished by adding an offset to
one motor and subtracting it from the other.  This does not effect
the balancing mechanism.  Obviously there is a lot left out of this
sample piece of code, but the principal is the same.

updated 2012 Feb 01 dpa

03 Aug 2001
dpa