Aasimon.org Wiki

Date: September 26th 2008
Duration of activity: 8-12.30
Participants: Kasper, Bent and Johnny

The Purpose of this exercise is too make a balancing robot, using the NXT.

Experiments with the LEGO sejway, legway.

First we set out to build a robot with two wheels, following the build instructions on http://www.philohome.com/nxtway/nxtway.htm

We first tried to use a boom to make Marvin balance, but it seemed that the effect were unnoticable. We achived the best results by applying larger wheels (as in larger diameter).

We also discovered that having an even coloured surface improved the performance. The value collected from the light sensor fluctuated greatly with the surrounding lights and to eliminate this, we used a tube around the lightsensor. This proved to be the tipping point in the mecanichs.

In the software we added a few lines of code. We added a range in error so if the error was negligible small, the error was set to zero to avoid oscilliations.

The last thing we added were another scale factor to reduce the power apllied to the motor. Again to avoid that Marvin were pushed too hard.

We used the code from Brian Bagnall, Maximum Lego NXTBuilding Robots with Java Brains, Chapter 11, 243 - 284. (http://www.variantpress.com/books/maximum-lego-nxt) as a basis for our own program.
The default values of the PID control parameters seemed to act too violently, so initially we tried changing the them and the scale factor:

// PID constants
final int KP = 28;
final int KI = 4;
final int KD = 33;
final int SCALE = 18;

We increased the D parameter to minimize the overshoot.

The easiest way to tune the parameters would be if it was possible to do a simulation and see the phasemargin, but this is not available. Therefore we have observed the behaivor of Marvin and used the manual tuning methods listed on Wikipedia at http://en.wikipedia.org/wiki/PID_controller#Manual_tuning
In the following we have used the above guidelines and observations.

Below are a listing of the final code:

    public void pidControl() {
	int normVal;
	int error;
	int deriv_error;
	int pid_val;
	int power;
	while (!Button.ESCAPE.isPressed()) {
	    normVal = ls.readNormalizedValue();
 
	    // Proportional Error:
	    error = normVal - offset;
	    if(Math.abs(error) < 3) error = 0;
	    // Adjust far and near light readings:
	    if (error < 0) error = (int)(error * 1.8F);
 
	    // Integral Error:
	    int_error = ((int_error + error) * 2)/3;
 
	    // Derivative Error:
	    deriv_error = error - prev_error;
	    prev_error = error;
 
	    pid_val = (int)(KP * error + KI * int_error + KD * deriv_error) / SCALE;
 
	    if (pid_val > 100)
		pid_val = 100;
	    if (pid_val < -100)
		pid_val = -100;
 
	    // Power derived from PID value:
	    power = Math.abs(pid_val);
	    power = 55 + (power * 45) / 100; // NORMALIZE POWER
	    power /= 3;
	    Motor.B.setPower(power);
	    Motor.C.setPower(power);
 
	    if (pid_val > 0) {
		Motor.B.forward();
		Motor.C.forward();
	    } else {
		Motor.B.backward();
		Motor.C.backward();
	    }
	}
    }