前言：自动跟随小车的功能可以使小车能够自动地跟随某个目标物体或者人移动。这种功能在很多场合都有着重要的作用和意义。比如：

摄影和影视制作：在摄影和影视制作中，自动跟随小车可以帮助摄影师或者导演更好地捕捉目标物体或者人的移动轨迹，从而制作出更加流畅和自然的画面。
物流和仓储：在物流和仓储领域，自动跟随小车可以帮助工人快速地将货物从一个地方运到另一个地方，提高工作效率和减少人力成本。
环境监测：在环境监测领域，自动跟随小车可以帮助科研人员快速地将监测设备移动到需要监测的位置，从而更好地收集环境数据。
军事和安全：在军事和安全领域，自动跟随小车可以帮助士兵或者警察在行动中更好地保持目标物体或者人的跟踪，提高作战效率和安全性。

本次通过超声波模块获取与特定目标的距离信息，实时控制小车与特定目标的距离到特定的范围内。主要使用Arduino Mage 2560作为下位控制器，通过超声波模块获取特定目标的距离信息。控制霍尔编码器直流电机的运动。旭日x3派作为上位控制器，用于实时监测小车的车速和超声波的距离信息（也可以结合其它传感器计算出转向，车速（PWM值）来实时控制小车运动）。其中Arduino和旭日x3派通过usb串口使用rosserial功能包进行实时通信。小车实物图如下。

一.小车跟随功能的实现

主要使用TB6612（下图(a)）电机驱动板、JGB370-520霍尔编码器电机、超声波和Arduino Mage 2560，TB6612电机驱动板主要是起到转接作用，JGB370-520霍尔编码器电机实现小车的运动，超声波传感器用于测定小车与目标的距离，Arduino Mage 2560根据测定的小车与目标的距离，计算出控制指令控制小车运动。

1.设置小车转速

指定电机motor，motor=1（2）代表电机A（B）；指定转速pwm，大小范围为0~255。

void SetPWM(int motor, int pwm)
{
  if(motor==3&&pwm>=0)//motor=1代表控制电机A，pwm>=0则(AIN1, AIN2)=(1, 0)为正转
  {
    digitalWrite(AIN3, 1);
    digitalWrite(AIN4, 0);
    analogWrite(PWMC, pwm);
  }
  else if(motor==3&&pwm<0)//motor=1代表控制电机A，pwm<0则(AIN1, AIN2)=(0, 1)为反转
  {
    digitalWrite(AIN3, 0);
    digitalWrite(AIN4, 1);
    analogWrite(PWMC, -pwm);
  }
  else if(motor==4&&pwm>=0)//motor=4代表控制电机D，pwm>=0则(BIN3, BIN4)=(0, 1)为正转
  {
    digitalWrite(BIN3, 1);
    digitalWrite(BIN4, 0);
    analogWrite(PWMD, pwm);
  }
  else if(motor==4&&pwm<0)//motor=2代表控制电机B，pwm<0则(BIN1, BIN2)=(1, 0)为反转
  {
    digitalWrite(BIN3, 0);
    digitalWrite(BIN4, 1);
    analogWrite(PWMD, -pwm);
  }
  else if(motor==2&&pwm>=0)//motor=2代表控制电机B，pwm>=0则(BIN1, BIN2)=(0, 1)为正转
  {
    digitalWrite(BIN1, 0);
    digitalWrite(BIN2, 1);
    analogWrite(PWMB, pwm);
  }
  else if(motor==2&&pwm<0)//motor=2代表控制电机B，pwm<0则(BIN1, BIN2)=(1, 0)为反转
  {
    digitalWrite(BIN1, 1);
    digitalWrite(BIN2, 0);
    analogWrite(PWMB, -pwm);
  }
  else if(motor==1&&pwm>=0)//motor=1代表控制电机A，pwm>=0则(AIN1, AIN2)=(1, 0)为正转
  {
    digitalWrite(AIN1, 1);
    digitalWrite(AIN2, 0);
    analogWrite(PWMA, pwm);
  }
  else if(motor==1&&pwm<0)//motor=1代表控制电机A，pwm<0则(AIN1, AIN2)=(0, 1)为反转
  {
    digitalWrite(AIN1, 0);
    digitalWrite(AIN2, 1);
    analogWrite(PWMA, -pwm);
  }
}

