처음에는 각도를 구간별로 나누어서 pwm을 결정해 보았으나 전후진 적용이 어려워서,

아래의 원문을 자세히 읽어보고 공식으로 변경해 보았다.

결론적으로 조금 개선된 것 같다.

현재 모터 인코더가 없어서 wheel에 대한 적용값은 없다.

TSKit

======================================================

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();
	velocity = wheel - last_wheel;
	last_wheel = wheel;

	torque = (angle * K1) + (angle_velocity * K2)
			+ (wheel * K3) + (wheel_velocity * 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, K3, 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.

03 Aug 2001
dpa

아래에서 발췌함.

 http://www.geology.smu.edu/~dpa-www/robo/nbot/