Khepera III Toolbox/Programming Hints

Writing programs for the Khepera III robot is pretty straightforward. Nevertheless, a good program structure and doing things right the first time will save you time - a lot of time! In this chapter, we therefore discuss a couple of design choices when implementing such programs.

General Structure of a Program

A Simple Program with One State

A simple program will have the following structure:

// Program initialization
commandline_init();
...
commandline_parse(argc, argv);
...
khepera3_init();

// Algorithm initialization
algorithm.configuration.wall_threshold = 1000;
...
khepera3_drive_start();

// Main loop
while (true) {
    // Read sensors
    khepera3_infrared_proximity();
    ...

    // Calculate actuator response
    speed_left = ...
    speed_right = ...

    // Termination condition
    if (...) {
        break;
    }

    // Set actuators
    khepera3_drive_set_speed(speed_left, speed_right);
    ...

    // Sleep for a while
    usleep(algorithm.configuration.wait_us);
}

// Algorithm termination/cleanup if necessary
khepera3_drive_set_speed(0, 0)

Most programs consist of a main loop which is executed after an initialization phase. The main loop implements the so-called perception-to-action loop - the loop which reads the sensor values, derives the action to take, and sets the actuators (motors) accordingly. The main loop sometimes also contains a termination condition which quits the program as soon as the goal has been reached.

If you are looking at the code of existing programs (e.g. motion_followline), you will notice that the above structure implemented in different functions. Typically, the structure is broken up into the following functions

help: Prints a help text.
algorithm_init: Initializes the algorithm.
algorithm_run: Executes the main loop.
main: Initializes the program and calls the above functions.

In addition, a structure holding the configuration and the state (if necessary) of the algorithm is defined:

// Algorithm configuration and state variables
struct sAlgorithm {
    struct {
        int wall_threshold;
        ...
        int verbosity;
    } configuration;
    struct {
        int remaining_targets;
        ...
    } state;
};

// Declare an instance of that structure
struct sAlgorithm algorithm;

// Access to the variables
algorithm.configuration.wall_threshold = ...
algorithm.state.remaining_targets = ...

Even if this looks complicated at first sight, using this nested structure has two advantages. First of all, all algorithm variables are declared in one place. If you want to reuse that algorithm somewhere else, you see at a glance which variables are needed. Second, the classification into configuration variables and state variables will help you implementing the algorithm in a clean way. Configuration variables will only be written during the initialization phase and should only be read in the main loop. State variables, however, are read and written in the main loop.

Programs with Multiple States

Often, programs have more than one state. A robot engaged in a search task, for instance, will maybe have an exploration state in which it walks randomly around, and a gradient following state in which it tries to move towards the target.

The simplest way to implement that is with different main loops, i.e. one main loop per state. A code skeleton for that looks as follows (note that the template program created by the k3-create-program script uses exactly this skeleton):

// Algorithm variables
struct sAlgorithm {
    ...
    struct {
        void (*hook)(); // Pointer to the current state function
        ...
    } state;
};

// Forward declaration of states
void state_exploration();
void state_gradient_follow();
void state_success();

// Algorithm initialization
void algorithm_init() {
    ...

    // Set the initial state
    algorithm.state.hook = state_exploration;
}

// The algorithm just switches from state to state by calling the current state function
void algorithm_run() {
    while (1) {
        algorithm.state.hook();
    }
}

void state_exploration() {
    // State initialization
    ...

    // Main loop
    while (1) {
        // Read sensors
        ...
    
        // State change condition
        if (...) {
            algorithm.state.hook = state_gradient_follow;  // Switch to the gradient follow state
            return;
        }

        // Set actuators
        ...

        // Sleep for a while
        usleep(algorithm.configuration.wait_us);
    }
}

void state_gradient_follow() {
    // State initialization
    ...

    // Main loop
    while (1) {
        // Read sensors
        ...
    
        // State change condition
        if (...) {
            algorithm.state.hook = state_exploration;  // Switch to the exploration state
            return;
        }

        // Termination condition
        if (...) {
            algorithm.state.hook = state_success;  // Switch to the success state, which terminates the program
            return;
        }

        // Set actuators
        ...

        // Sleep for a while
        usleep(algorithm.configuration.wait_us);
    }
}

void state_success() {
    // We are done
    exit(0);
}