2.小车跟随功能的实现

通过超声波雷达测定小车与目标的距离，生成控制小车运动的指令

void  followDrive() {  // 
    getDistance();  // 获取当前的距离
    if ((dist >= followDist - followBalance) && (dist <= followDist + followBalance)) {
      //analogWrite(leftSpeed, intSpeedPWM);
      //analogWrite(rightSpeed, intSpeedPWM);
      if (dist > followDist) {  // 当两者间的距离大于设定值时，小车前进
        forward();    
      }
    else if (dist < followDist) {  // 当两者间的距离大于设定值时，小车后退
        backward();   
      }
      else {
        pause();   // 小车停止
      }
    }
    else {
      pause();
    }
}


void getDistance() {  // 超声波测距函数
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(11);
  digitalWrite(TrigPin, LOW);
  dist = pulseIn(EchoPin, HIGH) / 58.0;
}

// 前进
void forward(){
  SetPWM(1, 255);//电机ABCD同时满速正转
  SetPWM(2, -255);
  SetPWM(3, -255);
  SetPWM(4, -255);
}
//后退
void backward(){
  SetPWM(1, -200);//电机ABCD同时满速反转
  SetPWM(2, 200);
  SetPWM(3, 200);
  SetPWM(4, 200);
}
//停止
void pause(){
  SetPWM(1, 0);//电机ABCD同时停
  SetPWM(2, 0);
  SetPWM(3, 0);
  SetPWM(4, 0);
}
void CountA() 
{
 if(digitalRead(pinB) == HIGH)
 {c = pos;}
  if(digitalRead(pinB) == LOW)
  {c = neg;}
  ppsA++;
}

3.电机测速的实现

AB相增量式编码器测速原理：

AB相编码器主要构成为A相与B相，每一相每转过单位的角度就发出一个脉冲信号(一圈可以发出N个脉冲信号)，A相、B相为相互延迟1/4周期的脉冲输出，根据延迟关系可以区别正反转，而且通过取A相、B相的上升和下降沿可以进行单频或2倍频或4倍频测速。

void flash() 
{
  int w = ppsA;
  velocity = c*v(w);
  ne_msg.data = velocity;
  motorne.publish( &ne_msg );
  nh.spinOnce();
  ppsA = 0;
  
  delay(2);
  Serial.println(velocity);
  
}

float v(float n)
{
  float vel = n/1.5; // n/(15*0.1)
  return vel;
}

完整代码如下：

///////////////////TB6612引脚接线///////////////////////////
//直流电机----------TB6612丝印标识----------ArduinoUNO主板引脚
//                     PWMA-----------------3
//                     AIN2-----------------4
//                     AIN1-----------------5
//                     STBY-----------------7
//                     BIN1-----------------8
//                     BIN2-----------------9
//                     PWMB-----------------10
//                     GND------------------GND
//                     ADC------------------A0 (只有带TB6612带稳压模块版才有ADC测量电池电压功能)    
//                     VM-------------------12V电池
//                     VCC------------------5V  （使用带有稳压模块版时，接线变动如下：  5V---------5V  板子可向arduino供电）
//                     GND------------------GND  
//电机A正极-------------AO1
//电机A负极-------------A02
//电机B负极-------------BO2
//电机B正极-------------BO1
//                     GND-----------------GND
//直流电机----------TB6612丝印标识----------ArduinoUNO主板引脚

