A Lego-robot with camera controlled by Matlab

Links to the different sections of this documentation:

| overview || parts || the_robot || java || legOS || legOS_program || matlab || matlab_programs || camera || discussion || download |

The robot in action, following a line :)

Author: Asmo Soinio, asmo@kortis.to, 8.8.2002 (contact information updated 14.10.2005)

Overview

My project was to create a robot that could be controlled from Matlab and that has a camera. An additional need was that the robot could be controlled from a PC running Windows 2000. This documentation is written for anyone whom is interested in doing something similar. The programs I wrote can be downloaded from the download page.

The Lego Mindstorms Robotic Invention System seemed to be an affordable way to create the robot. I didn't find any interface to communicate with the Mindstorms robot from Matlab, so I used a Java-Lego-Network-Protocol -interface that I edited to suit my needs. Java objects can be directly used from Matlab. The robot itself uses legOS (nowadays known as brickOS) as it's operating system. The camera is the one that comes with the Lego Vision Command-kit.

I had troubles with the PC-software provided by Lego, as it isn't compatible with Windows 2000. That didn't cause any problems, though, because none of the PC-software by Lego company is used.

Update 25.07.2003: My legOS program fails atleast with brickos-0.2.6.10, so please use legOS 0.2.4, which is the one I used

Disclaimer

LEGO^® is a trademark of the LEGO Group of companies which does not sponsor, authorize or endorse this site

<< Back to top

The differents parts of the project

I have documented the project by dividing it to a number of different parts. Here are the different parts of the project as a table. Each part uses (or controls) the part above it in the table. The parts documented here (more or less) are in bold text.

The Lego-robot
Lego-robot's operating system (legOS)	Camera on the robot
legOS program
IR-receiver and the IR-tower	VFM, a free tool for image grabbing in Matlab
The java-interface to communicate with legOS (using LNP)
Matlab programs
Matlab

<< Back to top

Picture of the robot

Constructing the robot

My first intention was to just build a robot and design as I built it. When I got the RIS i realized that it wasn't going to be such a trivial task, so I just built on of the basic models from the Legos Constructopedia book, the one called Roverbot. Testing the robot i realized that it would run most smoothly with tracks. I also added the bumper to the robot to detect collisions, but it isn't really used by the Matlab-programs.

The tracks

As I couldn't make the Matlab program run really fastly I decided to make the robot a bit slower instead. The tracks in the Constructopedia have a ratio of 1:3 from motor to the tracks, but i edited them by adding another gears in between giving the ratio of 1:9. These pictures should give some hints about how I did it:


The modified tracks	The modified tracks seen from below

Attaching the camera

I used two different positions for the camera, one for following a line and another for detecting objects and following a trolley. In the line-following-position the camera points more down so that the robot 'sees' thing that are really near the bumper.


Camera in line-following-position	Camera in the other position	Vision field of the robot, object detection

The IR-tower attached to rear the robot

The IR-communication

My idea at first was to have the IR-tower staticly somewhere above the robot so that the communication between the PC and the robot could be wireless. After some testing i realized that messages sent from the robot to the PC were easily lost even though i did put a reflecting plate in front of the robots IR-sender to help the communication. As i still had to have the USB-cable for the camera i decided to just skip the wireless communication. Attaching the IR-tower to the robot was the easiest way to do this, and that's what I did. Ugly, but it works. :) I also connected a longer cable to the IR-tower, and I use 2 wires in the same cable to feed the 9 V power to the tower and the 'Brick'.

Using an external power source

The RIS I use is of version 1.5, so it has no plug for external power source. Changing batteries sucks, so I simply opened the 'Brick' and attached wires to the points where the 'batterybox' is connected. This was a quite easy operation, but I take no responsibility if You brake your RIS-brick trying to do it. :) I use an AC/DC adaptor with 9 V / 300 mA output to feed the IR-tower and the robot, and it seems to be powerful enough.

<< Back to top

Java interface to communicate with the robot

The interface is based on JavaLNP. It can be used without Matlab by simply sunning the main class with

java robot3.Controller

This opens up two windows, one for showing the state of the robot and another for inputting commands to the robot. The robot should have the program robot3.lx running so that the communication will work.

The sources and classes are included in the projects distribution file available at the download page.

See also the Javadoc's of the Java-classes (also available in the distribution file).

Aside from the robot3-package the distribution file also includes the class-file for the class PortTest. It is used to avoid a bug in Matlab, see the Matlab page for more details.

<< Back to top

legOS for Windows

If you are not familiar with what legOS is, please visit http://brickos.sourceforge.net/ of http://www.noga.de/legOS/ first. As it wasn't at all trivial to find a working version of legOS for windows, i decided to dedicate one page for it as well...