Do not fear the function pointer (algorithm.state.hook)! Its declaration may look complicated, but it is very easy to use, as you can see in the state functions above. Adding, moving or removing states becomes extremely easy, as you can treat a state as one contiguous block of code. In case the robot does not behave as desired in one particular state, you can modify its code without hurting other states (which is typically the case if you put everything in one main loop with a series of if's).

Moreover, if you draw your algorithm as a state machine on a paper first, you can implement the code right away following exactly the schema on your paper. Each state will correspond to one function, and each arrow to one state change condition.

Stability and Execution Speed

From your control theory course, you probably know that the perception-to-action loop has to run at a certain minimum frequency such that the stability criterion is fulfilled. On the other hand, running that loop at higher speed also consumes more energy.

The speed of the perception-to-action loop shown in the code examples above is determined by three factors:

The communication overhead, i.e. the time required to read the sensors and set the actuators.
The processing overhead, i.e. the time required to derive the action from the current perception.
The waiting time (usleep(algorithm.configuration.wait_us)), which can be adjusted at will.

The communication overhead can only be optimized up to some point. There is a hard limit on the number of bytes you need to transfer between the robot and the Korebot board (processor board) to get a certain piece of information, and the communication channel (I2C bus) has a fixed speed of roughly 10 KB/s. Hence, a call to the khepera3_infrared_proximity function to obtain infrared proximity sensor values will return after about 3.1 ms (if the I2C bus is free), as it has to transfer 31 bytes from and to the microcontroller. The number of bytes transferred by each function can be found in the .h files of the khepera3 module.

Similarly, a certain amount of operations will be necessary for deriving the action given the sensor readings and the current state. Basic mathematical operations are fast as compared to the communication overhead, but making extensive use of log, exp or trigonometric functions will slow down the main loop. In addition, everything that has to be written to, or read from disk will involve substantial overhead as well. Last but not least, the printf can severly slow down your program depending on where the output is redirected.

While the communication overhead can be accurately calculated, the processing overhead needs to be measured in the environment the program is running (e.g. with stdout redirected to where it will be redirected for the real experiments). The waiting time can then be adjusted to achieve the desired perception-to-action loop frequency.

To Thread or Not To Thread

Whether to use threads or not is a huge debate in computer science. The Khepera III Toolbox does not force you to use or not use threads, but it has been written with the idea in mind that most programs won't be multithreaded.

If you are using the code skeleton provided above, you will naturally not use threads (or a single thread, if you want). This makes sense, considering that there is not much to parallelize: the robot can only be in one state at a time, and the inside of a perception-to-action loop has to be executed in a precise order.

Asynchronous IO and the select Syscall

When writing programs that are waiting for some data to arrive on open filehandles, the select syscall will be useful. This syscall takes a list of filehandles, and returns if either data is available on one of them, or a timeout has occurred. The select syscall is the solution to most problems where programmers think they need threads.

As an example for this, have a look at the motion_arrowkeys program, which waits (in an non-blocking fashion) for user input on the standard input and executes the main loop at the same time.

Using Threads anyway

If - for one reason or another - you need go for multiple threads, some care has to be taken when accessing the robot sensors and actuators. Basically, you are not allowed to access the khepera3 module or the i2cal module in a concurrent way, as these modules use statically allocated variables and are therefore not thread-safe. There are two solutions to that:

Design your program in such a way that only the main thread (or another dedicated thread) is accessing these modules. This is straightforward in some cases, but can get extremely complicated in other situations.
Synchronize all calls to the khepera3 module and all i2cal transactions. They need to be synchronized with the same mutex, as the khepera3 module uses the i2cal module to communicate with the sensors, i.e. a most khepera3 functions are transactions on the I2C bus.

In case you wonder: the current implementation is intentionally not thread-safe, since that would make the function calls more complicated and more error-prone for single-threaded applications. In the future, however, a thread-safe implementation may be added (e.g. as a separate module).

Splitting Up Algorithms into Independent Parts

Just as we break down the implementation of algorithms into functions that implement one state each, we could also split them up into different programs that implement a subset of the states each, and execute them the right order. This works, and is actually a very good technique in many situations!

The Maze Traversal Example

Assume you want to test different algorithms to traverse a maze. The maze entrance is on the left, and the exit on the right side of your arena. Since you want to run the experiment several times, you want your robot to automatically go back to the entrance once it has reached the exit, e.g. by following a line on the ground. You could of course add a new state (state_go_back_to_entrance) to all your algorithms, and switch to that state when the exit has been reached. However, you could also implement this as a separate program (maze_go_back_to_entrance) which you call immediately after any of your maze traversing programs.

Overhead and Speed Considerations

There is some processing overhead in starting and quitting programs, but from an operating system point of view, this overhead is small. You should be careful about your initialization phase, though. For instance, if you have to load big files (e.g. maps, ...) at program start, this may introduce some significant delay. In addition, if you launch your programs remotely over a WLAN connection, there is a small transmission delay (~ 10 ms) to take into account.

Splitting Rules

There is no definite answer on when to split and how much to implement in the same program. However, here are a couple of thoughts:

Do split if this helps you testing and debugging individual parts of your algorithm.
Do split if this avoids having exactly the same pieces of code in several programs (as in the maze traversal example above).
Do split if you think you can reuse at least one part somewhere else.
Do split if the two parts have nothing in common.
Do not split if the transition is complex (e.g. multiple target states possible)
Do not split if the precise timings or low delays are an issue.
Do not split if the two parts share state variables or a lot of data.

In addition, it is usually preferred to implement an algorithm to run exactly once, and then quit. If want to test your algorithm in multiple runs, simply launch that program multiple times (with a script, for example) and redirect the output to a different file each time. This approach is much more flexible and robust.

Code Examples

The measure_real_speed example uses a script to run the same program multiple times with different parameters.