//定义引脚名称
#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int16.h>
#include <MsTimer2.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define pinA 2
#define pinB 3
#define pos 1
#define neg -1
int ppsA=0;
int c;
float velocity = 0;
//Potentiometer
int pot = 0;
ros::NodeHandle nh;
std_msgs::Float32 ne_msg;
ros::Publisher motorne("motorne", &ne_msg);
std_msgs::Int16 ctrl_msg;
ros::Publisher ctrl("ctrl", &ctrl_msg);
const int TrigPin = 11;  //超声波触发引脚连接UNO引脚9
const int EchoPin = 12;    //超声波接受引脚连接UNO引脚2
#define PWMA 10  //3为模拟引脚，用于PWM控制
#define AIN1 8
#define AIN2 9
#define PWMC 14  //3为模拟引脚，用于PWM控制
#define AIN3 18
#define AIN4 19
#define PWMB 13  //10为模拟引脚，用于PWM控制
#define BIN1 5
#define BIN2 4
#define PWMD 15  //10为模拟引脚，用于PWM控制
#define BIN3 17
#define BIN4 16
#define STBY 7  //2、4、8、12、7为数字引脚，用于开关量控制
#define Voltage A0 //使用模拟引脚
int valA=0;
float  n;
int flag=0;
//int pwm=70;
unsigned long starttime,stoptime;
int PwmA, PwmB;
int PwmC, PwmD;
int dist;
int followDist = 40; // 定义反应的距离
int followBalance = 10; // 定义范围
void setup() {
  //TB6612电机驱动模块控制信号初始化
  pinMode(AIN1, OUTPUT);//控制电机A的方向，(AIN1, AIN2)=(1, 0)为正转，(AIN1, AIN2)=(0, 1)为反转
  pinMode(AIN2, OUTPUT);
  pinMode(AIN3, OUTPUT);//控制电机A的方向，(AIN1, AIN2)=(1, 0)为正转，(AIN1, AIN2)=(0, 1)为反转
  pinMode(AIN4, OUTPUT);
  pinMode(BIN1, OUTPUT);//控制电机B的方向，(BIN1, BIN2)=(0, 1)为正转，(BIN1, BIN2)=(1, 0)为反转
  pinMode(BIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);//A电机PWM
  pinMode(PWMB, OUTPUT);//B电机PWM
  pinMode(BIN3, OUTPUT);//控制电机B的方向，(BIN1, BIN2)=(0, 1)为正转，(BIN1, BIN2)=(1, 0)为反转
  pinMode(BIN4, OUTPUT);
  pinMode(PWMC, OUTPUT);//A电机PWM
  pinMode(PWMD, OUTPUT);//B电机PWM
  pinMode(STBY, OUTPUT);//TB6612FNG使能, 置0则所有电机停止, 置1才允许控制电机
  pinMode(TrigPin,OUTPUT);
  pinMode(EchoPin,INPUT);
  //初始化TB6612电机驱动模块
  digitalWrite(AIN1, 1);
  digitalWrite(AIN2, 0);
  digitalWrite(AIN3, 1);
  digitalWrite(AIN4, 0);
  digitalWrite(BIN1, 1);
  digitalWrite(BIN2, 0);
  digitalWrite(STBY, 1);
  digitalWrite(BIN3, 1);
  digitalWrite(BIN4, 0);
 // digitalWrite(STBY, 1);
  analogWrite(PWMC, 0);
  analogWrite(PWMD, 0);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);

 //初始化串口，用于输出距离
  Serial.begin(57600);
  //Serial.println(dist); 
  pwm.begin();
  pwm.setPWMFreq(50);
  nh.initNode();
  nh.advertise(motorne);
  nh.advertise(ctrl);
  attachInterrupt(0,CountA, FALLING);// 检测脉冲下降沿中断，并转到CountA函数
  MsTimer2::set(100,flash);          // 中断设置函数，0.1s
  MsTimer2::start(); 
}

