前言

 

现在我们开始解决与编程机器人控制器有关的主题。我们将设计一个简单的控制器，避免前面教程中创建的障碍（箱子）。  

本教程将向您介绍Webots中机器人编程的基础。在本章的最后，您应该了解场景树节点和控制器API之间的链接是什么，如何初始化和清理机器人控制器，如何初始化机器人设备，如何获取传感器值，如何命令执行器，以及如何编写简单的反馈回路。  

本教程仅解决Webots函数的正确用法。机器人算法的研究超出了本教程的目标，因此此处不再赘述。处理本章需要一些基本的编程知识。  

一、创建一个新的世界

 

此处我们不打算从零开始创建一个新环境，将上一篇文章中创建的仿真环境另存为，取名为avoid_collision。我们将在此仿真的基础上修改我们的控制器以实现简单的避障功能。为了更好地体现避障效果，我们多添加几个箱子。  

二、epuck模型

 

控制器编程需要一些与e-puck模型有关的信息。为了编写防撞算法，我们需要读取位于其转塔周围的8个红外距离传感器的值，根据传感器读数驱动两个车轮。在下图中显示了距离传感器的分布。  

距离传感器由机器人层次结构中的8个DistanceSensor节点建模。这些节点由其name字段（从ps0到ps7）引用。以后后我们再解释如何定义这些节点。  

现在，我只需要指导，可以通过Webots 的相关API（定义在webots/distance_sensor.h内），可以访问DistanceSensor节点。  

距离传感器返回的值在0到4096之间缩放（逐段线性变化到距离）。4096表示测量到的光量很大（障碍物很近），0表示没有测量到的光量（没有障碍物）。

  更多有关API函数的文档以及每个节点的说明，可以参考webots的《参考手册》。  

三、避障控制器

 

这一章我们将编写一个控制器，是机器人能够实现简单的避免碰撞的行为。他的简单运行原理如下：机器人前进过程中不断监测8个距离传感器的读数，检测是否存在障碍物，如果有，则转向无障碍物的方向。同样，我们新建一个控制器名为avoid_collision  

  将机器人链接到我们的新控制器  

 

1、加载头文件

 

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

#include <webots/Robot.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>

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

#define TIME_STEP 64

  最后，webots类所需的命名空间。  

using namespace webots;

部分代码在webots新创建控制器时会自动添加，我们可以按需更改

2、main函数部分

 

main函数是控制器程序开始执行的地方。传递给main函数的参数由Robot节点的controllerArgs字段给出。main函数的默认模板如下所示。  

// entry point of the controller
int main(int argc, char **argv) {
  // create the Robot instance.
  Robot *robot = new Robot();
  // initialize devices
  // feedback loop: step simulation until receiving an exit event
  while (robot->step(TIME_STEP) != -1) {
    // read sensors outputs
    // process behavior
    // write actuators inputs
  }
  delete robot;
  return 0; //EXIT_SUCCESS
}

3、添加功能

 

接下来我们往主函数当中添加我们所需要的功能。首先我们初始化距离传感器  

// 初始化设备
DistanceSensor *ps[8];
char psNames[8][4] = {
  "ps0", "ps1", "ps2", "ps3",
  "ps4", "ps5", "ps6", "ps7"
};

// 按照名称以此开启传感器
for (int i = 0; i < 8; i++) {
  ps[i] = robot->getDistanceSensor(psNames[i]);
  ps[i]->enable(TIME_STEP);
}

  初始化电动机：  

// 获取电机设备
Motor *leftMotor = robot->getMotor("left wheel motor");
Motor *rightMotor = robot->getMotor("right wheel motor");

// 设置电机为速度控制模式
leftMotor->setPosition(INFINITY);
rightMotor->setPosition(INFINITY);
leftMotor->setVelocity(0.0);
rightMotor->setVelocity(0.0);

  在while主循环中，即在注释// read sensors outputs之后，添加以下代码，读取距离传感器值，如下所示：  

