Webots学习笔记 4.机器人控制器代码介绍

关于控制器

本章目标:介绍Webots的基本编程
1.scene tree node和控制器API的关系
2.控制器如何初始化、清理，如何初始化机器人设备，如何获取传感器值，如何控制执行器，如何编写简单反馈回路。

新世界/新控制器

$ 将之前的世界另存为collision_avoidance.wbt
$ 创建一个C控制器，命名为e-puck_avoid_collision，并将其与e-puck node的controller filed关联。

了解e-puck 模型

initialize_robot

simulation_step

read_sensors

process_behavior

write_actuators

cleanup_robot

code

#include 
#include 
#include 

// 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] > 70.0 ||
      ps_values[1] > 70.0 ||
      ps_values[2] > 70.0;
    bool left_obstacle =
      ps_values[5] > 70.0 ||
      ps_values[6] > 70.0 ||
      ps_values[7] > 70.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
}

总结

1.wb_robot_init 头。
2.wb_robot_cleanup 尾。

3.设备根据设备节点的域名调用:
a. 拿句柄，wb_robot_get_device
b.使能，wb_distance_sensor_enable
c.读数据，wb_distance_sensor_get_value

4.每个控制器进程跟Webots进程独立，控制器的代码跟libController动态库关联，该库处理控制器跟Webots进程间通信。