1 新建控制器

这里我们为上一篇博客中创建的小车模型建立一个控制器，让小车在环境中避障运行。首先在 Wizards->New Robot Controller 菜单栏新建一个机器人控制器，保存为 my_controller 。这里选择使用C语言进行新建，这时候会再右侧栏出现机器人控制器的编程界面。填入以下内容：  

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

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

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

  // internal variables
  int i;
  int avoid_obstacle_counter = 0;

  // initialise distance sensors
  WbDeviceTag ds[2];
  char ds_names[2][10] = {"ds_left", "ds_right"};
  for (i = 0; i < 2; i++) {
    ds[i] = wb_robot_get_device(ds_names[i]);
    wb_distance_sensor_enable(ds[i], TIME_STEP);
  }

  // initialise motors
  WbDeviceTag wheels[4];
  char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
  for (i = 0; i < 4; i++) {
    wheels[i] = wb_robot_get_device(wheels_names[i]);
    wb_motor_set_position(wheels[i], INFINITY);
  }

  // feedback loop
  while (wb_robot_step(TIME_STEP) != -1) {
    // init speeds
    double left_speed = 1.0;
    double right_speed = 1.0;

    if (avoid_obstacle_counter > 0) {
      avoid_obstacle_counter--;
      left_speed = 1.0;
      right_speed = -1.0;
    } else {
      // read sensors outputs
      double ds_values[2];
      for (i = 0; i < 2; i++)
        ds_values[i] = wb_distance_sensor_get_value(ds[i]);

      // increase counter in case of obstacle
      if (ds_values[0] < 950.0 || ds_values[1] < 950.0)
        avoid_obstacle_counter = 100;
    }

    // write actuators inputs
    wb_motor_set_velocity(wheels[0], left_speed);
    wb_motor_set_velocity(wheels[1], right_speed);
    wb_motor_set_velocity(wheels[2], left_speed);
    wb_motor_set_velocity(wheels[3], right_speed);
  }

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

    新建完控制器以后，对控制器进行编译  
   

2 控制器说明

首先该控制器设计到的执行器只有四个电机、传感器有两个距离探测器，我们的目标是让小车实现避障（左边有障碍物就往右边走，反之亦然）。具体操作四个轮子是通过轮子的名称来实现的，这里的 “wheel1” 就是在上篇博客中自定义的电机的名称，  

  // initialise motors
  WbDeviceTag wheels[4];
  char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
  for (i = 0; i < 4; i++) {
    wheels[i] = wb_robot_get_device(wheels_names[i]);
    wb_motor_set_position(wheels[i], INFINITY);
  }

    最后通过函数设置电机的速度  

<code class="prism language-c has-numbering"> <span class="token function">wb_motor_set_velocity</span><span class="token punctuation">(</span>wheels<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">,</span> left_speed<span class="token punctuation">)</span><span class="token punctuation">;</span>
</code>

  同样距离传感器的操作也是通过名称 “ds_left”和 “ds_right”实现的 创建操作句柄：  

<code class="prism language-c has-numbering">  WbDeviceTag ds<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">;</span>
  <span class="token keyword">char</span> ds_names<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">[</span><span class="token number">10</span><span class="token punctuation">]</span> <span class="token operator">=</span> <span class="token punctuation">{</span><span class="token string">"ds_left"</span><span class="token punctuation">,</span> <span class="token string">"ds_right"</span><span class="token punctuation">}</span><span class="token punctuation">;</span>
  <span class="token keyword">for</span> <span class="token punctuation">(</span>i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> <span class="token number">2</span><span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token punctuation">{</span>
    ds<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">=</span> <span class="token function">wb_robot_get_device</span><span class="token punctuation">(</span>ds_names<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    <span class="token function">wb_distance_sensor_enable</span><span class="token punctuation">(</span>ds<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> TIME_STEP<span class="token punctuation">)</span><span class="token punctuation">;</span>
  <span class="token punctuation">}</span>
</code>

  读取距离传感器的值：  

<code class="prism language-c has-numbering"><span class="token keyword">double</span> ds_values<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">;</span>
      <span class="token keyword">for</span> <span class="token punctuation">(</span>i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> <span class="token number">2</span><span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span>
        ds_values<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">=</span> <span class="token function">wb_distance_sensor_get_value</span><span class="token punctuation">(</span>ds<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
</code>

3 运行效果

最终生成的模型文件可在此处下载,下图是小车在环境中避障运行的效果图：  
 

参考资料

[1] https://www.cyberbotics.com/doc/reference/motion [2] https://cyberbotics.com/doc/guide/tutorial-4-more-about-controllers?tab-language=c 如果大家觉得文章对你有所帮助，麻烦大家帮忙点个赞。O(∩_∩)O