webots续集2

用户11770632

发布于 2025-11-15 10:56:12

910

上次我发现他的动作真的很奇怪，那我们咋整，其实就是看看实际的传感器数据的值，以及他的逻辑问题，我们首先先看他的代码逻辑，那就上代码了，大佬可直接忽略

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <webots/device.h>
#include <webots/distance_sensor.h>
#include <webots/led.h>
#include <webots/motor.h>
#include <webots/nodes.h>
#include <webots/robot.h>

/* Device stuff */
#define DISTANCE_SENSORS_NUMBER 8
static WbDeviceTag distance_sensors[DISTANCE_SENSORS_NUMBER];
static double distance_sensors_values[DISTANCE_SENSORS_NUMBER];
static const char *distance_sensors_names[DISTANCE_SENSORS_NUMBER] = {"ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"};

#define GROUND_SENSORS_NUMBER 3
static WbDeviceTag ground_sensors[GROUND_SENSORS_NUMBER];
static double ground_sensors_values[GROUND_SENSORS_NUMBER] = {0.0, 0.0, 0.0};
static const char *ground_sensors_names[GROUND_SENSORS_NUMBER] = {"gs0", "gs1", "gs2"};

#define LEDS_NUMBER 10
static WbDeviceTag leds[LEDS_NUMBER];
static bool leds_values[LEDS_NUMBER];
static const char *leds_names[LEDS_NUMBER] = {"led0", "led1", "led2", "led3", "led4", "led5", "led6", "led7", "led8", "led9"};

static WbDeviceTag left_motor, right_motor;

#define LEFT 0
#define RIGHT 1
#define MAX_SPEED 6.28*2
static double speeds[2];

/* Breitenberg stuff */
static double weights[DISTANCE_SENSORS_NUMBER][2] = {{-1.3, -1.0}, {-1.3, -1.0}, {-0.5, 0.5}, {0.0, 0.0},
                                                     {0.0, 0.0},   {0.05, -0.5}, {-0.75, 0},  {-0.75, 0}};
static double offsets[2] = {0.5 * MAX_SPEED, 0.5 * MAX_SPEED};

static int get_time_step() {
  static int time_step = -1;
  if (time_step == -1)
    time_step = (int)wb_robot_get_basic_time_step();
  return time_step;
}

static void step() {
  if (wb_robot_step(get_time_step()) == -1) {
    wb_robot_cleanup();
    exit(EXIT_SUCCESS);
  }
}

static void passive_wait(double sec) {
  double start_time = wb_robot_get_time();
  do {
    step();
  } while (start_time + sec > wb_robot_get_time());
}

