Programming of Robots and Other Physical Devices (OK-PRF17) University of Southern Denmark
By Group 2: David Nyrup Dyrholm (dadyr13), Nicklas Mandrup Frederiksen (nifre12), Emil Lindgren Ipsen (emips12), and Martin Kjølbye Sørensen (maso413)


This paper focuses on getting several autonomous agents working together solving a task as a multi-agent system. In the idea of a multiagent system, an agent is a system that is capable of independent action on behalf of its owner. That means that an agent can figure out for itself what it needs to do in order to satisfy its objectives rather than having to be told exactly what to do at any given moment. A multiagent system is then one that consists of a number of agents, which interact with one another, for example exchanging messages to each other (Wooldridge, 2009, p. 5).

The main idea of the project is for the agents to sweep an area and jointly create a “map”. The area consists of several flat buildings made with colored carton wherein one of them has a hidden treasure. When all buildings have been found, the buildings will be swept to find a treasure, and if an agent finds the treasure all agents should move to it.

The agents are made with LEGO Mindstorms NXT with sensors and actuators attached to them. Each agent has its own internal map and shares data through the Lego Bricks built-in Bluetooth device.

During the project, the different tasks of the agents were divided among the project groups. Our group was to focus on building the Bluetooth part of the software. The task was to take a coordinate string and send it to another agent. The other agent should then send it to the next agent in line. Other tasks, that are maintained by the other project groups, includes

  • turning and moving towards a target,
  • sweeping the map for buildings,
  • sweeping a building to find the treasure, and
  • storing data internally.

By combining these different parts the task should be feasible.

Problem definition

The main tasks of this project are:

  • to build a robot agent using Lego Mindstorm NXT 2.0 that can autonomously find a treasure on a map
  • the agent should be able to sweep an area and store coordinates for found buildings
  • the agent should communicate with the other agents to create a shared map of the search area

Later in the project this definition was refined into five subtasks and divided among the project groups as follows:

  1. One should detect different buildings and obstacles with the color sensor.
  2. One should connect to other agents; one agent connects and sends data to another. It receives confirmation and closes the connection again. Then the other robot sends the received data to yet another robot and so on.
  3. One should sweep buildings to see if the treasure is inside the building.
  4. One should construct and coordinate shared data and likewise make it work like a database.
  5. One should navigate the robot to a specific GPS location by received coordinates (latitude and longitude).


In this section, we will describe our process during this project. There will be a description of our methods and the components that we are using.

The process

The process of the project can be divided into six parts which will be briefly described in the following section.

In-class brainstorm

The project started by an in-class brainstorm where the object was to define the main topic and goal for the project. These were found by an online voting tool where everybody in class had the opportunity to add keywords. This helped establish a broad understanding of what the class wanted to do in the course. In the next lecture, we then voted for, and discussed, which idea we wanted to go with.

Conceptual design

The requirements for the design is that the agent should have the ability to drive around easily on the map without any problems. In this case, the agent should not be too big or have any hassles to turn around and drive. The agent should, however, be big enough so it can contain the wished amount of sensors connected to it.

Building the robot

The building, and tweaking of the agent was done continuously during the process, but a big part of the construction happened in short sprints. This means that the agents firstly was built so they could drive around, and later some groups chose to rebuild their agent, to better afford the main goal of the project (eg. having areas to place sensors).

Discussing algorithms

At this point, it was time to decide how the agent should behave when facing the different tasks. Over the course of two hours, we examined which tasks the robot should do. The resulting algorithm can be seen in the Result section.

Assigned focus area: Bluetooth connection

At this point were assigned a part of the system we were to make. The task we got was: Get the robots to automatically assign master/slave-roles in a circular fashion, in order to share information across the MAS.

Assembling the parts

When all the groups were done with their tasks, each group was then expected to implement the other groups’ work on their agent. We tried to implement their code and adjust it to our system on the agent. Read more about the result in the Result section.

Components and materials

In this section, there will be a description of our components, materials and the environment where the agent should operate in.

LEGO Mindstorms NXT

LEGO Mindstorms NXT was used for this project, which comes with a various number of sensors and with the possibility to add more from external fabricators. The brick consists of four inputs for sensors (1-4), four inputs for motors (A-D) and has Bluetooth support.

