E-PUCK2-webots下的仿真实现

Webots是专业的移动机器人仿真软件包。它提供了快速的原型制作环境，使用户可以创建具有物理特性（例如质量，关节，摩擦系数等）的3D虚拟世界。用户可以添加简单的被动对象或称为移动机器人的主动对象。这些机器人可以具有不同的移动方案（轮式机器人，有腿机器人或飞行机器人）。此外，它们可能配备有许多传感器和执行器设备，例如距离传感器，驱动轮，摄像机，马达，触摸传感器，发射器，接收器等。最后，用户可以对每个机器人进行单独编程以表现出所需的行为。Webots包含大量机器人模型和控制器程序示例，以帮助用户入门。

Webots还包含许多与真实移动机器人的接口，因此，一旦您模拟的机器人表现出预期的行为，您就可以将其控制程序转移到诸如e-puck，DARwIn-OP，Nao等真实机器人上。可以添加新接口通过相关系统。

什么是世界？

Webots中的世界是对机器人及其环境的属性的3D描述。它包含对每个对象的描述：位置，方向，几何形状，外观（如颜色或亮度），物理属性，对象类型等。世界组织为层次结构，其中对象可以包含其他对象（例如VRML97）。例如，一个机器人可以包含两个轮子，一个距离传感器和一个关节，该关节本身包含一个摄像头等。世界文件不包含机器人的控制器代码；它仅指定每个机器人所需的控制器名称。世界保存在“ .wbt”文件中。“ .wbt”文件存储在每个Webots项目的“ worlds”子目录中。

什么是控制器？

控制器是控制世界文件中指定的机器人的计算机程序。可以使用Webots支持的任何编程语言编写控制器：C，C ++，Java，Python或MATLAB。当模拟开始时，Webots将启动指定的控制器，每个控制器都是一个单独的进程，并将控制器进程与模拟的机器人相关联。请注意，多个机器人可以使用相同的控制器代码，但是将为每个机器人启动一个不同的过程。

一些编程语言需要编译（C和C ++），其他语言需要解释（Python和MATLAB），而另一些则需要同时进行编译和解释（Java）。例如，C和C ++控制器被编译为平台相关的二进制可执行文件（例如Windows下的“ .exe”）。Python和MATLAB控制器由相应的运行时系统（必须安装）解释。Java控制器需要编译为字节码（“ .class”文件或“ .jar”），然后由Java虚拟机进行解释。

每个控制器的源文件和二进制文件一起存储在控制器目录中。控制器目录放置在每个Webots项目的“ controllers”子目录中。

/*
 * Copyright 1996-2019 Cyberbotics Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#define WHEEL_RADIUS 0.02
#define AXLE_LENGTH 0.052
#define RANGE (1024 / 2)

static void compute_odometry(WbDeviceTag left_position_sensor, WbDeviceTag right_position_sensor) {
  double l = wb_position_sensor_get_value(left_position_sensor);
  double r = wb_position_sensor_get_value(right_position_sensor);
  double dl = l * WHEEL_RADIUS;         // distance covered by left wheel in meter
  double dr = r * WHEEL_RADIUS;         // distance covered by right wheel in meter
  double da = (dr - dl) / AXLE_LENGTH;  // delta orientation
  printf("estimated distance covered by left wheel: %g m.\n", dl);
  printf("estimated distance covered by right wheel: %g m.\n", dr);
  printf("estimated change of orientation: %g rad.\n", da);
}

int main(int argc, char *argv[]) {
  /* define variables */
  WbDeviceTag distance_sensor[8], left_motor, right_motor, left_position_sensor, right_position_sensor;
  int i, j;
  double speed[2];
  double sensors_value[8];
  double braitenberg_coefficients[8][2] = {{0.942, -0.22}, {0.63, -0.1}, {0.5, -0.06},  {-0.06, -0.06},
                                           {-0.06, -0.06}, {-0.06, 0.5}, {-0.19, 0.63}, {-0.13, 0.942}};
  int time_step;
  int camera_time_step;
  /* initialize Webots */
  wb_robot_init();

  if (strcmp(wb_robot_get_model(), "GCtronic e-puck2") == 0) {
    printf("e-puck2 robot\n");
    time_step = 64;
    camera_time_step = 64;
  } else {  // original e-puck
    printf("e-puck robot\n");
    time_step = 256;
    camera_time_step = 1024;
  }

  /* get and enable the camera and accelerometer */
  WbDeviceTag camera = wb_robot_get_device("camera");
  wb_camera_enable(camera, camera_time_step);
  WbDeviceTag accelerometer = wb_robot_get_device("accelerometer");
  wb_accelerometer_enable(accelerometer, time_step);

  /* get a handler to the motors and set target position to infinity (speed control). */
  left_motor = wb_robot_get_device("left wheel motor");
  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);

  /* get a handler to the position sensors and enable them. */
  left_position_sensor = wb_robot_get_device("left wheel sensor");
  right_position_sensor = wb_robot_get_device("right wheel sensor");
  wb_position_sensor_enable(left_position_sensor, time_step);
  wb_position_sensor_enable(right_position_sensor, time_step);

  for (i = 0; i < 8; i++) {
    char device_name[4];

    /* get distance sensors */
    sprintf(device_name, "ps%d", i);
    distance_sensor[i] = wb_robot_get_device(device_name);
    wb_distance_sensor_enable(distance_sensor[i], time_step);
  }

  /* main loop */
  while (wb_robot_step(time_step) != -1) {
    /* get sensors values */
    for (i = 0; i < 8; i++)
      sensors_value[i] = wb_distance_sensor_get_value(distance_sensor[i]);
    const double *a = wb_accelerometer_get_values(accelerometer);
    printf("accelerometer values = %0.2f %0.2f %0.2f\n", a[0], a[1], a[2]);

    /* compute odometry and speed values*/
    compute_odometry(left_position_sensor, right_position_sensor);
    for (i = 0; i < 2; i++) {
      speed[i] = 0.0;
      for (j = 0; j < 8; j++)
        speed[i] += braitenberg_coefficients[j][i] * (1.0 - (sensors_value[j] / RANGE));
    }

    /* set speed values */
    wb_motor_set_velocity(left_motor, speed[0]);
    wb_motor_set_velocity(right_motor, speed[1]);
  }

  wb_robot_cleanup();

  return 0;
}