Balancing Lego Robot

 

In General

We succeeded to build a robot, using the Lego Mindstorms Robotics Invention System, that is able to control its balance. The standard light sensor is the only sensor we used and we programmed the RCX (which is the Mindstorms processor) with Lejos, the Java environment for the RCX.

MPEG4 - movie (QuickTime)      AVI (+divX) - movie

 

History

The most famous balancing robot is the nBot, designed by David P. Anderson, who is the director of the Geophysical Imaging Laboratory at Southern Methodist University in Dallas (http://www.geology.smu.edu/~dpa-www/robo/nbot/). The two-wheeled award-winning nBot is able to balance as well as drive and navigate. His website gives an overview of several related projects.

Steve Hassenplug has built the LegWay, which is a similar robot built with Lego Mindstorms Robotics Invention System and 2 Electro-Optical Proximity Detectors (http://www.teamhassenplug.org/robots/legway/). This one is able to balance and to follow a line.

So what difference does our robot make? We use a Java environment (slower software response?); we intend to focus on the fact that certain Java environments are suitable for real-time applications. Another difference is that we do not use any extra component; we cope with the limited resolution of the standard light sensor we use (a mirror will do the trick). At the other hand, the LegWay robot construction is more unstable than ours.

 

Requirements and construction

Following things were needed to construct our LegoBot:

We have constructed a reasonably unstable robot using LEGO (note that the RCX module is  unstable because of the batteries at the backside).

The light sensor is attached to an arm to increase the variation of the distance to the mirror. It is attached in a way that the angle between the sensor and the robot is easily adaptable by humans (although that was the intention ; ) )

 

Measurement

How does the sensor work? Why a mirror?

The standard light sensor from the Mindstorms kit has got a red LED and an eye that just measures light intensity. The software translates the intensity into percentage with a granularity of 1%. On a normal surface (such as wood or paper) and in daylight there is a range of less than 10%, which is useless. Only on a mirror, the emitted light is reflected into the eye, which broadens the range.

Because of the reflection of the mirror, the angle of the sensor has a high influence on the intensity measurement.

Which values are measured in which situations?

The sensor measures a maximum value (70) when it is placed 4 mm from and perpendicular to the mirror. When we turn the sensor, the value decreases to a minimal value at 45° that depends on the environmental light. When we turn it more than 45°, the value increases again because of the reflection properties of the mirror. Distance has a minor influence.

Robot angle versus light intensity measurement?

Starting from the equilibrium point, our robot could fall 20° forward and 40° backward. In our first experiments, we tuned the position of the sensor perpendicular to the mirror at the equilibrium point, which was useless because it is not easy to deduce whether the robot is falling backward or forward. So at a first stage, we always try to tune the position of the sensor to a light measurement in the middle of the range.

The graph below depicts the relation between the robot angle and the light intensity measurement, after tuning. We choose an angle of 0° in the equilibrium point and negative values when it falls forward. The blue line is a situation in daylight, the red one with neon light.

How does the program use the measurements? Filtering noise?

During balancing, each program cycle the light intensity is measured. This measurement could be subjected to noise. To filter this noise, we use a moving average filter.

averagen = (averagen-1 + sensorvaluen) / 2 and average0 = sensorvalue0

This filter introduces a slower reaction time. Each cycle, the average is used to control the robot motors, which keep the robot in balance.

 

The algorithm

The program starts with a calibration procedure. This is inevitable because the range (and therefore the optimal equilibrium sensor value in the middle of the range) highly depends on the light in the environment.  So during calibration, the sensor value at the equilibrium point is set. You can adjust the angle between the sensor and the robot manually so that the equilibrium sensor value is in the middle of the range.

Then the balancing algorithm starts. During balancing, a program loop is executed continuously (in between each loop there is a pause of 20 ms). Each cycle the light intensity moving average is measured and calculated. What we need is an adequate reaction of the program that makes the motors keep the robot in balance.

We tried several algorithms used after calibration. In none of the cases a (temporary) motor power higher than 0 seemed to be satisfactory in the range around the equilibrium. (The Mindstorms motor power ranges from 0 to 7.)

We found a lot of algorithms on the Internet, some of them originating from control systems theory, however most of them use inputs we can not dispose of. The simplicity of the algorithm Steve Hassenplug uses, gave us hope to make the thing work without control systems theory. (As the distance decreases, LegWay moves forward. As the distance increases, LegWay moves backward.) We tried it but it anticipates sometimes too strongly.

We split the light intensity range around the calibrated equilibrium point into 3 zones:

The robot is in zone A when it measures an average value that does not deviate more than x percentage from the equilibrium value. The robot is in zone B when it measures an average value that deviates between x and x+y percentage from the equilibrium value. The robot is in zone C when it measures an average value that deviates more than x+y percentage from the equilibrium value.

  • In zone A, the motors are put in stop-mode. (We had some experiments in float-mode, i.e. it tolerates further motion, but it was not satisfactory)

  • In zone B with a light intensity higher than the equilibrium value, the motors move forward as long as the light intensity increases.  In zone B with a light intensity lower than the equilibrium value, the motors move backward as long as the light intensity decreases. In fact, the algorithm of Steve Hassenplug applies in zone B.

  • In zone C, the motors are always moving; in forward direction above the equilibrium value and in backward direction below the equilibrium value.

To keep things simple, we assume that there is a linear relation between the robot angle and the measured light intensity around the calibration point. Note that the robot tends to fall slower forward than backward (because of the batteries).

During balancing, it is still possible to adjust x and y. The optimal values for x and y depend on the calibrated value and the environmental light. Typical values are x = 4, y = 4 or x = 3, y = 5.

Results for the time being. As shown in the movie, the Lego robot is able to balance for a period of minutes. The definition of a zone D, in which the motor power is higher than 0, could be the next thing to experiment with.

 

The program

The program code is shown below. The flat loop structure does not really support parallel execution nor time control. Use the Timer and/or Behavior class to realise that. Note that no objects are used inside the loops to avoid memory overflow. If there is a need to, use the Recycling classes.

import josx.platform.rcx.*;

 

public class LightRX

            {

           

            // During the CALI procedure an equilibrium sensor value is set (preferably

            // in the middle of the range). The motor drives in the right direction,

            // when the sensor value deviates more than (grens1) percentage from the

            // CALI-value. In addition, the motor stops immediately when the sensor

            // value stabilises or evoluates in the right direction. The latter

            // statement is not the case when the sensor value deviates more than

            // (grens1+offset2) percentage from the CALI-value.

 

            // Manual:

            // 1 - CALIBRATION PROCEDURE

            //           choose view to set a calibration value

            //           there is a countdown after setting this value

            // 2 - BALANCING

            //           on the screen: (grens1) (offset2) and (filt. sensor value)

            //                                

            //           choose VIEW to adjust (grens1) (0 to 9)

            //           choose PRGM to adjust (offset2) (0 to 9)

            //           choose RUN to exit

           

            public static void main (String[] aArg) throws Exception

            {

           

            // Announcement of the calibration procedure

            LCD.clear();

            TextLCD.print ("CALI");

            Thread.sleep(500);

            LCD.clear();

           

            // Initialisation of the sensor.

            Sensor.S1.setTypeAndMode (SensorConstants.SENSOR_TYPE_LIGHT,

                                      SensorConstants.SENSOR_MODE_PCT);

            Sensor.S1.activate();

            int x = 0;

            int average = (-1);

           

            // Calibration loop

            while (true) {

                       

                        // Press VIEW to set the value

                        if (Button.VIEW.isPressed()) break;

                       

                        // Reading the sensor

                        x = Sensor.S1.readValue();

                       

                        // Moving average filter

                        if (average==(-1)) average = x;

                                    else average = (average + x) /2;

 

                        // Screen dump

                        TextLCD.printChar((char)((average/10)+48), 2);

                        TextLCD.printChar((char)((average%10)+48), 1);

                        LCD.refresh();

                       

                        // Delay  

                        Thread.sleep(100);

                        }

           

            // Fixation of the calibrated value

            int cali = x;

            TextLCD.print (Integer.toString (x)+"C");

            Sound.playTone(1000,10);

            Thread.sleep(1500);

           

            // Countdown

            TextLCD.print (">> 3");

            Sound.playTone(3200,15);

            Thread.sleep(500);

            TextLCD.print (">> 2");   

            Sound.playTone(2400,15);

            Thread.sleep(500);

            TextLCD.print (">> 1"); 

            Sound.playTone(1600,15);

            Thread.sleep(500);

           

            // Initialisation balancing loop

            x = 0;

            average = (-1);

            int oldaverage = (-1);

            int intervalms = 20;

            int power = 0;

            int grens1 = 4;

            int offset2 = 4;

           

            Motor.A.setPower(power);

            Motor.C.setPower(power);

           

            // Balancing loop         

            while (true) {

                       

                        // Press RUN to exit

                        if (Button.RUN.isPressed()) break;

                       

                        // Press VIEW to change (grens1)

                        if (Button.VIEW.isPressed()) {

                                   grens1++;

                                   if (grens1==(10)) grens1= 0;}

                                  

                        // Press PRGM to change (offset2)     

                        if (Button.PRGM.isPressed()) {

                                   offset2++;

                                   if (offset2==(10)) offset2= 0;   }

                       

                        // Reading the sensor

                        x = Sensor.S1.readValue();

                       

                        // Moving average filter           

                        if (average!=(-1)) {average = (average + x) /2;}

                                   else {average = x;}

                                  

                        // Situation in which the motors drive forward

                        if ( ((average > (cali+grens1)) && (average > oldaverage)) ||

                             (average > (cali+(grens1+offset2))) )

                                   {Motor.A.forward();

                                    Motor.C.forward();}

                                    

                        // Situation in which the motors drive backward

                        else if ( ((average < (cali-grens1)) && (average < oldaverage)) ||

                                                 (average < (cali-(grens1+offset2))) ){

                                   Motor.A.backward();

                                   Motor.C.backward();}

 

                        // Situation in which the motors stop

                        else {Motor.A.stop();Motor.C.stop();}

                       

                        oldaverage = average;

 

                        // Screen dump

                        TextLCD.printChar((char)((grens1%10)+48), 4);

                        TextLCD.printChar((char)((offset2%10)+48), 3);      

                        TextLCD.printChar((char)((average/10)+48), 1);

                        TextLCD.printChar((char)((average%10)+48), 0);

                        LCD.refresh();

                       

                        // Delay

                        Thread.sleep(intervalms);

                        }         

           

            // Exit procedure

            TextLCD.print ("EXIT");

            Motor.A.stop();

            Motor.C.stop();

            Thread.sleep(500);

           

            }

}

 

Contact

Any questions? Feel free to contact joris |DOT| maervoet |AT| kahosl |DOT| be

Flemish company interested in embedded hardware/software? ADVIES