9、教程-3控制器的更多方面

现在我们开始讨论与机器人控制器编程相关的主题。我们将设计一个简单的控制器，以避免在前面的教程中创建的障碍。
本教程将向您介绍Webots中机器人编程的基础知识。在本章末尾，您应该了解场景树节点和控制器API之间的链接是什么，机器人控制器必须如何初始化和清理，如何初始化机器人设备，如何获取传感器值，如何命令执行器，以及如何编程简单的反馈环。
本教程仅介绍Webots函数的正确用法。机器人算法的研究超出了本教程的目标，因此此处不再赘述。处理本章需要一些基本的编程知识（任何C教程都应该是足够的介绍）。在本章的最后，给出了进一步的机器人算法的链接。

将之前的.wbt文件保存为新的文件epuck_avoid_collision.wbt

理解e-puck 模型

控制器编程需要一些与电子吸盘模型相关的信息。为了创建防撞算法，我们需要读取其炮塔周围的8个红外距离传感器的值，并启动其两轮。距离传感器分布在转台周围的方式和电子吸盘方向如图所示。
距离传感器由机器人层次结构中的8个DistanceSensor节点建模。这些节点由它们的名称字段（从ps0到ps7）引用。我们稍后将解释如何定义这些节点。现在，只需注意，可以通过Webots API的相关模块（通过Webots/distance_sensor.h包含文件）访问DistanceSensor节点。距离传感器返回的值在0和4096之间按比例缩放（与距离分段线性）。4096表示测量到大量的光（障碍物很近），0表示没有测量到光（没有障碍物）。
控制器API是一个编程接口，可以让您访问机器人的模拟传感器和执行器。例如，包含webots/distance_sensor.h文件允许使用wb_dinstance_sensor_*函数，并且使用这些函数可以查询DistanceSensor节点的值。有关API函数的文档可以在参考手册中找到，并附带每个节点的说明。

 编程一个控制器

我们想编程一个非常简单的防撞行为。您将对机器人进行编程，使其向前移动，直到前方距离传感器检测到障碍物，然后转向无障碍方向。为了做到这一点，我们将使用图中UML状态机中描述的简单反馈循环。
该控制器的完整代码将在下一小节中给出。

在控制器文件的开头，添加与Robot、DistanceSensor和Motor节点相对应的include指令，以便能够使用相应的API：

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

在include语句之后添加一个宏，定义每个物理步骤的持续时间。此宏将用作wb_robot_step函数的参数，也将用于启用设备。此持续时间以毫秒为单位指定，并且必须是WorldInfo节点的basicTimeStep字段中值的倍数。

#define TIME_STEP 64

主要功能是控制器程序开始执行的地方。传递给主函数的参数由Robot节点的controllerArgs字段给定。Webots API必须使用wb_robot_init函数进行初始化，并且必须使用wp_robot_cleanup函数进行清理。

编写主要功能的原型如下：

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

机器人设备由WbDeviceTag引用。WbDeviceTag由wb_robot_get_device函数检索。然后，它被用作与该设备有关的每个函数调用的第一个参数。使用前必须启用DistanceSensor等传感器。启用函数的第二个参数定义了刷新传感器的速率。

注释//初始化设备后，按如下方式获取并启用距离传感器：

// 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);
}

设备初始化后，对电机进行初始化：

WbDeviceTag left_motor = wb_robot_get_device("left wheel motor");
WbDeviceTag right_motor = wb_robot_get_device("right wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_position(right_motor, INFINITY);
wb_motor_set_velocity(left_motor, 0.0);
wb_motor_set_velocity(right_motor, 0.0);

在主循环中，就在注释//读取传感器输出之后，读取距离传感器值，如下所示

// 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 right_obstacle =
  ps_values[0] > 80.0 ||
  ps_values[1] > 80.0 ||
  ps_values[2] > 80.0;
bool left_obstacle =
  ps_values[5] > 80.0 ||
  ps_values[6] > 80.0 ||
  ps_values[7] > 80.0;

最后，使用有关障碍物的信息按如下方式启动车轮：

#define MAX_SPEED 6.28
...
// initialize motor speeds at 50% of MAX_SPEED.
double left_speed  = 0.5 * MAX_SPEED;
double right_speed = 0.5 * MAX_SPEED;
// modify speeds according to obstacles
if (left_obstacle) {
  // turn right
  left_speed  = 0.5 * MAX_SPEED;
  right_speed = -0.5 * MAX_SPEED;
}
else if (right_obstacle) {
  // turn left
  left_speed  = -0.5 * MAX_SPEED;
  right_speed = 0.5 * MAX_SPEED;
}
// write actuators inputs
wb_motor_set_velocity(left_motor, left_speed);
wb_motor_set_velocity(right_motor, right_speed);

通过选择Build/Build菜单项编译代码。编译错误在控制台中显示为红色。如果有，请修复它们，然后重试编译。重新加载世界。

完整代码如下：

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

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

#define MAX_SPEED 6.28

// 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);
  }

  WbDeviceTag left_motor = wb_robot_get_device("left wheel motor");
  WbDeviceTag right_motor = wb_robot_get_device("right wheel motor");
  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_position(right_motor, INFINITY);
  wb_motor_set_velocity(left_motor, 0.0);
  wb_motor_set_velocity(right_motor, 0.0);

  // feedback loop: step simulation until an exit event is received
  while (wb_robot_step(TIME_STEP) != -1) {
    // 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 right_obstacle =
      ps_values[0] > 80.0 ||
      ps_values[1] > 80.0 ||
      ps_values[2] > 80.0;
    bool left_obstacle =
      ps_values[5] > 80.0 ||
      ps_values[6] > 80.0 ||
      ps_values[7] > 80.0;

    // initialize motor speeds at 50% of MAX_SPEED.
    double left_speed  = 0.5 * MAX_SPEED;
    double right_speed = 0.5 * MAX_SPEED;

    // modify speeds according to obstacles
    if (left_obstacle) {
      // turn right
      left_speed  = 0.5 * MAX_SPEED;
      right_speed = -0.5 * MAX_SPEED;
    }
    else if (right_obstacle) {
      // turn left
      left_speed  = -0.5 * MAX_SPEED;
      right_speed = 0.5 * MAX_SPEED;
    }

    // write actuators inputs
    wb_motor_set_velocity(left_motor, left_speed);
    wb_motor_set_velocity(right_motor, right_speed);
  }

  // cleanup the Webots API
  wb_robot_cleanup();
  return 0; //EXIT_SUCCESS
}