Note: It will be hard to predict how long the information here will be valid, as legOS has changed it's name to brickOS, and some of the http-addresses might change due to that.

I just wanted to get a working version of legOS that I could download the operating system and own programs to the robot, and a compiler so that I could write my own programs for the robot in C. The file to search for if called winlegos.exe, which is a Rossz Vamos-Wentworth's cygwin-free legOS version. It is also possible to run the 'nix version of legOS under windows using cygwin, but the installation using WinLegOS is much easier. Check out legOS documentation to find out how to download the firmware to the 'Brick'.

Update 25.07.2003: My legOS program fails atleast with brickos-0.2.6.10, so please use legOS 0.2.4, which is the one I used.

Check out these pages for download's and installation

http://legos.sourceforge.net/files/windows/djgpp/
http://legos.sourceforge.net/files/windows/cygwin/legOS-0.2.4.html
http://www.docs.uu.se/~tobiasa/RT-TF01/assignments/running_legos_at_home

You might also want to search for "winlegos.exe" on google.

<< Back to top

The legOS program

I wrote the legOS program in C and compiled it with the compiler that came with WinLegOS. Both the source code (robot2.c) and the legOS-binary (robot2.lx) are in the distribution file, under the directory legos. The program can be downloaded to the robot using the dll-tool of the legOS (assuming that you have legOS installed to the robot and the robot is on):

dll robot3.lx

The program for legOS is really simple. It just sets up the function to handle the integrity messages using

lnp_integrity_set_handler(my_integrity_handler);

After this the function my_integrity_handler is called when a message is received, and it sets the motor levels and directions according to the messages received. The main method runs all the time and shows the number of messages reveived on the lcd. This number can be used to make sure that every message sent by the PC is received, as the PC's Java-interface shows the number of messages sent.

<< Back to top

Setting up Matlab

My version of Matlab is 6.1, but I think the Java-interface works similarily with other version of Matlab as well. To use the java-classes in Matlab the directory of the classes must be added to Matlab's classpath. This can be easily done from Matlab by typing

edit classpath.txt

The directory to add is the root directory of the Java classes, for example c:\matlab_robot\java\classes, not the c:\matlab_robot\java\classes\robot2-directory. More information about Matlab's Java-interface can be found from the Matlab's help.

A bug and how to live with it

I had a weird bug in the system that prevented the java objects used from Matlab using the serial ports. I wrote a java program that lists the available ports and realized that running the PortTest program before changing from the default working directory to the directory where I had my robot programs made the bug disappear. The PortTest program is included in the distribution file, but without sources as I managed to delete them during some testing. :) The output of PortTest when everything is fine:

>> PortTest
List of available communication ports:
COM1 type:1 isOwned:false
COM2 type:1 isOwned:false
LPT1 type:2 isOwned:false
LPT2 type:2 isOwned:false

ans =

PortTest@75d726

>>

I think the bug has something to do with the file javax.comm.properties that defines the communication ports, but I didn't find any way to fix it

<< Back to top

The Matlab programs

The Matlab programs are documented in the code, so comments about all of them can be found using the Matlab command HELP FUN.

The function paramlabel is not written by me, but I couldn't find the authors name in the file, thanks to him/her anyway.

A thing that should be noticed is that for some reason my robot had a lot of difference in the power of the two motors. That is why in each of the motor-controlling Matlab-functions there's defined SPEED_NORMAL_A=50; and SPEED_NORMAL_B=150; as these were nice values with which the robot moved straight forward. These values will probably not work with some other motors, you should test to see what works with your robot.

These are the main programs you propably want to try (more info available with Matlab's help):

Program Comment

robot_travel_v2

Picture of the line I used, notice the
perpendicular bit in the end (there's one in
the other end as well)

Follows the line in the floor using robot_find_line and robot_move_to_end_v2, the version with a PD-regulator

If the script robot_init hasn't been ran, this script initializes the robot by running it.

The line is detected by counting the horisontal gradient of the image. The line is where the horisontal contrast is biggest. The perpendicular ending is detected by counting the maximum of vertical gradients of each vertical line. If the meadian of these is big enough, the robot sees the vertical line and is in the end. Any striped line should work just fine.

For line following i used resolution 160x120 so that the analysis can easily be done in realtime.

follow_trolley

Picture of the "trolley" I used

Tries to follow the trolley keeping it in the middle of the image and 30 cm away from the robot.

The function detect_trolley is used for (surprisingly =] ) detecting where the trolley is in the picture. It is detected based on it's colour, pink seemed a good idea since I have no other pink items in my room.

Uses two PD-regulators, one for the speed of the robot and one for the angle.

If the robot doesn't see the trolley it will turn left/right until it is detected in the picture.

For trolley following i used resolution 160x120 so that the analysis can easily be done in realtime.

