Robotika.cz » Robots » Ester » Control Software

Control Software

Author: Zbyněk Winkler, translated by Sebastian Henschel, 2004-07-16

how to program a robot and not to go mad while doing so

Last year's experiences lead to the decision to completely rebuild the software for Ester to allow easier modifications and tests of different strategies. The robot's control considerably depends on the detection of its position and its surroundings (e.g. I am in front of the goal and I have the ball) and the application of the appropriate solution (shoot). The classic tool for programming that kind of application is FSM (finite state machine), but the usual implementations seem hard to maintain and in general are somewhat clumsy. Can't we do better?

FSM — finite state machines

The basic scheme we used in more or less all of our robots since Alena looks like this:

while (true)
{
    sendReceive(commandStruct, stateStruct);
    switch(state)
    {
        case INIT:               state = init(stateStruct); break;
        case GOING_TO_SUN:       state = goingToSun(stateStruct); break;
        case SUN_REACHED:        state = sunReached(stateStruct); break;
        case DEPLOYING_SUN_FLAG: state = deployingSunFlag(stateStruct); break;
        …
    }
}

It is a simple and straight forward solution whose functioning can be explained very easily. Basically, it is an endless loop in which we alternate communication with the robot and the processing of the incoming data.

The communication with the robot stabilized after the initial experiments with different asynchronous message types over the serial interface to synchronous communication using to two basic structures. The structure commandStruct contains the state we intend the microprocessor output pins to have and stateStruct represents the state of the input devices (A/D converter and digital input).

The processing of incoming data determines the robot's internal state. In the state INIT, for example, the robot waits for the start signal and in that case begins to measure the time and advances to the state GOING_TO_SUN. In all the other states it checks if 90s have passed and if the transitional condition, specified for the given state, is met.

Simple, clear, comprehensible at the first look. So where is the problem? The FSM, implemented in that way, fits a system which can be analyzed thoroughly and specified precisely all the needed states and transitions. Experiments show that a mobile robot is not such a system. There are many activities which seem to go across all the states. When we try to handle these activities, the number of states explodes beyond tolerable limits while many states are very similar. So the code structured like this changes quickly into a mess and becomes hard to maintain or extend. The main reason the code duplication (i.e. in the case of Alena, driving to the sun is nearly identical to driving to another planet etc.).

As you can seen in the mentioned examples even the actual definition of single states can become very hard problem. As long as there are states with multiple input edges, you (the programmer) have to guarantee that all the edges have the same or at least similar preconditions. For example in the state responsible for the sun's flag placement, it is necessary that the robot is next to the sun (otherwise the action wouldn't make sense). Due to the amount of code it is almost an impossible task. The structure of the code does not help, quite the opposite. In theory it is possible to transition from any state to any other state (complete graph) and it depends solely on the programmer (and his discipline) if the proper conditions are met. You can probably guess what happens when several programmers work simultaneously on the code or when someone has to continue in a work started by someone else.

Attempt to improve no. 1

The first attempt to regain control of our code was the implementation of a nested FSM. The motivation was that it is the same if the robot drives to the sun, the puck or the goal, so that driving to a target can be an independent skill that can be used in different situations. This is also valid for other activities.

The main loop will then look like this:

while (true)
{
    sendReceive(commandStruct, stateStruct);
    switch(state)
    {
        case AWAITING_START:
            controller.stop();
            if (stateStruct.startCable)
            {
                timeLeft = 90.0;
                state = PLAYING;
            }
            break;

        case PLAYING:
            controller.execute();
            timeLeft -= stateStruct.dt;
            if (timeLeft <= 0)
            {
                state = GAME_OVER;
            }
            break;

        case GAME_OVER:
            controller.stop();
            if (currentSpeed == 0)
            {
                state = AWAITING_START;
                controller.reset();
            }
            break;
    }
}

As you can see we've achieved starting the robot by starting cable and stopping after 90s (after the end of match, the control program resets and prepares for another fight which helps testing). The code for this activity is isolated from the rest of the control system and its easy to visualize what and why it does what it does. The control system is completely implemented in the controller object.

The necessary FSM-implementation of driving to a target can be isolated in a similar fashion:

