Lego lab lesson 6 - Robot Race

The Race

The purpose of the exercise is to start from the bottom of the track, drive up to the top, turn around and get to the bottom again. The numbers shown in the following diagram are used as references to track 1, 2 and 3 in the rest of the document and the code. At the end of each track we call it plateau 1, 2 and 3.


Robot construction

The first consideration to take for the race was how to construct the robot car. Which wheels to mount, and how many and which sensors to use.
The robot was constructed as in the first lab session for the line follower exercise. The wheels where changed to bigger ones, being able to get higher speed (and so higher chances to win the race!).
But no additional sensor was added. To make the robot and the control routine simple, only the light sensor and the taco counter provided by the motors have been used. If more sensors were used, the control routine would need more threads listening and reacting to those sensors, and that would probably make the control slower and more complicated. That means, of course, that there is not much information available from the environment. But using it correctly, it should be enough to complete the race.


Following the line

In lab 5, a routine was developed for following a line with the robot using a PID control [3]. The same routine was used in this case, but the different constants were recalculated because the size of the wheels was different. We go back to the fact that the physical environment is very important and affects the software inside.

public void run () {     int count = 0;     while (true)     {       try  {         lightValue = sensor.light();     pidCalculate(lightValue);          if (!pause)     {     if (stop)     Car.stop();         else     if (rightTurn)     Car.forward(powerA, powerC);     else     Car.forward(powerC, powerA);          // After a pause starts at reduced speed     if (++count == 5)     {     // Accelerate to full speed     if (Tp < Tp_saved) Tp = Tp + 1;     count = 0;     }     }     else     count = 0;         Thread.sleep(dT);         } catch (InterruptedException ie)  {          }        }        }

The pause variable’s purpose is to stop following the line and start again. It is used when the robot is changing direction in the corners of the track, since there is more than one line to follow and the robot does not know which one to follow.
When starting following the line again, it does not do it at full speed from the start, but it accelerates slowly. In the beginning it was returning to full speed after being stopped, but the robot had some difficulties on following the line after such an abrupt change of speed. After adding this slower acceleration, everything ran more smoothly.


Strategy

The overall strategy is to use the PID controlled line follower. This line controller will be running in a separate thread being responsible to follow the line on each of the 3 tracks on the Alishan train track. A state machine is defined to keep track on where we are on the Alishan train track. This state controller is running in another separate thread. At each plateau the state machine will take over turning the robot car in the right direction. That means we have chosen a mixture of a sequential and reactive control strategy as described by Fred G. Martin [5].

The UML class diagram illustrates the structure of the program. The StateController threads is scheduled with a periodicity of 10 ms and the LineFollowerPID with a periodicity of 5 ms. The main thread in the RoboRace is updating the LCD screen each second with light and time information. It also implements functions to increase/decrease the speed and pause the line follower thread manually. These function has only been used for testing.


The PID controlled line follower is a reactive controller with a closed loop feedback using the light sensor to control the power of the motor, keeping it following the black line on one side of the line (See states 1, 10, 20). The state machine is keeping track on where we are. It turns the robot car at the plateaus without using the light sensor. Turing is a open loop operation without any feedback only using time to decide on when the turn is completed (See state 2, 11, 21). When time is expired the state machine starts looking for the black line again and telling the line follower thread which side of the line we expect to be (State 3, 12). At the top of the Alishan train track we turn the robot car 180 degrees, decreases the speed and starts on the way down.


Below is listed the first 3 states of the StateController.

private void handelStates() throws InterruptedException    { dist = Car.updateTachoCounter(); LCD.drawString("Tacho ", 0, 7); LCD.drawInt((int)(dist),4,10,7);         switch (state)     {     case 1: // Moving up first time track 1     if (lookForThreshold(dist, 2100)) // Track 1 2100     {        lineFollower.pause(true);        time = 0;        state = 2;     }    break;     case 2: // At plateau no. 1     if (time > 150)     {        state = 3;     }     else        Car.forward(60, 37); // Turn on plateau no. 1     break;     case 3:     if (lineFollower.sensor.black())     {        Car.resetTachoCounter();        lineFollower.pause(false);        time = 0;        state = 10;         }     else     Car.forward(40, 30); // Turn on plateau no. 1     break; [...] //Rest of the states } [...] }

Impediments / challenges

Battery level and power applied to the motor
After using many hours fine tuning the parameters in the routine and being very close to having a robot that could go through the track, the robot started reacting strangely... the batteries had much less power than in the start! And the methods used for controlling the motors were not taking into account, since when you apply a certain power to a motor, the speed is dependant of the power available in the batteries. Instead, the setSpeed method should have been used for more precision and less dependency on the battery power. [4]

Friction between the wheels and the floor
The wheels collected dust after being used in different tracks, modifying the friction of the robot with the surface. This was altering the calibration of the constants in the control routine.

Video recording
http://www.youtube.com/watch?v=H05ODk-qB5o

Source Code
http://code.google.com/p/josemariakim/source/browse/#svn/trunk/lab6/RoboRace

References

[1] LegoLab assignment - http://www.legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson6.dir/Lesson.html
[2] Original Rules - http://www.legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson6.dir/alishan_timber_loader.doc
[3] Lab 5 report - http://josemariakim.blogspot.com/2010/10/lab-session-5-line-follower.html
[4] Controlling motors - http://lejos.sourceforge.net/nxt/nxj/tutorial/MotorTutorial/ControllingMotors.htm
[5] Fred G. Martin, Robotic Explorations

Lab session 5: Line follower

Black white detection

The BlackWhiteSensor.java program has been used to evaluate the sensor readings over different dark and bright areas.
In the meassurements below we have observed that the values meassured depends on the color of the surface and the distance to the surface.

White paper readings in different distances:
60 – 5 mm
56 – 10 mm
50 – 20 mm
Black paper readings in different distances:
36 – 5 mm
34 – 10 mm
32 – 20 mm
Gray paper readings in different distances:
46 – 5 mm
44 – 10 mm
43 – 20 mm

Alishan train track readings in the distance of 10 mm:
White – 56
Border between black and white - 44
Black – 38

It seems like the sensor values are not symmetric. With a distance of 5 mm there is a bigger difference between gray and white (14) than gray and black (10). At the distance 20 mm the difference is inversed. The different between sensor readings gray and white is now 8 and gray and black 11. At the distance 10 mm measured at the Alishan track black has a higher value than on a pure black surface due to reflections from the white surroundings. One should think that symmetric readings should be optimal to make a motor controller for the line follower.
In use of sensor inputs for control of an actuator the number of values in the input scale is important in how accurate the mapping from inputs values to outputs values can be. If we have a fine grained scale (many values) we will be able to control the output actuator in smaller steps. If we as an example should use a proportional gain to map between input sensor values to an output motor power.To make a compromise between the above observations we have chosen the sensor distance to be 10 mm even though it isn't symmetric.

Line Follower with Calibration (PID)

First we tested the car following a line by using the LineFollowerCal.java program. The result is a car that is able to follow a straight black line but with a lot of oscillation.

Then we implemented the PID controller as described in [1] and started with setting the proportional gain Kp = 2. Based on the difference between gray and white = 12 when we multiply this value by Kp we get a maximum turn of 24 in power difference to the motor. The car was following the line more smoothly and precise.

PID calculation method:

public void pidCalculate(int lvalue) {
int turn;
int error;
float derivative = 0;

error = lvalue - offset;
integral = (integral*2)/3 + error;
derivative = error - lastError;
turn = (int)(Kp*error + Ki*integral + Kd*derivative);

powerA = limitPower(Tp + turn);
powerC = limitPower(Tp - turn);

lastError = error;
}

Main loop reading sensor and calling pidCalculate with the periodicity of ~10ms:

while (! Button.ESCAPE.isPressed()){
if (Button.ENTER.isPressed())
time = 0;
LightValue = sensor.light();
LCD.drawInt(LightValue,4,10,2);
LCD.drawInt(time,4,10,3);
LCD.refresh();
lineFollower.pidCalculate(LightValue);
Car.forward(powerA, powerC);
Thread.sleep(dT);
time++;
}

We managed to improve the line follower even better by adjusting the integral and derivative constants as described in [1]. First we used an execl sheet to automatically calculate the Kp, Ki and Kd based on a selected Kc.
We have measured the oscillation rate base on time and counting 10 robot oscillation turns. First we stated with a dT = 10 ms, but found it better using a sampling rate of 5 ms. The response time adjusting the PID and motor made a faster response. Especially when trying to make sharp turns following a black line.

1. Test
dt = 10ms
Kc = 2, dt = 10ms, pc = 800 ms (10 turns in ~8 sec)
Calculation of PID constants:

Kp = 0.6*Kc = 1.2
Ki = 2*Kp*dt/pc = 2*1.2*10/800 = 0.03
Kd = Kp * (pc/8*dt) = 1.2 (800/80) = 10

Results
	Test1	Test2	Test3	Test4	Test5
dt	10	10	10	5	5
kc	2	4	3	4	5
pc	800	600	800	700	600
kp	1.2	2.4	1.8	2.4	3
ki	0.03	0.08	0.045	0.034286	0.05
kd	12	18	18	42	45

Complete source code to be found here: LineFollower

See video of PID controlled line follower here: PID controlled line follower

ColorSensor with Calibration

We decided to use the BlackWhiteSensor.java class [insert reference] as a base for our colour sensor application. This class is the one introduced in the first section. As it was previously mentioned, the class is using the lego NXT light sensor. One of the changes we introduced was to substitute the NXT light sensor and use the RCX equivalent since it has got high enough resolution to distinguish between several colours.

The main structure of the class is explained in the following lines.

The class is composed by three different types of attributes. The first one is an instance of the RCX light sensor class, that is provided by the Lejos API. This class will allow us to deal with the signals coming from the light sensor and to obtain the readings. The next attributes are integer variables that store the values corresponding to the black, white and green areas. Finally, the different thresholds that define the transitions between white, green and black are defined. The followed criteria to define those thresholds will be explained later.

private int greenBlackThreshold;
private int blackLightValue;
private int whiteLightValue;
private int greenLightValue;
private int whiteGreenThreshold;
private int greenBlackThreshold;

­­The main methods present at the class are the constructor, the read method and the calibration methods, and finally Boolean tests to detect whether the region is black, white or green.

The class constructor is creating an instance of the RCX light sensor class and defining that the sensor is going to be used in reflection mode.

public ColorSensor(SensorPort p)
{
rcsLs = new RCXLightSensor(p);
rcsLs.setFloodlight(true);
}

The calibrate method is executing blocking read operations in order to define the values that should be stored in the fields blackLightValue, whiteLightValue and greenLightValue. After those readings have been performed, the thresholds are defined by calculating the average value between two adjacent colours.

public void calibrate()
{
blackLightValue = read("black");
whiteLightValue = read("white");
greenLightValue = read("green");
greenBlackThreshold = (blackLightValue+greenLightValue)/2;
whiteGreenThreshold = (greenLightValue+whiteLightValue)/2;
}

Finally the Boolean test functions black, white and green are defined. Those functions test the conditions that must be fulfilled in order to identify one colour or another. Those conditions are defined by the thresholds, obtained from the readings performed in the calibration stage. In the following example it can be seen the example for the green detection function, that will return true in the case the detected colour is green.

public boolean green()
{
return(rcsLs.readValue() <> greenBlackThreshold);
}

[1] A PID Controller For Lego Mindstorms Robots.
[2] Caprani, Ole. BlackWhiteSensor.java