analyze_object_types Grabs an image and analyzes the objects in it

Uses get_object_type to determine the type of each object, otherwise does the same as analyze_objects.

The objects are distinguished by a double threshold; really bright pixels and pixels a bit darker than the median are considered as objects. This works fine for me, but depends ofcourse totally on the color of the floor and lighting.

For object analysis i used resolution 352x280.

Example of a grabbed image and the objects detected Map of the objects

r2 Uses robot_move_to_end_v2 to make the robot to move to the end of the line and then plots the results

robot_init This script initializes the robot-controller and some global variables used to control the robot

The functions robot_travel, robot_travel_v2 and follow_trolley run this script automatically if it hasn't already been ran.

<< Back to top

Software for the camera

A few words about how I mounted the camera can be found from the 'Constructing the robot' -page.

As mentioned earlier, I used the camera that comes with Lego Mindstorms Vision Command. The program that came with the camera didn't want to install on my Windows 2000, but that caused no problems. The camera is actually a repacked Logitech QuickCam Web, and my Windows found the drivers automatically when I plugged in the camera to the USB port. If the drivers can't be automatically found they can be downloaded from Logitech's website.

Getting the images to Matlab

To grab the images to Matlab i use a tool called Vision For Matlab by Farzad Pezeshkpour. The binaries needed by the Matlab programs are included in my distribution file. The source codes of VFM can be found at http://www.isr.ist.utl.pt/~alex/Resources/vfm/ .

A few notes about VFM

When I installed another webcam to my computer (for taking the photos used in this documentation, BTW), I had some serious trouble with the VFM. It nicely asked upon startup which camera to use, but when doing the realtime image analysis it really often just stopped updating the picture returned to Matlab. Uninstalling the software and drivers for both cameras and reinstalling only the Vision Commander -camera helped.
VFM has a 'preview' feature (accessible through the menus) that updates picture in the VFM-window in realtime. When this option is on, the VFM will spend quite a lot of time updating the picture, so when doing the realtime analysis it's best to have the preview-mode off. (On my computer almost half of the time during the realtime-analysis is spent on waiting for new picture from VFM, so when it slows down to about one third of it's original speed the analysis will be much slower.)

<< Back to top

Discussion

I did manage to create a robot that can be controlled from Matlab and I did some real time image analysis programs to control the robot, and they worked quite nicely. I used a PD-regulator to make the robot follow a twisted line and I think the robot is a good tool for regulator realization as results are directly and clearly seen by anyone.

There are still some thing that should definitely have been done.

To do

Add rotation sensors. The basic RIS-kit doesn't include rotation sensors, so I didn't have them. They would have helped the control of the robot really a lot, as with the current system there's no way (other than doing image analysis on the camera's picture) to know exactly how much the robot has moved and how fast it is moving. If rotation sensors are included in the system the commands and legOS program should propably be totally rewritten. It should then be possible to order the robot to move some defined number of steps.
(I was thinking about doing a program that would make the robot drive some 'virtual' route that isn't visible, but that is almost impossible without precise control of the robot, ie. rotation sensors.)
Design the robot. It might be that the control would be more accurate if the robot had a better design. I just built it to have something to play with and changed the design as needed.
Mathematical model of the robot. My control functions were based purely on trial and error, but to do some more advanced things one should have a model of how the robots speed and angular speed depend of the motor levels. Here the rotation sensors would also help a lot.

<< Back to top

Downloads

If you want to use my software to create a Matlab controlled robot, or if something else documented here interests you, you can download the distribution file ( 281 KB) of the project.

To download the operating system and the program to the RCX Brick you will also need WinLegOs.

The distribution has following directories:

Directory Contents

\matlab_robot\matlab\ The Matlab files to control the robot and to use the camera

\matlab_robot\legos\ The legOS sources and binaries, ie. the program that runs in the RCX Brick

\matlab_robot\java\classes The Java-class-files for the Java interface that communicates with the robot

\matlab_robot\java\sources The Java-sources for the Java interface that communicates with the robot

\matlab_robot\java\javadocs Javadoc-documentation of the Java interface

\matlab_robot\docs\ This documentation of the project as one big HTML (the printable version)

Directory	Contents
`\matlab_robot\matlab\`	The Matlab files to control the robot and to use the camera
`\matlab_robot\legos\`	The legOS sources and binaries, ie. the program that runs in the RCX Brick
`\matlab_robot\java\classes`	The Java-class-files for the Java interface that communicates with the robot
`\matlab_robot\java\sources`	The Java-sources for the Java interface that communicates with the robot
`\matlab_robot\java\javadocs`	Javadoc-documentation of the Java interface
`\matlab_robot\docs\`	This documentation of the project as one big HTML (the printable version)

<< Back to top