Aasimon.org Wiki

Date: September 26th 2008
Duration of activity: 8-12
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 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

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();
	    }
	}
    }