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 <webots/robot.h>
#include <webots/distance_sensor.h>
#include <webots/motor.h>// 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 APIwb_robot_init();// internal variablesint i;WbDeviceTag ps[8];char ps_names[8][4] = {"ps0", "ps1", "ps2", "ps3","ps4", "ps5", "ps6", "ps7"};// initialize devicesfor (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 receivedwhile (wb_robot_step(TIME_STEP) != -1) {// read sensors outputsdouble ps_values[8];for (i = 0; i < 8 ; i++)ps_values[i] = wb_distance_sensor_get_value(ps[i]);// detect obstaclesbool 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 obstaclesif (left_obstacle) {// turn rightleft_speed  += 0.5 * MAX_SPEED;right_speed -= 0.5 * MAX_SPEED;}else if (right_obstacle) {// turn leftleft_speed  -= 0.5 * MAX_SPEED;right_speed += 0.5 * MAX_SPEED;}// write actuators inputswb_motor_set_velocity(left_motor, left_speed);wb_motor_set_velocity(right_motor, right_speed);}// cleanup the Webots APIwb_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进程间通信。