Webots学习笔记—四轮小车的巡线(基于红外传感器)

卡卡南安

已于 2022-05-22 09:43:58 修改

阅读量5.3k

点赞数 8

分类专栏： Webots学习笔记文章标签：经验分享

于 2020-07-08 17:07:18 首次发布

本文链接：https://blog.csdn.net/cyj972628089/article/details/107186254

版权

Webots学习笔记专栏收录该内容

6 篇文章 17 订阅

订阅专栏

前言

本次所用到的四轮小车模型如下
在这里插入图片描述
该四轮小车的模型搭建过程请阅读我的另一篇文章：
https://blog.csdn.net/cyj972628089/article/details/107009528（Webots学习笔记—四轮小车的模型搭建和简单控制）

修改地面

地面图案的修改方式如图所示，在url下面的添加自己想要的背景图案。
在这里插入图片描述
这里把巡线的地面图案提供给大家。

红外传感器介绍

本项目用到的是Base nodes下的DistanceSensor节点，关于这个节点的详细介绍请大家自行查阅文档Reference Manual，这里我们只做一个与本项目有关的简单介绍。

首先我们查阅官方文档，看看DistanceSensor是什么
在这里插入图片描述
可见通过修改节点的相关属性，可以模拟通用传感器、红外传感器、声纳传感器或激光测距仪四种。本项目中我们使用的是红外传感器。

在这里插入图片描述
阅读官方文档我们可以看到，利用DistanceSensor可以实现巡线功能，在官方项目中也提供了相关的案例，大家可以点开来看看。可以在代码中添加一个printf输出红外传感器返回值，方便我们观察红外传感器照射在不同颜色地面时的不同值。

通过这个案例我们可以看到，当红外传感器照射到地面黑色和白色部分时，传回来的值是不同的，而且相差很大，利用这个性质，我们就可以实现四轮小车的巡线。

添加红外传感器

在Robots节点下的children属性中添加一个DistanceSensor节点
在这里插入图片描述
修改DistanceSensor下的lookupTable参数和type

修改红外传感器的位置和坐标轴，让X轴也就是红色的轴垂直向下，修改右边红外传感器的名字

复制这个传感器，改变他的位置到小车左边位置，并于右边的红外传感器对称，两个传感器之间的距离要大于黑线的宽度，修改左边红外传感器的名称
在这里插入图片描述

至此，红外传感器就添加完毕了。

四轮小车的巡线

先贴上代码

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

#define TIME_STEP 64

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

  wb_robot_init();
  
  int i;

     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);
  }

   
     WbDeviceTag motors[4];
  char motors_names[4][8] = {"motor_1", "motor_2", "motor_3", "motor_4"};
  for (int i = 0; i < 4; i++) {
    motors[i] = wb_robot_get_device(motors_names[i]);
    wb_motor_set_position(motors[i], INFINITY);
    wb_motor_set_velocity(motors[i],0);
  }
  
   double left_speed = 1.0;
   double right_speed = 1.0;

  while (wb_robot_step(TIME_STEP) != -1) {

    float  lf_wheel = wb_distance_sensor_get_value(ds[0]);
    float  rt_wheel = wb_distance_sensor_get_value(ds[1]);
    printf("left:%f,right:%f\n",lf_wheel,rt_wheel);
  
    if((lf_wheel <= 60)&&(rt_wheel <= 60))
    {
      left_speed = 1.0;
      right_speed = 1.0;
    }
    else if((lf_wheel <=60)&&(rt_wheel > 60))
    {
      left_speed = 1.0;
      right_speed = -0.4;
    }
    else if((lf_wheel > 60)&&(rt_wheel <= 60))
    {
      left_speed = -0.4;
      right_speed = 1.0;
    }

    wb_motor_set_velocity(motors[0],left_speed);
    wb_motor_set_velocity(motors[1],right_speed);
    wb_motor_set_velocity(motors[2],left_speed);
    wb_motor_set_velocity(motors[3],right_speed);

  };

  wb_robot_cleanup();

  return 0;
}

prinf函数是为了输出红外传感器传回来的值，在不同情况下传回来的值可能会有所差别，读者需要根据实际情况修改下面这段代码的判断条件

 if((lf_wheel <= 60)&&(rt_wheel <= 60))
    {
      left_speed = 1.0;
      right_speed = 1.0;
    }
    else if((lf_wheel <=60)&&(rt_wheel > 60))
    {
      left_speed = 1.0;
      right_speed = -0.4;
    }
    else if((lf_wheel > 60)&&(rt_wheel <= 60))
    {
      left_speed = -0.4;
      right_speed = 1.0;
    }