# NXT Segway problem. Need advice/help

I'm attempting to build a segway robot using a gyrosensor and accelerometer.

I'm having trouble getting the robot to remain standing, for some reason, and I can't identify the problem. Here's what I know:

The gyroscope API for the lejos NXT platform is here:

http://www.lejos.org/nxt/nxj/api/

By using timestamps and angular velocity, the project attempts to infer the angle of the robot. The API suggests that in order to be accurate, it must be polled 100 times per second (or every 10ms on average).

The problem is that simply polling the gyrosensor takes 4ms. Polling the accelerometer takes 10ms.

The dimensions of the robot: Height: 28cm wheel circumference : 13.25cm Radius of a wheel, given the circumference: 2.1cm

The accelerometer is mounted on the top of the robot (at approximately 28cm from the ground, 26cm from axis of rotation)

In order to keep the correction amount linear (as opposed to trying to correct an arbitrary angle) , I translate the angle of the robot to a distance to travel along the ground to "right" the robot. This might be a bit naive, and I'm open to suggestion here. Basically it's just the horizontal distance calculated using a right-angle triangle with the angle of the robot at the top and hypotenuse of 28cm.

If that's not clear, it's essentially the horizontal distance from the top of the robot and the bottom of the robot.

Right now my main concern is the amount of drift the gyroscope seems to be experiencing. Given the fact that with the NXT java software package, it's nearly impossible to poll 100 times per second, the amount of error accumulated by the gyroscope is fairly large.

Finally, I've implemented a PID control system. The thing I'm not clear about with respect to this system is the integral and derivative of error must be calculated given a set of values. Say, the last 20 error measurements recorded.

If the amount of past errors recorded is a variable, and the PID constants are variable, and the speed of the wheels is a variable, it seems this problem begs for some kind of automated optimization. But how to do it? If I set the speed to 120 RPM (roughly the max of the NXT servos) and take the past 20 errors for calculating the integral and derivative of the error, will it be possible to optimize the PID constants successfully? Or must all 5 variables be tuned together?

Thanks ahead for any insight on the problem.