The robots are programmed in Not eXactly C (NXC) which is an Open Source C-like language.

For tracking the destination of the agent, a GPS was used to track the position of the agent and to record any tracked buildings. Besides longitude and latitude, the GPS module also provides information about speed, time, distance to a location and heading to a location. The heading functionality in the GPS works in the way that it compares two coordinates but if it stands still, it returns a random value. Thus, a compass was necessary and for this the NXT Compass Sensor was ideal. For detecting colors on the map, a color sensor was attached to the agent. This was also done by using the NXT native color sensor. By distinguishing a range of six colors, the agent can scan and react to various colored buildings. Two servo motors are used for the agent to navigate. The motors also have a built-in rotation sensor that enables the agent to go forward, backward and turn left and right with great precision.

Fig 1: Compass Sensor Fig 2: Color Sensor Fig 3: Servo Motor Fig 4: GPS

The Environment

The environment is a printed set of buildings from the map of the University of Southern Denmark. The full map with the buildings marked can be seen in Fig 5.

By creating a bigger map and bigger buildings, the less dependent on the GPS accuracy is the agent, but the greater are the costs for creating the map and the area needed for the final demonstration. Every building has its own color which is used as an id for that building.

Fig 5: An overview of the map

The map is also divided into tiles as shown in Fig 5. In the database, every tile contains a number representing a state. The different states are as follows:

State Description
0 is empty
1 is marked as upcoming tile to be searched
2 is currently being searched
3 is tile has been searched as is out of treasure
4 to 9 grey building = 4, blue building = 5 and so forth


Through the initial brainstorming process, the class decided that the topic of this project should be a treasure hunt. Before hunting for the treasure, the agents should map the area they moved in by saving coordinates for buildings/obstacles and share them with the other agents. This led to our conceptual model: The agent needs to be able to find and share GPS coordinates, generate an internal map from the coordinates, and then scan the found buildings for a treasure.

An agent was constructed with the required sensors for solving the task. These included a GPS, a compass, and a color sensor as described in Components and Materials. The robot can be seen in the images below.

Fig 6: Final build of our agent Fig 7: Final build of our agent

It was found that the GPS was rather inaccurate inside buildings as the walls were made of concrete. The GPS reacted inaccurately or out of operation when testing inside the building. Thus, all tests and the final demonstration took place outdoor. As above mentioned, the compass sensor was during the project added to the agent because of the inaccuracy of the GPS. Both to be able to find the heading of the agent and for a more flawless drive. Furthermore, in the specification of the GPS, it says that it has a precision of three meters, but the testing showed more inaccuracy in some cases. This made it impossible to complete a test with any reliable results.

Now, the behavior of the agents had to be decided upon. This was done by first finding the tasks that the agent needed to be able to do and then finding out which behavior the agent should have in each situation. The proposed behavior can be seen in the slideshow in Appendix A (Google Slide). The tasks were as follows:

  • Finding coordinates
  • Sending coordinates
  • Receiving coordinates from other robots
  • Saving coordinates in internal database using NXT filesystem
  • Turning and moving towards received coordinates

To have a higher chance of being able to finish the project in time each group was assigned a primary focus area. In this paper, the following task will be presented and reviewed:

Get the robots to automatically assign master/slave-roles in a circular fashion, in order to share information across the MAS.

Fig 8: Network Topology

The idea of sharing data was a network circle (see Picture 8), where one agent sends data to another agent, which then compares with its data and then sends it to the next agent and so on. A complication arose as LEGO Mindstorm NXT can only be either master or slave at a time – not both. For solving this, a theory was discussed: an agent connects to another agent and sends data. The slave agent then sends back an acknowledgment to the master agent whereas it disconnects. The former slave agent now connects to another robot and becomes master itself and repeats the process.

We worked on this by acquiring multiple NXT-units and testing the code on them. The code was made iteratively – sometimes by one person coding it and sometimes by using collaborative coding. We debugged by printing text to the LCD screen and verifying that the returned values were correct.

When all groups had done their part, each group were to assemble the methods into a single algorithm which their agent should use. As of now, not all groups have turned in their code and some are not working properly. Unfortunately, we were also not able to finish the code. This was mainly because of NXC’s lack of documentation. Because of this, we ended up using a lot of time guessing return values of native methods. Googling NXC problems also gave little to no information. This has made the assembly of the final code very hard to achieve.

