Webots User Guide - chapter 7 - section 5

Webots User Guide


7.5 Tutorial 4: More about Controllers (20 minutes)

Now we start to tackle the topics related to programming robot controllers. We will design a simple controller that avoids the obstacles created in the previous tutorials.

This tutorial will introduce you to the basics of robot programming in Webots. At the end of this chapter, you should understand what is the link between the scene tree nodes and the controller API, how the robot controller has to be initialized and cleaned up, how to initialize the robot devices, how to get the sensor values, how to command the actuators, and how to program a simple feedback loop.

This chapter only addresses the correct usage of Webots functions. The study of robotics algorithms is beyond the goals of this tutorial and so this won't be adressed here. Some rudimentary programming knowledge is required to tackle this chapter (any C tutorial should be a sufficient introduction). At the end of the chapter, links to further robotics algorithmics are given.

7.5.1 New World and new Controller

Save the previous world as collision_avoidance.wbt.
Create a new C controller called epuck_collision_avoidance using the wizard. Modify the controller field of the EPuck node in order to link it to the new controller.

7.5.2 Understand the e-puck Model

Controller programming requires some information related to the e-puck model. For doing the collision avoidance algorithm, we need to read the values of its 8 infra-red distance sensors located around its turret, and we need to actuate its two wheels. The way that the distance sensors are distributed around the turret and the e-puck direction are depicted in figure 7.9.

The distance sensors are modeled by 8 DistanceSensor nodes in the hierarchy of the robot. These nodes are referenced by their name fields (from "ps0" to "ps7"). We will explain later how these nodes are defined. For now, simply note that a DistanceSensor node can be accessed through the related module of the Webots API (through the webots/distance_sensor.h include file). The values returned by the distance sensors are scaled between 0 and 4096 (piecewise linearly to the distance), while 4096 means that a big amount of light is measured (an obstable is close) and 0 means that no light is measured (no obstacle).

In the same way, the e-puck root node is a DifferentialWheel node and can be access by the webots/differential_wheel.h include file. The speed is given in a number of ticks/seconds where 1000 ticks correspond to a complete rotation of the wheel. The values are clamped between -1000 and 1000.

The controller API is the programming interface that gives you access to the simulated sensors and actuators of the robot. For example, including the webots/distance_sensor.h file allows to use the wb_distance_sensor_*() functions and with these functions you can query the values of the DistanceSensor nodes. The documentation on the API functions can be found in Chapter 3 of the Reference Manual together with the description of each node.
tutorial_e-puck_top_view

Figure 7.9: Top view of the e-puck model. The green arrow indicates the front of the robot. The red lines represent the directions of the infrared distance sensors. The string labels corresponds to the distance sensor names.

tutorial_feedback_loop

Figure 7.10: UML state machine of a simple feedback loop

7.5.3 Program a Controller

We would like to program a very simple collision avoidance behavior. You will program the robot to go forwards until an obstacle is detected by the front distance sensors, and then to turn towards the obstacle-free direction. For doing that, we will use the simple feedback loop depicted in the UML state machine in figure 7.10.

The complete code of this controller is given in the next subsection.

At the beginning of the controller file, add the include directives corresponding to the Robot, the DifferentialWheels and the DistanceSensor nodes in order to be able to use the corresponding API (documented in chapter 3 of the Reference Manual):

#include <webots/robot.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>

Just after the include statements add a macro that defines the duration of each physics step. This macro will be used as argument to the wb_robot_step() function, and it will also be used to enable the devices. This duration is specified in milliseconds and it must be a multiple of the value in the basicTimeStep field of the WorldInfo node.

#define TIME_STEP 64

The function called main() is where the controller program starts execution. The arguments passed to main() are given by the controllerArgs field of the Robot node. The Webots API has to be initialized using the wb_robot_init() function and it has to be cleaned up using the wb_robot_cleanup() function.
Write the prototype of the main() function as follows:

// entry point of the controller
int main(int argc, char **argv)
{
  // initialize the Webots API
  wb_robot_init();
  // initialize devices
  // feedback loop
  while (1) {
    // step simulation
    int delay = wb_robot_step(TIME_STEP);
    if (delay == -1) // exit event from webots
      break;
    // read sensors outputs
    // process behavior
    // write actuators inputs
  }
  // cleanup the Webots API
  wb_robot_cleanup();
  return 0; //EXIT_SUCCESS
}