int goTo(goal)
{
    offset = calculateOffsetToGoal(goal);

    switch (action(offset))
    {
        case TURN:        action = &turn;       return Command::IN_PROGRESS;
        case STRAIGHT:    action = &straightFW; return Command::IN_PROGRESS;
        case STOP:        action = &stop;       return Command::IN_PROGRESS;
        case IN_PROGRESS:                       return Command::IN_PROGRESS;
        case RESULT_OK:                         return Command::RESULT_OK;
    }
    return Command::IN_PROGRESS;
}

In this way, we've reached one of our goals — we managed to divide the code into smaller sections so different people can easily work with it (the coupling was reduced). But up to a certain point , we still have the problem with consistency of the transitions. Nested FSMs must be (re)initialized in certain situations and it is typically expected that its action procedures are called periodically in every step until they say “enough” themselves and that they keep complete control of some of the robot's sections (i.e. it means the goTo(goal) FSM controls the motors exclusively). However, these requirements still stay the responsibility of the programmer, even when the code modularization helps. Dana was programmed this way.

What about the competition?

Because we had not been completely satisfied with our code structure, we were interested how the competition solves these problems. We managed to take a look into the following projects:

PlayerStage — operating system for robots

This software package offers a variety of abstract interfaces to the most common platforms of mobile robots. It is built upon three principles: (a) interface, (b) device, (c) driver. In general it implements a layer that performs a basic post processing of the input data into a hardware-independent form. For the sake of simpleness, we can compare this system to an operating system.

Several application libraries in different languages can be chosen, but which do not pose fundamental restrictions on the client code's structure. We did not manage to find any extensive example of usage, although the project is formed around a number of (mostly American) scientists who allegedly use that system for the implementation of their algorithms.

CARMEN — collection of modules for all aspects of steering and navigation

Carnegie Mellon Robot Navigation Toolkit consists of several modules which communicate via IPC. IPC is a package that enables the communication between modules by TCP/IP. The communication works with sending messages and enables a publish/subscribe architecture (the module generates data to request another module without knowing it) and also a query/response architecture (the module generates a query and awaits a response). The major usage drawback is probably the need to construct each module as an independent process. That causes unnecessary inconvenience while testing and debugging. As a free reward, we gain an collection of single modules which can run on different computers (with transparent communication).

Tekkotsu — specially for AIBO

Tekkotsu is strongly tied to the robot dog AIBO. Everything needed for its operation is included and it is intended for extending pre-defined classes for specific tasks of algorithms (tutorial, overview). Adding a new functionality is done by reimplementing pre-defined virtual functions in combination with (similarly to IPC) notifications in the form of messages (by registering some handler functions).

Robocode — simulation of a tank battle in Java

Robocode differs from the program packages mentioned above especially by its focus on simulated robots. The basic API which can be used for programming the robot has a synchronous character. More can be seen in an example:

// A basic robot that moves around in a square
public void run() {
    turnLeft(getHeading());
    while(true) {
        ahead(100);
        turnRight(90);
    }
}

Apart from this synchronous API, Robocode offers a so-called AdvancedRobot API which is more low level and lets you peek under the hood of the whole system. The API function ahead(int) can be implemented using the low-level API like the following:

public void ahead(int dist) {
    setAhead(dist);
    while(dist– > 0)
        execute();
    setStop();
    execute();
  }
}

The method execute() works here like our sendReceive(commandStruct, stateStruct) and setAhead(dist) sets the power on the motors. The synchronous API is still our original FSM, but somehow turned inside out.

Lessons learned