//void timer()
//{
//  valA++;
//  stoptime=millis(); 
//   if((stoptime-starttime)>100)
//   { 
//    detachInterrupt(0);
//    flag=1;
//   }
//}
/**************************************************************************
函数功能：设置指定电机转速
入口参数：指定电机motor，motor=1（2）代表电机A（B）； 指定转速pwm，大小范围为0~255，代表停转和满速
返回  值：无
**************************************************************************/
void SetPWM(int motor, int pwm)
{
  if(motor==3&&pwm>=0)//motor=1代表控制电机A，pwm>=0则(AIN1, AIN2)=(1, 0)为正转
  {
    digitalWrite(AIN3, 1);
    digitalWrite(AIN4, 0);
    analogWrite(PWMC, pwm);
  }
  else if(motor==3&&pwm<0)//motor=1代表控制电机A，pwm<0则(AIN1, AIN2)=(0, 1)为反转
  {
    digitalWrite(AIN3, 0);
    digitalWrite(AIN4, 1);
    analogWrite(PWMC, -pwm);
  }
  else if(motor==4&&pwm>=0)//motor=4代表控制电机D，pwm>=0则(BIN3, BIN4)=(0, 1)为正转
  {
    digitalWrite(BIN3, 1);
    digitalWrite(BIN4, 0);
    analogWrite(PWMD, pwm);
  }
  else if(motor==4&&pwm<0)//motor=2代表控制电机B，pwm<0则(BIN1, BIN2)=(1, 0)为反转
  {
    digitalWrite(BIN3, 0);
    digitalWrite(BIN4, 1);
    analogWrite(PWMD, -pwm);
  }
  else if(motor==2&&pwm>=0)//motor=2代表控制电机B，pwm>=0则(BIN1, BIN2)=(0, 1)为正转
  {
    digitalWrite(BIN1, 0);
    digitalWrite(BIN2, 1);
    analogWrite(PWMB, pwm);
  }
  else if(motor==2&&pwm<0)//motor=2代表控制电机B，pwm<0则(BIN1, BIN2)=(1, 0)为反转
  {
    digitalWrite(BIN1, 1);
    digitalWrite(BIN2, 0);
    analogWrite(PWMB, -pwm);
  }
  else if(motor==1&&pwm>=0)//motor=1代表控制电机A，pwm>=0则(AIN1, AIN2)=(1, 0)为正转
  {
    digitalWrite(AIN1, 1);
    digitalWrite(AIN2, 0);
    analogWrite(PWMA, pwm);
  }
  else if(motor==1&&pwm<0)//motor=1代表控制电机A，pwm<0则(AIN1, AIN2)=(0, 1)为反转
  {
    digitalWrite(AIN1, 0);
    digitalWrite(AIN2, 1);
    analogWrite(PWMA, -pwm);
  }
}
//******************************
//  功能：跟随模式
//  参数：无
//*******************************
void  followDrive() {  // 
    getDistance();  // 获取当前的距离
    if ((dist >= followDist - followBalance) && (dist <= followDist + followBalance)) {
      //analogWrite(leftSpeed, intSpeedPWM);
      //analogWrite(rightSpeed, intSpeedPWM);
      if (dist > followDist) {  // 当两者间的距离大于设定值时，小车前进
        forward();    
      }
    else if (dist < followDist) {  // 当两者间的距离大于设定值时，小车后退
        backward();   
      }
      else {
        pause();   // 小车停止
      }
    }
    else {
      pause();
    }
}


void getDistance() {  // 超声波测距函数
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(11);
  digitalWrite(TrigPin, LOW);
  dist = pulseIn(EchoPin, HIGH) / 58.0;
}

// 前进
void forward(){
  SetPWM(1, 255);//电机ABCD同时满速正转
  SetPWM(2, -255);
  SetPWM(3, -255);
  SetPWM(4, -255);
}
//后退
void backward(){
  SetPWM(1, -200);//电机ABCD同时满速反转
  SetPWM(2, 200);
  SetPWM(3, 200);
  SetPWM(4, 200);
}
//停止
void pause(){
  SetPWM(1, 0);//电机ABCD同时停
  SetPWM(2, 0);
  SetPWM(3, 0);
  SetPWM(4, 0);
}
void CountA() 
{
 if(digitalRead(pinB) == HIGH)
 {c = pos;}
  if(digitalRead(pinB) == LOW)
  {c = neg;}
  ppsA++;
}

