关于控制器
本章目标:介绍Webots的基本编程
1.scene tree node和控制器API的关系
2.控制器如何初始化、清理,如何初始化机器人设备,如何获取传感器值,如何控制执行器,如何编写简单反馈回路。
新世界/新控制器
$ 将之前的世界另存为collision_avoidance.wbt
$ 创建一个C控制器,命名为e-puck_avoid_collision,并将其与e-puck node的controller filed关联。
了解e-puck 模型
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 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进程间通信。