What have we learned from our colleagues? It is very important to preprocess the incoming and outgoing data into a hardware-independent form. Although it does not have to be obvious on first sight, altogether it is a demanding task (the Player solves essentially only this problem and also two thirds of Ester's is just this). Consequently, it simplifies the application code and also the interface for a simulation.

Another common denominator of all projects is the pursuit for the code's modularization with biggest possible isolation of the modules, with exactly defined interfaces usually implemented by some form of message passing.

The last (but not the least), the synchronous API of Robocode should be mentioned because it impressed us by its simplicity and directness. Its design was simplified presumably by the fact that it is targeted from the beginning at simulated robots and that it does not have to deal with such "details" as how motors work or how the encoders tick. Consequently it helps in simulating the robot and also makes the control easier.

Attempt to improve no. 2 — Ester

By using the nested FSM model we managed to achieve a certain degree of modularization of our code so that it became cleaner and several people could work on it together. Alas, we did not manage to solve the fundamental problem introduced by the FSM-implemented using switch(state) construction — the fact that we can pass between arbitrary states and the visualization and understanding of these transitions is not easy. Also, it is still the programmers responsibility to properly initialize the different nested FSMs. That in turn leads to unnecessary difficulties while extending the code, but also while only doing maintenance tasks.

On thorough examination of the switch-based FSM, we discover that we are actually using very peculiar way of implementing something, that we programmers deal with every day (as long as you do not program only in Prolog ). An example will help again. How would the implementation of a robot driving in a square looked like using the switch-based implementation?

state = INIT;

while (true)
{
    sendReceive(commandStruct, stateStruct);
    switch(state)
    {
        case INIT:
            if (getAbsHeading() == 0)
            {
                state = STRAIGHT;
                dist = 100;
                break;
            }
            setLeft();
            break;

        case STRAIGHT:
            if (dist == 0)
            {
                state = TURN;
                angle = 90;
                break;
            }
            dist-@-;
            setAhead();
            break;

        case TURN:
            if (angle == 0)
            {
                state = STRAIGHT;
                dist = 100;
                break;
            }
            angle-@-;
            setLeft();
            break;
    }
}

For the sake of simplicity we defined the distance and the angle in the number of steps. The programmed activity can be described like this: First, it turns so its absolute orientation is 0 and then it drives 100 ticks straight and 90 ticks to the left. Already in such a simple example we can see something, that can be fairly easily described, requires a lot of code that in addition to that does not really resemble the original description. On the other hand the algorithm coded using the synchronous Robocode API:

// A basic robot that moves around in a square
public void run() {
    turnLeft(getHeading());
    while(true) {
        ahead(100);
        turnRight(90);
    }
}

The interesting thing is that this code is consequently an FSM as well, only that the compiler helps us extensively so that the code stays readable. Instead of the state variable we have the IP (instruction pointer) here which points to the currently executed instruction and the stack for function calls (that in turn implements the nested FSM and more)

The fundamental advantage of this solution though is that the majority of the things the programmer had to care about is now handled by the compiler. For instance it is automatically guaranteed that we do not enter the initialization sequence (expressed by the state INIT) again while the robot is moving and that at repeated calls to run() it is properly executed (without any work required on the caller's side).

Also the already mentioned simplicity and clearness of the code is worth to emphasize again. It is even more impressive in the situation when it is necessary to implement a complicated algorithm which contains several variously nested cycle with more complicated functions of the type ahead() or turnRight().

The whole control algorithm for Ester at the competition Eurobot 2004 can be described with such a simple code:

virtual int main()
{
    mapPalms();
    while (true)
    {
        try
        {
            BallDetect bd;
            EnemyDetect ed;
            while (true)
            {
                searchBall();
                randomMove();
            }
        }
        catch (BallDetect::done)  { shootBall();  }
        catch (EnemyDetect::done) { randomMove(); }
        catch (std::runtime_error){ randomMove(); }
    }
    return 0;
}

Ester first looks for the position of the palms. She does that simultaneously while driving across the playing field along its longer side, beginning from the start position. She monitors the distance on the right side returned by Sharp infrared distance meter. Afterwards it creates two detectors that register to the system so that they are called in every step and Ester can start searching for the balls. As long as she does not find a ball in her vicinity, she picks a new random target on the playing field and continues searching. These two activities alternate until no ball other ball can be found or Ester hits a rival. The found ball will be shot to the goal while she is evading the opponent robot. In both situations she continues searching for another ball afterwards.

Conclusion

Ester's control system would be unbearably complicated if being implemented by the switch-based FSMs (in the order of several hundred states in several tens of FSMs). Thanks to the usage of classical techniques that every programmer is used to (constructions like

if,
else, while

or function calls) for controlling the robot with a synchronous API that we put in this year, it brought us an impressive increase in working efficiency, which therefore enabled us consequently to work out a reliable control system which, in combination with more reliable hardware, won Ester the 4th rank.

Of course, a current system has its rough edges. As described up to now, a parallel steering of the robot's movement and the control of more periphery would be impossible (in the case of Ester a roller for the balls' manipulation). We solved that by using multiple threads of execution which lead to enormous complications when accessing commonly used resources and synchronization in general.

For the implementation of the outlined API, you have to prepare a lot of support code (especially for the maintenance of multiple threads). Though, in our opinion, such an investment into overcoming the initial difficulties will be highly rewarded later.

If you have questions or remakes – contact us. We will gladly answer.