void flash() 
{
/*
  pot = analogRead(A0);
  PWMout = map(pot,0,1023,0,255);
  analogWrite(ENA,PWMout);
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  ctrl_msg.data = PWMout;
  ctrl.publish( &ctrl_msg );
*/  
  int w = ppsA;
  velocity = c*v(w);
  ne_msg.data = velocity;
  motorne.publish( &ne_msg );
  nh.spinOnce();
  ppsA = 0;
  
  delay(2);
  Serial.println(velocity);
  
}

float v(float n)
{
  float vel = n/1.5; // n/(15*0.1)
  return vel;
}
void fgfd() 
{
  pot = analogRead(A0);
  //PWMout = map(pot,0,1023,0,100);
  //ctrl_msg.data = PWMout;
  ctrl.publish( &ctrl_msg );
  nh.spinOnce();
}
void loop() 
{  fgfd();
   followDrive(); 
  //初始化串口，用于输出距离
   Serial.println(dist);
   Serial.println(velocity);
}

二.旭日x3派+ROS noetic+Arduino+ rosserial_arduino配置

本次博客旭日x3派主要起到监测小车车速的作用，后期可以通过该功能包对小车运动以及更多功能的开发进行扩展。rosserial_arduino功能包可参考http://t.csdn.cn/xKMKW 和https://appougfvr0w9908.h5.xiaoeknow.com/p/course/column/p_62414bd7e4b0f7cb7c77a51a?type=3。

1.安装arduino ide

sudo apt-get install Arduino

2.构建ros_lib库

（1）配置依赖环境（根据自己的ros版本安装）

sudo apt-get install ros-noetic-rosserial-arduino

sudo apt-get install ros-noetic-rosserial

rospack profile

（2）cd到Arduino/libraries,构建ros_lib库

cd Arduino/libraries

rosrun rosserial_arduino make_libraries.py .

3.启动rosserial_server服务器节点

rosserial_server功能包包含主机端rosserial连接的C++实现。它会自动处理已连接的启用rosserial的设备的设置，发布和订阅。

首先，使用usb串口连接arduino与旭日x3派，在x3派端启动roscore

然后，启动主服务器节点。启动文件用于启动串行链接。如果未指定端口，则默认为 /dev/ttyACM0。启动方法如下

roslaunch rosserial_server serial.launch port:=/dev/ttyACM0

rostopic list可以查看发布的话题。启动rqt_plot可以监测电机转度，效果如下：

参考链接：http://t.csdn.cn/GiJX3

http://t.csdn.cn/xKMKW

https://appougfvr0w9908.h5.xiaoeknow.com/p/course/column/p_62414bd7e4b0f7cb7c77a51a?type=3

旭日x3派自动跟随小车

阿#杰

一.小车跟随功能的实现

1.设置小车转速

2.小车跟随功能的实现

3.电机测速的实现

二.旭日x3派+ROS noetic+Arduino+ rosserial_arduino配置

1.安装arduino ide

2.构建ros_lib库

3.启动rosserial_server服务器节点

为你推荐

【物联网】总结SPI、I2C、UART三者的区别以及有关知识

Arduino基础项目八：振动检测

Arduino Uno 实验4——电位器

树莓派Python开发：led实验

单片机蓝桥杯——超声波测距

从零开始制作STM32F103RCT6小车（四）

评论（0）

关于作者

阿#杰

5

0

0

2

使用百度API和opencv在x3派中实现人脸识别

旭日x3派自动跟随小车

使用旭日X3派实现手势检测

相关推荐

Moco-ML 开源四足机器人 项目教程4：Moco-Minitaur LTS机器人的机架设计

飞越650四轴无人机安装全程详解（多图）

基于Arduino的智能环境监测与反馈系统

基于STM32智能小车-＞超声波篇

STM32——FSMC模拟8080时序点亮液晶屏

基于STM32智能小车-＞电机驱动

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

Moco-ML 开源四足机器人项目教程4：Moco-Minitaur LTS机器人的机架设计

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板