1 新建控制器
這裏我們爲上一篇博客中創建的小車模型建立一個控制器，讓小車在環境中避障運行。首先在 Wizards->New Robot Controller 菜單欄新建一個機器人控制器，保存爲 my_controller 。這裏選擇使用C語言進行新建，這時候會再右側欄出現機器人控制器的編程界面。填入以下內容：
#include <webots/distance_sensor.h>
#include <webots/motor.h>
#include <webots/robot.h>
// time in [ms] of a simulation step
#define TIME_STEP 64
// entree point of the controller
int main(int argc, char **argv) {
// initialise the Webots API
wb_robot_init();
// internal variables
int i;
int avoid_obstacle_counter = 0;
// initialise distance sensors
WbDeviceTag ds[2];
char ds_names[2][10] = {"ds_left", "ds_right"};
for (i = 0; i < 2; i++) {
ds[i] = wb_robot_get_device(ds_names[i]);
wb_distance_sensor_enable(ds[i], TIME_STEP);
}
// initialise motors
WbDeviceTag wheels[4];
char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
for (i = 0; i < 4; i++) {
wheels[i] = wb_robot_get_device(wheels_names[i]);
wb_motor_set_position(wheels[i], INFINITY);
}
// feedback loop
while (wb_robot_step(TIME_STEP) != -1) {
// init speeds
double left_speed = 1.0;
double right_speed = 1.0;
if (avoid_obstacle_counter > 0) {
avoid_obstacle_counter--;
left_speed = 1.0;
right_speed = -1.0;
} else {
// read sensors outputs
double ds_values[2];
for (i = 0; i < 2; i++)
ds_values[i] = wb_distance_sensor_get_value(ds[i]);
// increase counter in case of obstacle
if (ds_values[0] < 950.0 || ds_values[1] < 950.0)
avoid_obstacle_counter = 100;
}
// write actuators inputs
wb_motor_set_velocity(wheels[0], left_speed);
wb_motor_set_velocity(wheels[1], right_speed);
wb_motor_set_velocity(wheels[2], left_speed);
wb_motor_set_velocity(wheels[3], right_speed);
}
// cleanup the Webots API
wb_robot_cleanup();
return 0; // EXIT_SUCCESS
}
新建完控制器以後，對控制器進行編譯
2 控制器說明
首先該控制器設計到的執行器只有四個電機、傳感器有兩個距離探測器，我們的目標是讓小車實現避障（左邊有障礙物就往右邊走，反之亦然）。具體操作四個輪子是通過輪子的名稱來實現的，這裏的 “wheel1” 就是在上篇博客中自定義的電機的名稱，
// initialise motors
WbDeviceTag wheels[4];
char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
for (i = 0; i < 4; i++) {
wheels[i] = wb_robot_get_device(wheels_names[i]);
wb_motor_set_position(wheels[i], INFINITY);
}
最後通過函數設置電機的速度
wb_motor_set_velocity(wheels[0], left_speed);
同樣距離傳感器的操作也是通過名稱 "ds_left"和 "ds_right"實現的
創建操作句柄：
WbDeviceTag ds[2];
char ds_names[2][10] = {"ds_left", "ds_right"};
for (i = 0; i < 2; i++) {
ds[i] = wb_robot_get_device(ds_names[i]);
wb_distance_sensor_enable(ds[i], TIME_STEP);
}
讀取距離傳感器的值：
double ds_values[2];
for (i = 0; i < 2; i++)
ds_values[i] = wb_distance_sensor_get_value(ds[i]);
3 運行效果
最終生成的模型文件可在此處下載,下圖是小車在環境中避障運行的效果圖：
參考資料
[1] https://www.cyberbotics.com/doc/reference/motion
[2] https://cyberbotics.com/doc/guide/tutorial-4-more-about-controllers?tab-language=c
如果大家覺得文章對你有所幫助，麻煩大家幫忙點個贊。O(∩_∩)O
歡迎大家在評論區交流討論（[email protected]）