// read sensors outputs
double psValues[8];
for (int i = 0; i < 8 ; i++)
  psValues[i] = ps[i]->getValue();

  在主循环中，在// process behavior后添加以下代码，检测是否发生碰撞（即，距离传感器返回的值大于阈值），如下所示：  

// detect obstacles
bool right_obstacle =
  psValues[0] > 80.0 ||
  psValues[1] > 80.0 ||
  psValues[2] > 80.0;
bool left_obstacle =
  psValues[5] > 80.0 ||
  psValues[6] > 80.0 ||
  psValues[7] > 80.0;

  最后，根据障碍物的信息来驱动车轮，如下所示：  

#define MAX_SPEED 6.28
...
// initialize motor speeds at 50% of MAX_SPEED.
double leftSpeed  = 0.5 * MAX_SPEED;
double rightSpeed = 0.5 * MAX_SPEED;
// modify speeds according to obstacles
if (left_obstacle) {
  // turn right
  leftSpeed  = 0.5 * MAX_SPEED;
  rightSpeed = -0.5 * MAX_SPEED;
}
else if (right_obstacle) {
  // turn left
  leftSpeed  = -0.5 * MAX_SPEED;
  rightSpeed = 0.5 * MAX_SPEED;
}
// write actuators inputs
leftMotor->setVelocity(leftSpeed);
rightMotor->setVelocity(rightSpeed);

  添加完相关功能之后我们编译代码  

4、完整代码

 

#include <webots/Robot.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>

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

#define MAX_SPEED 6.28

// All the webots classes are defined in the "webots" namespace
using namespace webots;

// entry point of the controller
int main(int argc, char **argv) {
  // create the Robot instance.
  Robot *robot = new Robot();

  // initialize devices
  DistanceSensor *ps[8];
  char psNames[8][4] = {
    "ps0", "ps1", "ps2", "ps3",
    "ps4", "ps5", "ps6", "ps7"
  };

  for (int i = 0; i < 8; i++) {
    ps[i] = robot->getDistanceSensor(psNames[i]);
    ps[i]->enable(TIME_STEP);
  }

  Motor *leftMotor = robot->getMotor("left wheel motor");
  Motor *rightMotor = robot->getMotor("right wheel motor");
  leftMotor->setPosition(INFINITY);
  rightMotor->setPosition(INFINITY);
  leftMotor->setVelocity(0.0);
  rightMotor->setVelocity(0.0);

  // feedback loop: step simulation until an exit event is received
  while (robot->step(TIME_STEP) != -1) {
    // read sensors outputs
    double psValues[8];
    for (int i = 0; i < 8 ; i++)
      psValues[i] = ps[i]->getValue();

    // detect obstacles
    bool right_obstacle =
      psValues[0] > 80.0 ||
      psValues[1] > 80.0 ||
      psValues[2] > 80.0;
    bool left_obstacle =
      psValues[5] > 80.0 ||
      psValues[6] > 80.0 ||
      psValues[7] > 80.0;

    // initialize motor speeds at 50% of MAX_SPEED.
    double leftSpeed  = 0.7 * MAX_SPEED;
    double rightSpeed = 0.7 * MAX_SPEED;
    // modify speeds according to obstacles
    if (left_obstacle) {
      // turn right
      leftSpeed  = 0.7 * MAX_SPEED;
      rightSpeed = -0.7 * MAX_SPEED;
    }
    else if (right_obstacle) {
      // turn left
      leftSpeed  = -0.7 * MAX_SPEED;
      rightSpeed = 0.7 * MAX_SPEED;
    }
    // write actuators inputs
    leftMotor->setVelocity(leftSpeed);
    rightMotor->setVelocity(rightSpeed);
  }

  delete robot;
  return 0; //EXIT_SUCCESS
}

保存世界，并重置仿真之后我们可以看到以下运行效果