Date: 12 09 2008
Duration of activity: 3 hours
Group members participating: all group members
1. The Goal
The goal of the lesson 2 is to try out the ultrasonic sensor, to see how it measures the distance and to construct a LEGO robot that follows a wall.
2. The Plan
- Construct a LEGO robot with an ultrasonic sensor
- Run the sensor test program, write about the results
- Try out "Tracker Beam" application
- Make the LEGO robot follow a wall
- Make conclusions about different possible algorithms
3. The Results
There are many things that we have accomplished during the lab session 2. We have mounted the LEGO robot with ultrasonic sensor and did many experiments in order to figure out how exactly things work.
- So first of all, it is worth mentioning that we were successful to install and make things work not only on Debian (as it is thoroughly described in the entry of the first lab session) but also on Windows Vista. As there is a common knowledge that there are too many problems related to Vista and flashing, this option wasn't considered at all. Using the given USB bluetooth dongle, the installation was successful and it was possible to transfer a sample program and make in work on NXT.
- So, as mentioned, we have mounted the LEGO robot with ultrasonic sensor and played a little in order to see how well the sensor measures the distance. For that we compiled and uploaded to NXT the SonicSensorTest.java. Read more about the results in 3.1. Testing ultrasonic sensor
- First thing to do with the sensor was to make use of the fact that the robot "can feel" the distance from the objects. For that we used the sample code of Car.java and Tracker.java. It was fun to see how the robot stops when it meets an obstacle in some given distance. To read more about the conclusions of the experiment confer 3.2. Tracking experiment
- Now when we were sure that the sensor works as intended, it was time to make the wall follower. While we were preparing the NXT to use bluetooth, we called the robot Thomas. So meet Thomas (the ultrasonic robot):
We have used Philippe Hurbain's source code (that is written in NQC) as an example of how to program our wall follower. To make it work was not an easy task. Read more about lego-robot-the-wall-follower in 3.3. Building the wall follower.
3.1. Testing ultrasonic sensor
After we uploaded the program and realized that the sensor is responding to the environment, we took it to try out. We've put it against the wall in various distances. And, as the distance in the display is given in centimeters, we checked how many centimeters our sensor can catch. That infinity is measured by 255 centimeters, we realized very quickly. Now we had to check how far the sensor can be from the wall and sense it. This is what we got:
So, the sensor we used could not sense the wall further than 210 cm away. To check if the measurement is correct, we borrowed a ruler to measure by ourselves. All we could see that the sensor is (quite accurately) measuring correctly.
To see what angles to use for the sensor to sense, we used a chair and placed it in various positions in front of the sensor. It was hard to say, as we didn't have anything to measure angle, but basically it seemed the the sensor could sense the chair, standing about 1 meter away, in a little bit more than 10 degree angle. In an interesting note, if you stand in front of the sensor like half a meter away, the sensor will not pick up that there are legs in between it and the wall.
The limiting factor of the sensor is the speed of sound. From the meassurements, we conclude that the microphone can only pick up the echo signal within about 12 milliseconds. If that is correct, the sound will indeed only be able travel about two meters and back, within the time slot.
3.2. Tracking experiment
We had now tested the sensors capabilities, so it was time to make a program use them. The first program we tried out with the new sensor was Tracker.java. The Tracker program will make the robot drive itself forward or backward to a desired distance from the wall (hardcoded to 35cm).
The first thing to notice is that the system only works if there is an object in front of it within 2,1 meters. The next thing to notice is a bit more exciting. The Tracker program makes Thomas (the robot) speed-up if the wall is far away/very close. This makes the control system a proportional control system. In a feedback loop measures an "error" defined as the desired distance minus the actual distance from the wall. The measured variable -- the distance from the wall -- is then used to influence the controlled variables -- the motors. The corrections are made according to the size of the error, and not just a binary response to the environment.
What sets this program apart from previous program is only two lines:
error = distance - desiredDistance;
power = (int)(gain * error);
These are the lines that makes Thomas act more dynamically to the environment. The rest of the program is only used to make the robot go forward or backward, and to apply the power to the motors.
Now that the code has been debunked, lets look at the results we got from changing the code. The tracker program run a feedback-loop every 300 msec, which gives Thomas plenty of time to go too close to the wall, and back too far away from the wall. As a way to combat oscillation, we reduced the amount of power given to the motors when getting closer to the wall. As a result Thomas stopped oscillation, but for the wrong reasons. With only 60% (power can be a integer value between 0-100) motorpower Thomas was too heavy to move, resulting in him never reaching the desired distance.
After this tinkering about, we decided Thomas needed some more exercise, and started to rebuild him for the wall following program.
3.3. Building the wall follower
The only thing we needed to change on Thomas was to turn his sonic sensor to a 45 degree angle. So we made a turning turret with the ability to lock. To begin with the turret was mounted on the top (pictured), which put the sensor fairly high up. Later we mounted the turret on the front, making it easier for it to follow the low walls of the obstacle-course box.
The Not-quite-C-program from Philippe Hurbain was chaotic. It
- uses arbitrary constant names
- interfaces its distance sensor extremely ad-hoc
- uses to-his-sensor specific raw values
Besides this, it's fair to say that the program isn't quite C (pun intended), so the language is only recognizable, while not being familiar. All in all, there's a latent challenge in understanding its implemented algorithm, let alone convert the program to the Java/lejos API.
But that was the matter at hand. As time progressed, it appeared that the XLn-constants designate left-bounds on distance, while being measured negatively: Larger values means more close. The XRn-constants are also distance bounds, but this time they're right-bounds, with high values corresponding to less closeness. XR3 is defined but not used (!).
At this point it was chaotic, and the major steps of the algorithm were replicated instead of stringently converting the program. The program does stepwise bang-bang with steps defined by distances. I.e. within certain thresholds, applies certain corrective measures, such as having the one motor float while the other one goes ahead at full speed.
We have the luxury of measuring in centimeters, so our code's constants could be assigned some sense, and expressions like ``far from the wall'' could be quantified in some way.
Features of our algorithm
Here is the java code we ended up with:
public class TrackerMod
private static final int VERY_CLOSE_LEFT = 20;
private static final int SOMEWHAT_CLOSE_LEFT = 25;
private static final int NOT_THAT_CLOSE_LEFT = 30;
private static final int SOMEWHAT_FAR_RIGHT = 35;
private static final int VERY_FAR_RIGHT = 40;
private static final int ASSIGNED_SPEED = 70;
private static MotorPort leftMotor = MotorPort.C;
private static MotorPort rightMotor = MotorPort.B;
public static void main (String aArg)
UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
int noObject = 255;
int distance = 0,
desiredDistance = 35, // cm
minPower = 60;
float error, gain = 0.5f;
int lmotor_speed = ASSIGNED_SPEED,
lmotor_mode = 1,
rmotor_speed = ASSIGNED_SPEED,
rmotor_mode = 1;
LCD.drawString("Pres enter.", 0, 1);
while (! Button.ENTER.isPressed());
while (! Button.ESCAPE.isPressed())
lastdistance = distance;
distance = us.getDistance();
difference_between_this_and_last = lastdistance - distance;
lmotor_speed = ASSIGNED_SPEED;
lmotor_mode = 1;
rmotor_speed = ASSIGNED_SPEED;
rmotor_mode = 1;
if (distance >= NOT_THAT_CLOSE_LEFT)
if (distance >= NOT_THAT_CLOSE_LEFT)
rmotor_speed = 0; rmotor_mode = 4; //stop (by float)
else if (distance >= SOMEWHAT_CLOSE_LEFT)
rmotor_speed = 0; rmotor_mode = 3; //stop
else if (distance >= VERY_CLOSE_LEFT)
rmotor_speed = ASSIGNED_SPEED; rmotor_mode = 2; //back
if (distance <= SOMEWHAT_FAR_RIGHT)
if (difference_between_this_and_last < 2)
lmotor_speed = 0; lmotor_mode = 3; //stop
else if (distance <= VERY_FAR_RIGHT)
//lmotor_speed = 0; lmotor_mode = 4; //stop (by float)
lmotor_speed = ASSIGNED_SPEED/2; lmotor_mode = 1; // slow turn
LCD.drawString("Distance: "+distance+" ", 0, 1);
rmotor_speed+"@"+rmotor_mode+".", 0, 3);
LCD.drawString("Program stopped", 0, 0);
Unlike the text-book algorithm, our source code actually have variable-names that make sense (or so we claim). Also the lejos frame-work also makes the code more readable. A command like Button.ENTER.isPressed() isn't hard to understand.
As the text-book algorithm, our algorithm regulates the controlled variable (again the motors) through the measured variable (the distance to the wall). The distance to the wall will be within one of 5 intervals: VERY_CLOSE_LEFT, SOMEWHAT_CLOSE_LEFT, NOT_THAT_CLOSE_LEFT and so on. The motors are then regulated to drive more or less to the left or right.
In the end of every loop the stats of the robot is being displayed at the LCD.
Testing our algorithm
When we tested Thomas with our software, it worked, and it was able to make a course with walls and corners. But the code isn't optimal. The turning speed when to far or close to the wall is a bit too aggressive. As a result Thomas ended up working like a bang-bang control system, because he would oscillate between VERY_CLOSE_LEFT and VERY_FAR_RIGHT.
We tinkered around with the sensor, and made it able to see various distances. We also were able to explain the limitations of the sensor via arguments of speed-of-sound, and have tested its accuracy.
The Philippe Hurbain source code was chaotic, and it turns out that all it does is stepwise proportional control, with the common case practically being equal to bang/bang within the two nearest thresholds. We however did not try to see the algorithm's reaction to various abnormal situations. (with the exception of the too-far-to-see case, which we did try to refine upon).