A robot device is referenced by a WbDeviceTag. The WbDeviceTag is retrieved by the wb_robot_get_device() function. Then it is used as first argument in every function call concerning this device. A sensor such as the DistanceSensor has to be enabled before use. The second argument of the enable function defines at which rate the sensor will be refreshed.
Just after the comment "// initialize devices", get and enable the distance sensors as follows:

// initialize devices
int i;
WbDeviceTag ps[8];
char ps_names[8][4] = {
  "ps0", "ps1", "ps2", "ps3",
  "ps4", "ps5", "ps6", "ps7"
};

for (i=0; i<8; i++) {
  ps[i] = wb_robot_get_device(ps_names[i]);
  wb_distance_sensor_enable(ps[i], TIME_STEP);
}

In the main loop, just after the comment "// read sensors outputs", read the distance sensor values as follows:

// read sensors outputs
double ps_values[8];
for (i=0; i<8 ; i++)
  ps_values[i] = wb_distance_sensor_get_value(ps[i]);

In the main loop, just after the comment "// process behavior", detect if a collision occurs (i.e. the value returned by a distance sensor is bigger than a threshold) as follows:

// detect obsctacles
bool left_obstacle =
  ps_values[0] > 100.0 ||
  ps_values[1] > 100.0 ||
  ps_values[2] > 100.0;
bool right_obstacle =
  ps_values[5] > 100.0 ||
  ps_values[6] > 100.0 ||
  ps_values[7] > 100.0;

Finally, use the information about the obstacle to actuate the wheels as follows:

// init speeds
double left_speed  = 500;
double right_speed = 500;
// modify speeds according to obstacles
if (left_obstacle) {
  // turn right
  left_speed  -= 500;
  right_speed += 500;
}
else if (right_obstacle) {
  // turn left
  left_speed  += 500;
  right_speed -= 500;
}
// write actuators inputs
wb_differential_wheels_set_speed(left_speed, right_speed);

Compile your code by selecting the Build > Build menu item. Compilation errors are displayed in red in the console. If there are any, fix them and retry to compile. Revert the simulation.

7.5.4 The Controller Code

Here is the complete code of the controller detailed in the previous subsection.

#include <webots/robot.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>

// time in [ms] of a simulation step
#define TIME_STEP 64

// entry point of the controller
int main(int argc, char **argv)
{
  // initialize the Webots API
  wb_robot_init();

  // internal variables
  int i;
  WbDeviceTag ps[8];
  char ps_names[8][4] = {
    "ps0", "ps1", "ps2", "ps3",
    "ps4", "ps5", "ps6", "ps7"
  };
  
  // initialize devices
  for (i=0; i<8 ; i++) {
    ps[i] = wb_robot_get_device(ps_names[i]);
    wb_distance_sensor_enable(ps[i], TIME_STEP);
  }
  
  // feedback loop
  while (1) { 
    // step simulation
    int delay = wb_robot_step(TIME_STEP);
    if (delay == -1) // exit event from webots
      break;
      
    // read sensors outputs
    double ps_values[8];
    for (i=0; i<8 ; i++)
      ps_values[i] = wb_distance_sensor_get_value(ps[i]);
    
    // detect obstacles
    bool left_obstacle =
      ps_values[0] > 100.0 ||
      ps_values[1] > 100.0 ||
      ps_values[2] > 100.0;
    bool right_obstacle =
      ps_values[5] > 100.0 ||
      ps_values[6] > 100.0 ||
      ps_values[7] > 100.0;

    // init speeds
    double left_speed  = 500;
    double right_speed = 500;
    
    // modify speeds according to obstacles
    if (left_obstacle) {
      left_speed  -= 500;
      right_speed += 500;
    }
    else if (right_obstacle) {
      left_speed  += 500;
      right_speed -= 500;
    }
    
    // write actuators inputs
    wb_differential_wheels_set_speed(left_speed, right_speed);
  }
  
  // cleanup the Webots API
  wb_robot_cleanup();
  return 0; //EXIT_SUCCESS
}

7.5.5 Conclusion

Here is a quick summary of the key points you need to understand before going on:

The section 6.1 explains in more detail controller programming. We invite you to read carefully this section before going on.

release 7.0.2
Copyright © 2012 Cyberbotics Ltd. All right reserved.