The code

Nonetheless, we have tried to implement the different parts from the other groups into a single algorithm. In the code, we use pseudo-code to show subtasks that did not get made or that we could not implement due to differences in understanding of the project task. This also means that the code does not work, but more on that later.

The main algorithm is run as a state machine. This means that we control the flow of the algorithm by continuously changing the mainState variable. The overall flow will be described the “Proposed structure of the final robot code” section. 

mainState = 1;
task main() {
    brickName = BrickDataName(); //Gets current NXT name

    //Check if gps has satelite connection before doing anything
    if (DGPSreadStatus(DGPS)) {
        switch (mainState) {
        case 1: //Finding initial coordinates

            //By setting coordinates the robot starts to go
            destLat = lat[0];
            destLng = lng[getRobotIndex(brickName)];
            currentLngLatIndexes[0] = 0;
            currentLngLatIndexes[1] = getRobotIndex(brickName);

            //Update map and send to other robots
            UpdateMapField(0, getRobotIndex(brickName), 1);

            //prevent this state from rerunning
            mainState = 100;

        case 2: //Driver end controller - stops the robot
            //Stop the robot
            destLat = NULL;
            destLng = NULL;

            mainState = 100; //prevent this state from rerunning

        case 3: //Find new tile
            findNewTile(0, TRUE);

            mainState = 100; //prevent this state from rerunning

        case 4: //Find new building tile of the same number
            findNewTile(5, FALSE);

            mainState = 100; //prevent this state from rerunning

        case 5: //Treasure found by us
            //Stop robot!
            destLat = NULL;
            destLng = NULL;

        case 6: //Treasure found by others

	    //Set destination for the treasure
            destLng = treasureLngLat[0];
            destLat = treasureLngLat[1];

            mainState = 100; //prevent this state from rerunning

        default: //State nothing
            //Do nothing

        Precedes(BTMaster, BTSlave, ScannerDriver);
    } else {
        TextOut(0, LCD_LINE8, "GPS: DOWN");

Below here we have the tasks that run beside the main task. These are run on their own thread and are started by using the Precedes() method as can be seen in the main task above on line 64. The tasks are also, like main() controller by states though they are a little more subtle in the code. An example: in the ScannerDriver task, here on line 23, we have a conditional checking of destLat and destLng is set. This means that if there are currently a set of coordinates available the robot heads for those (on line 25).

task BTMaster() {
    //Both sendString and receiveString should be run inside a loop
    while (TRUE) {
        //sendString returns a boolean
        if (sendString(stringData, getNextRobot(brickName))) {
            stringData = ""; //Setting this to nothing prevents sendString from doing anything

task BTSlave() {
    while (TRUE) {
        response = receiveString();

task ScannerDriver() {
    SetSensorLowspeed(Compass); //Necessary for compas to work properly

    while (TRUE) {
        //If a destination is set
        if (destLat && destLng) {
            //move to destination
            moveRobotTowardsLocation(convertFloatToLong(destLat), convertFloatToLong(destLng));
        } else if (treasureFound) {

            while (TRUE) {
                //TREASURE IS FOUND! HURRAH!

        } else {
            if (allExplored == FALSE) {
                sweepTile(currentLngLatIndexes[0], currentLngLatIndexes[1]);
            } else if (allExplored == TRUE) {
                sweepBuilding(currentLngLatIndexes[0], currentLngLatIndexes[1]);

Here we have the some of the code for our subtask, connecting through Bluetooth. It consists of two methods, sendString() and receiveString(), which are both internally state machines. SendString waits in its default state until it receives a string to send and then starts sending by setting BT_Master_state to zero (first state in the sending process). ReceiveString likewise waits in its default state for when a connection opens up on the NXTs master port (port zero). Then it sets in motion, going to state zero, which is its “wait for data”-state. The states of the methods can be seen on line 1-8 and 96-102.

int BT_Master_State = 100; //Default is 100
//sendString states:
//0 = connecting,
//1 = connected / sending data,
//2 = data sent / waiting for acknoledgement,
//3 = acknoledgement received / validating
//4 = closing connection
//100 = default state: waiting for a string to send

string ack = ""; //Used to save acknowledgement string

 * Send the specified string to the specified robot
 * @params: string stringData: the string that should be sent; string robotName: name of the robot you want to send to
 * @return: boolean
bool sendString(string stringData, string slaveRobotName) {
    switch (BT_Master_State) {
    case 0: // Connecting
        until(connectToRobot(slaveRobotName, SLAVE_PORT) == TRUE);
        BT_Master_State = 1;

        if (BT_DEBUG) {
            TextOut(0, LCD_LINE2, BrickDataName());
            TextOut(40, LCD_LINE2, ">");
            TextOut(47, LCD_LINE2, getNextRobot(BrickDataName()));

    case 1: // Connected / sending data
        if (SendRemoteString(SLAVE_PORT, MAILBOX1, stringData)) {
            BT_Master_State = 2;

    case 2: // Data sent / waiting for acknowledgement
        until(ReceiveRemoteString(MAILBOX2, true, ack) != STAT_MSG_EMPTY_MAILBOX);

        BT_Master_State = 3;

        if (BT_DEBUG) {
            TextOut(0, LCD_LINE7, "Re.Akc:");
            TextOut(50, LCD_LINE7, ack);
            TextOut(0, LCD_LINE3, "Awaiting Ack.");

    case 3: // Acknowledgement received / validating
        if (ack == stringData) {
            BT_Master_State = 4;

            if (BT_DEBUG) {
                TextOut(0, LCD_LINE3, "Data Ack.");
        } else {
            if (BT_DEBUG) {
                TextOut(0, LCD_LINE3, "Error. Resending");

            SendRemoteBool(MAILBOX4, TRUE, TRUE);
            BT_Master_State = 1;

        ack = "";

    case 4: // closing connection
        until(closeConnection(slaveRobotName, SLAVE_PORT) == TRUE);
        BT_Master_State = 100; //Setting Master to default state
        return true;

    default: // Checking input
        if (stringData != "") {
            if (BT_DEBUG) {
                TextOut(0, LCD_LINE8, "I'm the Master");
            BT_Master_State = 0;
        } else {
            if (BT_DEBUG) {
                TextOut(0, LCD_LINE1, "No Data to Send");

int BT_Slave_State = 100; //Default is 100
//receiveString states:
//0 = Connected / receiveing data
//1 = Data received / sending acknowledgement
//2 = Awaiting response from Master
//3 = Data confirmed - return data
//100 = Waiting for a connection on port 0 (master port)

 * Listens for and receives a string from a master robot
 * @params: null
 * @return: string
string receiveString() {
    string receivedString;
    switch (BT_Slave_State) {
    case 0: // Connected / receiveing data
        until(ReceiveRemoteString(MAILBOX1, true, receivedString) != STAT_MSG_EMPTY_MAILBOX);
        BT_Slave_State = 1;

        if (BT_DEBUG) {
            TextOut(0, LCD_LINE2, "Received:");
            TextOut(0, LCD_LINE3, receivedString);

    case 1: // Data received / sending acknowledgement
        SendResponseString(MAILBOX2, receivedString);
        BT_Slave_State = 2;

        if (BT_DEBUG) {
            TextOut(0, LCD_LINE4, "Sending Ack.");

    case 2: // Awaiting response from Master
        while (TRUE) {
            bool error;
            ReceiveRemoteBool(MAILBOX3, true, error);
            if (error == TRUE) {
                BT_Slave_State = 1;
            } else if (checkConnection(0) == FALSE) {
                BT_Slave_State = 3;

    case 3: // Data confirmed - return data
        BT_Slave_State = 100; //Return to default state
        return receivedString;

    default: // no master connection,
        if (checkConnection(0) == TRUE) {
            BT_Slave_State = 0;

            if (BT_DEBUG) {
                TextOut(0, LCD_LINE8, "I'm the Slave");

    return "";

The complete code can be downloaded from here: Grp2 code.

Because the code does not work we have made a flowchart where we try to visualize how we think the robot is supposed to utilize the different subtasks. This will be shown in the next section.

Proposed structure of the final robot code

Fig 9: Flowchart over the system

As stated before, each group in the project had been assigned a functionality which they should code. This section will briefly describe a proposed structure of the final code and will refer to the code that the groups wrote. This proposed structure should be seen as a reference point that would assist the groups in making a unified codebase for the NXT units, and will not deal with current software issues in the code.

This setup requires the agent to run four tasks/threads: two Bluetooth (connector and listener), a data handler and a scanner/driver (which can also be split into two tasks if necessary). As the two Bluetooth tasks depend on each other, they will be described together in a section. In the following section, the tasks will be referred to as BTM (Bluetooth Master), BTS (Bluetooth Slave), DH (Data Handler) and SD (Scanner & Driver). A flowchart showing the structure can be seen above.

Bluetooth connector task (master) and listener task (slave)

The two Bluetooth tasks handle sending and receiving data. BTM is idling until it receives data from DH. When it receives data it tries to establish a connection to its designated slave. The BTS, which also was idling, then receives a connection handshake from the master and confirms the connection. When the BTM receives the confirmation it sends data to the BTS, and wait for it to return the data. If the data returned is the same as the data delivered, it will terminate the connection. If the data is inconsistent, it will send forth and back the data again, until it is the same. When the connection is terminated, the BTS then sends the data to the DH, and then both Bluetooth tasks goes back to idling.

Data handler flow

The DH is not a task but on the other hand a set of methods, that when called, handles the data. When it receives the data, it updates the map, whereafter it checks whether the treasure is found or not. If the treasure is still not found, it sends the updated map data to the BTM, which then sends the updated map to the next slave. If the treasure is found it sends the treasure data to the BTM. Then it updates the treasureFound boolean the SD is checking.

Scanner & Driver

The SD checks if it has any coordinates assigned, and then drives to these coordinates (if any). Then it checks whether the treasure is found or not. If the treasure is not found, it checks if the whole map has been explored yet (by scanning the array for unsearched tiles), then finds a unscanned tile, drives to the tile, and then sweep it while scanning. When it confirms a color, it sends the data to the DH. If all tiles are explored (which mean there are no available tiles), the robot assigns itself a building color it then starts sweeping (the same procedure as sweeping tiles for buildings). If the robot finds the treasure, it sends a signal to the other robots, which will then drive towards the tile with the treasure. And on the other hand, if the robot receives a signal telling it that the treasure is found, it drives towards the treasure containing tile – and when it reaches the goal, the program ends.


The main goal of the project was to build a robot that could collaborate with other agents. This should be easy in theory, but in reality, it gave us a lot of problems. One basic problem we encountered was the communication between the groups building the agents. The problem was that we built different agents that did not work together. To combat this we later in the process divided the subtasks between the groups, so everyone worked on the same agent, but unfortunately, that did not make it less confusing. Managing a divided project can be hard if there is no central person coordinating the bits and pieces. This meant that not all groups coded something that could be used by others because that is hard to do when you do not know their code and there is no one to guide you in the right direction.


Another problem was working with the Bluetooth module of the NXT. Our subtask was to setup the communication between agents through Bluetooth, but the NXT Bluetooth module is not optimal for this task as it can not handle being both master and slave at the same time. This meant that to pass data around like shown in picture 8 an agent needs to open a connection, send data and then close the connection again. This should, in theory, be possible, but the NXC language and documentation gave us a lot of trouble trying to build this. Therefore we were not able to get the communication between multiple agents through Bluetooth to work. It should be possible to do with more time. Another solution could be to rely on WIFI communication instead as this is more capable of creating the kind of communication we need. Also moving away from NXC could be advantageous. More on this later in this section.

Bluetooth was not the only thing that gave us problems. Another problem the GPS module which has difficulties working indoor. This is because the module has trouble receiving signals through walls and thus, testing becomes a hassle as you have to be near a window or, in the worst case, go outside for debugging the code. An alternative solution could be to program the agent to wait for more accurate location if the accuracy of the found location if it is too much off by some fixed number. The agent should then not go for the inaccurate location, but wait for the more accurate location. Another problem was the precision of the GPS. This has had a huge impact on the overall project at most of the tasks the agent needed to do required precise measurements from the GPS. Finding a better GPS-module would be the obvious solution, but another one could be positioning using wifi. This should also work indoor.

Another component we did not get to implementing but would like to add if we had more time was an ultrasonic sensor. This is for giving the agent vision to detect and avoid other agents as they drive around on the map. This was also suggested by other groups.


In the process of gluing all the source codes together, a discussion took place about synchronizing the map between agents. A problem could occur if two agents independently of each other searched and updated their own map and then sent it to the next agent – two agents could have two different maps because the period between agents connecting and disconnecting is at least a couple of seconds. This adds up to many seconds for an agent to pass information to the next agent and so on until the circle is closed. In between the time for the circle to close, two or more agents could have done a task and updated their own map and thus, two or more fields in the grid would be conflicting when comparing field states. As a result, we made a modification to the code for the map; instead of just overriding the map with the received string, the method for updating the map checks for whether the fields contain a greater state number and only then updates the current field state. This way, a field state can only go upwards and we can assure that any actions in between synchronization of the map do not get lost.

By using a circle as network topology with Bluetooth on multiple NXTs, it takes a minimum of 3-5 seconds per NXT for the circle to complete which is not optimal. The network topology could be modified for further work.

Coding, debugging, and testing

Something we really struggled with was the NXC language. Early in the process, we found that the standard Lego block language was too limited so we switched to NXC. This was initially a good idea because it gave us more options for programming the NXT, but the more we worked with NXC the more it showed its flaws. Its biggest flaw is its horrible documentation. We ended up using a lot of time trying to find methods for even the most simple operations. Also, it did not always list what a particular method would return. This led to a lot of programming where we basically coded in the blind. This was one of the main reasons we did not complete our task. A solution for this could be to just select some other language with better documentation. The NXT is very flexible and there are a lot of languages that you can use for programming it. The only requirements, if you ask us, are that it should have a clear and precise documentation and that there is a person available that knows the language enough so that we can ask for help if needed.

To test we had to rely on the LCD-screens on the agents as the IDE did not have any built-in tools. We started with only two agents that sometimes were able to automatically connect and disconnect and also send and receive data. A third agent was never incorporated in the tests because we could not get the NXTs to close the connection and thus freeing it for opening a new one.

Good/bad things

Though this project has been frustrating at times there are some good things we would like to point out. One is definitely the new experience that we have earned, especially with programming NXC and with the components that we didn’t know beforehand. It has been a challenging and instructive project from the beginning and by failing a project like this we have learned that we should talk more with the other groups and be better as a class to collaborate. This also shows how important management is in large projects.

And especially the lack of management and leadership was one bad thing in this project. As an example, we had class conversations where we spent a huge amount of time discussing how we should solve project issues. To be precise: there is a reason that SCRUM teams are usually not larger than 9 people. It is simply too hard to agree on anything in larger groups. Thus it has been really difficult to get to an agreement between all five groups and here it would be very helpful if someone, the teacher or a student, was appointed as lead manager and could make the final decision.

Conclusion and Perspective

A final demonstration never took place, and in short, we have to conclude that we couldn’t solve the given problem in the given timeframe, with the resources we were given, and the amount of inter-group work we had. However, the project as a whole was an important and interesting experience – both in regards to the subject and group work.

That being said, it is not impossible to think of a scenario in which the problem could have been solved to a greater extent. For such a scenario to happen, one or more of the following points had to be met:

  • A broader timeframe
    • This could be done by shortening/skipping the pre-project brainstorming phase and the paper presentations
  • More reliable hardware
    • This mainly includes the imprecise GPS module and the spotty Bluetooth signal
  • Know-how of the NXC language before starting the project
    • Either before the course or concurrently with the pre-project brainstorming
    • A more thorough introduction or study into the language could also be a possibility
  • More structured/coordinated work in between the groups
    • Getting the designated subtasks earlier, and working towards/with a specified template, would greatly have helped coordinate the shared codebase.
    • Also having someone act as project manager and coordinator could have helped when people were confused about the project description



Wooldridge, M.J. (2009). Multi-agent systems: an introduction. Chichester: Wiley.




Fig 1: (Accessed 01-05-2017)

Fig 2: (Accessed 01-05-2017)

Fig 3: (Accessed 01-05-2017)

Fig 4: (Accessed 01-05-2017)

Fig 5: Course material (Accessed 28-04-2017)

Fig 6: Private picture (Accessed 01-05-2017)

Fig 7: Private picture (Accessed 01-05-2017)

Fig 8: Course material (Accessed 22-04-2017)

Fig 9: Own flowchart (Accessed 01-05-2017)



Presented midterm algorithm:

The complete project code: code


Leave a Reply