6、编程基础-控制器编程

控制器编程

下页介绍如何编写控制器代码。尽管最初专注于C，但大多数相关和非语言特定的细节已被翻译成C++、Java、Python和MATLAB。要更深入地了解其他语言中的等效函数/方法，请检查节点和API函数以及C++/Java/Python。

Hello World 例子

c语言
#include <webots/robot.h>
#include <stdio.h>

int main() {
  wb_robot_init();

  while(wb_robot_step(32) != -1)
    printf("Hello World!\n");

  wb_robot_cleanup();
  return 0;
}
c++语言
#include <webots/Robot.hpp>
#include <iostream>

using namespace webots;

int main() {
  Robot *robot = new Robot();

  while (robot->step(32) != -1)
    std::cout << "Hello World!" << std::endl;

  delete robot;
  return 0;
}

此代码将“Hello World！”重复打印到重定向到Webots控制台的标准输出流中。对于所有支持Webots的语言，标准输出和错误流都会自动重定向到Webots控制台。

 Webots C API（应用程序编程接口）由常规的C头文件提供。这些头文件必须使用#include＜webots/xyz.h＞等语句包含，其中xyz以小写形式表示webots节点的名称。与任何常规C代码一样，也可以包含标准的C头文件，例如#include<stdio.h>。在任何其他C API函数调用之前，都需要调用初始化wb_robot_init函数。此函数用于初始化控制器和Webots之间的通信。wb_robot_cleanup函数的作用正好相反：它关闭控制器和Webots之间的通信，以顺利终止控制器。请注意，wb_robot_init和wb_roubot_cleanup函数仅存在于C API中，在其他受支持的编程语言中没有任何等效函数。
通常，最高级别的控制代码被放置在for或while循环中。在该循环中，有一个对wb_robot_step函数的调用。此功能使控制器的数据与模拟器同步。wb_robot_step函数需要存在于每个控制器中，并且必须定期调用，因此它通常被放置在主循环中，如上例所示。值32指定控制步骤的持续时间，即wb_robot_step函数应计算32毫秒的模拟，然后返回。此持续时间指定了模拟时间量，而不是实际（挂钟）时间，因此实际可能需要1毫秒或1分钟的实时时间，具体取决于模拟世界的复杂性。
请注意，在这个“Hello World！”示例中，while循环的退出条件是wb_robot_step函数的返回值。当Webots终止控制器时，此函数确实会返回-1（请参阅控制器终止）。因此，在本例中，只要模拟运行，控制回路就会运行。当循环存在时，不可能与Webots进行进一步的通信，唯一的选择是通过调用wb_robot_cleanup函数向Webots确认关闭通信。

读传感器

既然我们已经了解了如何将消息打印到控制台，我们将了解如何读取机器人的传感器。下一个示例将持续更新并打印DistanceSensor返回的值：

c语言：
#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <stdio.h>

#define TIME_STEP 32

int main() {
  wb_robot_init();

  WbDeviceTag sensor = wb_robot_get_device("my_distance_sensor");
  wb_distance_sensor_enable(sensor, TIME_STEP);

  while (wb_robot_step(TIME_STEP) != -1) {
    const double value = wb_distance_sensor_get_value(sensor);
    printf("Sensor value is %f\n", value);
  }

  wb_robot_cleanup();
  return 0;
}

c++语言
#include <webots/Robot.hpp>
#include <webots/DistanceSensor.hpp>
#include <iostream>

#define TIME_STEP 32

using namespace webots;

int main() {
  Robot *robot = new Robot();

  DistanceSensor *sensor = robot->getDistanceSensor("my_distance_sensor");
  sensor->enable(TIME_STEP);

  while (robot->step(TIME_STEP) != -1) {
    const double value = sensor->getValue();
    std::cout << "Sensor value is: " << value << std::endl;
  }

  delete robot;
  return 0;
}

正如您所注意到的，在使用设备之前，有必要获取相应的设备标签（WbDeviceTag）；这是使用wbrobotget_device函数完成的。WbDeviceTag是一种不透明类型，用于识别控制器代码中的设备。请注意，传递给此函数的字符串，在本例中为“my_dinstance_sensor”，指的是robot描述（“.wbt”或“.proto”文件）中指定的设备名称。如果robot没有指定名称的设备，此函数将返回0。
每个传感器必须启用后才能使用。如果传感器未启用，则返回未定义的值。启用传感器是通过使用相应的wb_*_enable函数实现的，其中星号（*）代表传感器类型。每个wb_*_enable函数都允许指定以毫秒为单位的更新延迟。更新延迟指定传感器数据的两次更新之间的期望间隔。
在通常情况下，更新延迟被选择为类似于控制步骤（TIME_step），因此传感器将在每次wb_robot_step函数调用时更新。例如，如果选择更新延迟为控制步长的两倍，则传感器数据将每两次wb_robot_step函数调用更新一次：这可用于模拟慢速设备。请注意，较大的更新延迟也可以加快模拟速度，尤其是对于像Camera这样的CPU密集型设备。相反，选择小于控制步骤的更新延迟是没有意义的，因为控制器不可能以比控制步骤施加的频率更高的频率处理设备的数据。可以使用相应的wb_*_disable功能随时禁用设备。这可能会提高模拟速度。
传感器值在调用wb_robot_step函数期间更新。对wb_dinstance_sensor_get_value函数的调用将检索最新的值。
请注意，有些设备返回矢量值而不是标量值，例如以下函数：

c语言
const double *wb_gps_get_values(WbDeviceTag tag);
const double *wb_accelerometer_get_values(WbDeviceTag tag);
const double *wb_gyro_get_values(WbDeviceTag tag);
c++语言
const double *webots::GPS::getValues() const;
const double *webots::Accelerometer::getValues() const;
const double *webots::Gyro::getValues() const;

在C和C++中，每个函数都返回一个指向三个双值的指针。指针是函数内部分配的数组的地址。控制器代码不应显式删除这些数组。它们将在必要时自动删除。该数组正好包含三个双值。因此，访问索引2以外的数组是非法的，并且可能使控制器崩溃。最后，请注意，不应修改数组元素，因此指针被声明为const。以下是使用这些函数的正确代码示例：

C语言
const double *values = wb_gps_get_values(gps);

// OK, to read the values they should never be explicitly deleted by the controller code
printf("MY_ROBOT is at position: %g %g %g\n", values[0], values[1], values[2]);

// OK, to copy the values
double x, y, z;
x = values[0];
y = values[1];
z = values[2];
c++语言
const double *values = gps.getValues();

// OK, to read the values they should never be explicitly deleted by the controller code
std::cout << "MY_ROBOT is at position: " << values[0] << ' ' << values[1] << ' ' << values[2] << std::endl;

// OK, to copy the values
double x, y, z;
x = values[0];
y = values[1];
z = values[2];

后续如果要实现控制器再进行详细讨论；