static void init_devices() {
  int i;
  for (i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {
    distance_sensors[i] = wb_robot_get_device(distance_sensors_names[i]);
    wb_distance_sensor_enable(distance_sensors[i], get_time_step());
  }
  for (i = 0; i < LEDS_NUMBER; i++)
    leds[i] = wb_robot_get_device(leds_names[i]);

  // silently initialize the ground sensors if they exists
  for (i = 0; i < GROUND_SENSORS_NUMBER; i++)
    ground_sensors[i] = (WbDeviceTag)0;
  int ndevices = wb_robot_get_number_of_devices();
  for (i = 0; i < ndevices; i++) {
    WbDeviceTag dtag = wb_robot_get_device_by_index(i);
    const char *dname = wb_device_get_name(dtag);
    WbNodeType dtype = wb_device_get_node_type(dtag);
    if (dtype == WB_NODE_DISTANCE_SENSOR && strlen(dname) == 3 && dname[0] == 'g' && dname[1] == 's') {
      int id = dname[2] - '0';
      if (id >= 0 && id < GROUND_SENSORS_NUMBER) {
        ground_sensors[id] = wb_robot_get_device(ground_sensors_names[id]);
        wb_distance_sensor_enable(ground_sensors[id], get_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);

  step();
}

static void reset_actuator_values() {
  int i;
  for (i = 0; i < 2; i++)
    speeds[i] = 0.0;
  for (i = 0; i < LEDS_NUMBER; i++)
    leds_values[i] = false;
}

static void get_sensor_input() {
  int i;
  for (i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {
    distance_sensors_values[i] = wb_distance_sensor_get_value(distance_sensors[i]);

    // scale the data in order to have a value between 0.0 and 1.0
    // 1.0 representing something to avoid, 0.0 representing nothing to avoid
    distance_sensors_values[i] /= 4096;
  }

  for (i = 0; i < GROUND_SENSORS_NUMBER; i++) {
    if (ground_sensors[i])
      ground_sensors_values[i] = wb_distance_sensor_get_value(ground_sensors[i]);
  }
}

static bool cliff_detected() {
  int i;
  for (i = 0; i < GROUND_SENSORS_NUMBER; i++) {
    if (!ground_sensors[i])
      return false;
    if (ground_sensors_values[i] < 500.0)
      return true;
  }
  return false;
}

static void set_actuators() {
  int i;
  for (i = 0; i < LEDS_NUMBER; i++)
    wb_led_set(leds[i], leds_values[i]);
  wb_motor_set_velocity(left_motor, speeds[LEFT]);
  wb_motor_set_velocity(right_motor, speeds[RIGHT]);
}

static void blink_leds() {
  static int counter = 0;
  counter++;
  leds_values[(counter / 10) % LEDS_NUMBER] = true;
}

static void run_braitenberg() {
  int i, j;
  for (i = 0; i < 2; i++) {
    speeds[i] = 0.0;
    for (j = 0; j < DISTANCE_SENSORS_NUMBER; j++)
      speeds[i] += distance_sensors_values[j] * weights[j][i];

    speeds[i] = offsets[i] + speeds[i] * MAX_SPEED;
    if (speeds[i] > MAX_SPEED)
      speeds[i] = MAX_SPEED;
    else if (speeds[i] < -MAX_SPEED)
      speeds[i] = -MAX_SPEED;
  }
}

static void go_backwards() {
  wb_motor_set_velocity(left_motor, -MAX_SPEED);
  wb_motor_set_velocity(right_motor, -MAX_SPEED);
  passive_wait(0.2);
}

static void turn_left() {
  wb_motor_set_velocity(left_motor, -MAX_SPEED);
  wb_motor_set_velocity(right_motor, MAX_SPEED);
  passive_wait(0.2);
}

int main(int argc, char **argv) {
  wb_robot_init();

  printf("Default controller of the e-puck robot started...\n");

  init_devices();

  while (true) {
    reset_actuator_values();
    get_sensor_input();
    blink_leds();
    if (cliff_detected()) {
      go_backwards();
      turn_left();
    } else {
      run_braitenberg();
    }
    set_actuators();
    step();
  };

  return EXIT_SUCCESS;
}

我先把他的整体代码贴上来，大致过一下

主要功能概述

初始化机器人设备（电机、传感器、LED）。
读取传感器值（避障和防止掉落）。
使用 Braitenberg 算法控制移动方向（避障行为）。
遇到悬崖（黑线）时倒退并左转。
循环控制机器人行为直到退出模拟。

别的大家就自己看看，一个个解释等下次吧

然后我就想着先测测传感器的值吧，我找了好久传感器在webots中的设置，找不到，是在他的元模型里面添加的，所以改不了，我们测一下他的传感器数值，新建一个控制器

OK了

先把代码粘贴到新建文件中，代码如下

#include <stdio.h>
#include <webots/robot.h>
#include <webots/distance_sensor.h>

#define TIME_STEP 64  // e-puck 默认时间步
#define DISTANCE_SENSORS_NUMBER 8
#define GROUND_SENSORS_NUMBER 3

static WbDeviceTag distance_sensors[DISTANCE_SENSORS_NUMBER];
static const char *distance_sensors_names[DISTANCE_SENSORS_NUMBER] = {
  "ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"
};

static WbDeviceTag ground_sensors[GROUND_SENSORS_NUMBER];
static const char *ground_sensors_names[GROUND_SENSORS_NUMBER] = {
  "gs0", "gs1", "gs2"
};

int main() {
  wb_robot_init();

  // 初始化距离传感器
  for (int i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {
    distance_sensors[i] = wb_robot_get_device(distance_sensors_names[i]);
    wb_distance_sensor_enable(distance_sensors[i], TIME_STEP);
  }

  // 初始化地面传感器
  for (int i = 0; i < GROUND_SENSORS_NUMBER; i++) {
    ground_sensors[i] = wb_robot_get_device(ground_sensors_names[i]);
    wb_distance_sensor_enable(ground_sensors[i], TIME_STEP);
  }

  // 主循环
  while (wb_robot_step(TIME_STEP) != -1) {
    printf("Distance sensors:\n");
    for (int i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {
      double value = wb_distance_sensor_get_value(distance_sensors[i]);
      printf("  %s: %.2f\n", distance_sensors_names[i], value);
    }

    printf("Ground sensors:\n");
    for (int i = 0; i < GROUND_SENSORS_NUMBER; i++) {
      double value = wb_distance_sensor_get_value(ground_sensors[i]);
      printf("  %s: %.2f\n", ground_sensors_names[i], value);
    }

    printf("---------------\n");
  }

  wb_robot_cleanup();
  return 0;
}

按图上编译保存，更换控制器

点击开始，下方开始打印信息，发现红色报警，

这个问题是说明你尝试从一个 未正确初始化或未启用的距离传感器（Distance Sensor）设备 中读取数据，也就是说：

wb_distance_sensor_get_value() 使用的设备 tag 是无效的（NULL 或未设置好）；
通常是由于：
1. 没有使用 wb_robot_get_device() 获取设备；
2. 或者获取了设备但没有用 wb_distance_sensor_enable() 启用；
3. 获取设备名称拼写错误；
4. 没有在 Webots 仿真场景中放入对应的传感器设备。

咱们是看避障，地面传感器先不要了，简化一下程序，打印速度太快，改成一秒一次

#include <stdio.h>
#include <webots/robot.h>
#include <webots/distance_sensor.h>

#define TIME_STEP 64  // 每次 step 的时间，单位 ms
#define DISTANCE_SENSORS_NUMBER 8

static WbDeviceTag distance_sensors[DISTANCE_SENSORS_NUMBER];
static const char *distance_sensors_names[DISTANCE_SENSORS_NUMBER] = {
  "ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"
};

int main() {
  wb_robot_init();

  for (int i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {
    distance_sensors[i] = wb_robot_get_device(distance_sensors_names[i]);
    wb_distance_sensor_enable(distance_sensors[i], TIME_STEP);
  }

  // 新增：计数器
  int counter = 0;
  int steps_per_second = 1000 / TIME_STEP;  // 一秒钟多少步

  while (wb_robot_step(TIME_STEP) != -1) {
    counter++;

    // 每秒打印一次
    if (counter >= steps_per_second) {
      counter = 0;

      printf("Distance sensors:\n");
      for (int i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {
        double value = wb_distance_sensor_get_value(distance_sensors[i]);
        printf("  %s: %.2f\n", distance_sensors_names[i], value);
      }
      printf("---------------\n");
    }
  }

  wb_robot_cleanup();
  return 0;
}

粘贴到控制器中，重新编译，再次尝试