简介：
在机器人开发中虽然我们有激光雷达去感知周围的障碍物，但有的时候应为激光雷达安装位置的原因，无法识别到较低位置的障碍物，此时就需要一个性价比较高的传感器来进行辅助，一般情况可以使用红外或者超声波模块，红外受光线和颜色的干扰，因此用超声波模块是比较好的。本文将介绍怎么获取超声波数据，并在ROS中使用。
1、所需工具
starrobot底层开发板、HC-SR04/US-100、USB数据线、Keil5
starrobot底层开发板具有超声波模块接口，可以与超声波模块进行连接。下面介绍一下超声波模块测距的基本原理和模块的一些参数，超声波测距原理是在超声波发射装置发出超声波，接收器接收超声波，根据接收器接到超声波时的时间差以及超声波在介质中的传播速度，从而计算出物体距离模块的距离，与雷达测距原理相似。 超声波发射器向某一方向发射超声波，在发射时刻的同时开始计时，超声波在空气中传播，途中碰到障碍物就立即返回来，超声波接收器收到反射波就立即停止计时。
超声波在空气中的传播速度为340m/s（当然温度不同，传播速度也不同，带温度校准的模块得到的数据会更准），根据计时器记录的时间t（秒），就可以计算出发射点距障碍物的距离(s)，即：s=340t/2。
原理如图所示：
US-100超声波模块介绍：
US-100 超声波测距模块可实现 2cm~4.5m 的非接触测距功能，拥有 2.4~5.5V 的宽电压输入范围，静态功耗低于 2mA，自带温度传感器对测距结果进行校正，同时具有 GPIO，串口两种通信方式实现数据的读取，内带看门狗，工作稳定可靠。这里为了和HC-RS04模块通用，我们主要讲解GPIO方式。
由时序图可以知道，我们给模块的Trig一个10us的高电平即可触发模块内部发出信号，然后通过检测回响信号高电平的时间即可得到检测的距离。STM32的定时器具有输入捕获功能，使用STM32的定时器捕获功能可以更准确的检测出回响信号的高电平时间，具体驱动代码如下：
//定时器10通道1输入捕获配置
//arr：自动重装值(TIM10是16位的!!)
//psc：时钟预分频数
void Sonar2_TIM10_Init(u16 arr,u16 psc)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10,ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //速度100MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; //下拉
GPIO_Init(GPIOB,&GPIO_InitStructure);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource8,GPIO_AF_TIM10);
TIM_TimeBaseStructure.TIM_Prescaler=psc; //定时器分频
TIM_TimeBaseStructure.TIM_Period=arr; //自动重装载值
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM10,&TIM_TimeBaseStructure);
//初始化TIM10输入捕获参数
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; //CC1S=01 选择输入端 IC1映射到TI1上
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; //上升沿捕获
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; //映射到TI1上
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; //配置输入分频,不分频
TIM_ICInitStructure.TIM_ICFilter = 0x00;//IC1F=0000 配置输入滤波器 不滤波
TIM_ICInit(TIM10, &TIM_ICInitStructure);
TIM_ITConfig(TIM10,TIM_IT_Update|TIM_IT_CC1,ENABLE);//允许更新中断 ,允许CC1IE捕获中断
TIM_Cmd(TIM10,ENABLE ); //使能定时器5
NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_TIM10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
NVIC_InitStructure.NVIC_IRQChannelSubPriority =0; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器、
}
void Sonar_2_TRIG_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
//GPIO初始化设置
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_ResetBits(GPIOE,GPIO_Pin_0);
}
void Sonar_2_TRIG_Enabled(void)
{
GPIO_SetBits(GPIOE,GPIO_Pin_0);
delay_us(20);
GPIO_ResetBits(GPIOE,GPIO_Pin_0);
}
//捕获状态
//[7]:0,没有成功的捕获;1,成功捕获到一次.
//[6]:0,还没捕获到低电平;1,已经捕获到低电平了.
//[5:0]:捕获低电平后溢出的次数(对于32位定时器来说,1us计数器加1,溢出时间:4294秒)
//定时器10中断服务程序
void TIM1_UP_TIM10_IRQHandler(void)
{
if((Soanr2_CAPTURE_STA&0X80)==0)//还未成功捕获
{
if(TIM_GetITStatus(TIM10, TIM_IT_Update) != RESET)//溢出
{
if(Soanr2_CAPTURE_STA&0X40)//已经捕获到高电平了
{
if((Soanr2_CAPTURE_STA&0X3F)==0X3F)//高电平太长了
{
Soanr2_CAPTURE_STA|=0X80; //标记成功捕获了一次
Soanr2_CAPTURE_VAL=0XFFFF;
}
else
Soanr2_CAPTURE_STA++;
}
}
if(TIM_GetITStatus(TIM10, TIM_IT_CC1) != RESET)//捕获1发生捕获事件
{
if(Soanr2_CAPTURE_STA&0X40) //捕获到一个下降沿
{
Soanr2_CAPTURE_STA|=0X80; //标记成功捕获到一次高电平脉宽
Soanr2_CAPTURE_VAL=TIM_GetCapture1(TIM10);//获取当前的捕获值.
TIM_OC1PolarityConfig(TIM10,TIM_ICPolarity_Rising); //CC1P=0 设置为上升沿捕获
}
else //还未开始,第一次捕获上升沿
{
Soanr2_CAPTURE_STA=0; //清空
Soanr2_CAPTURE_VAL=0;
Soanr2_CAPTURE_STA|=0X40; //标记捕获到了上升沿
TIM_Cmd(TIM10,DISABLE ); //关闭定时器10
TIM_SetCounter(TIM10,0);
TIM_OC1PolarityConfig(TIM10,TIM_ICPolarity_Falling); //CC1P=1 设置为下降沿捕获
TIM_Cmd(TIM10,ENABLE ); //使能定时器10
}
}
}
TIM_ClearITPendingBit(TIM10, TIM_IT_CC1|TIM_IT_Update); //清除中断标志位
}
/*
//获取超声波数据函数
///函数名称：GetSonarValue
//输入参数：Num 取平均值次数
//输入参数：SonaerID 0：使用超声波1和2 1：使用超声波1，2：使用超声波2
///返回值：SonarDate 超声波1和2的数据
*/
SonarDate GetSonarValue(uint8_t Num,uint8_t SonarID)
{
SonarDate DateS;
int i;
long long temp1 = 0,temp2 = 0;
Sonar_1_TRIG_Enabled();
Sonar_2_TRIG_Enabled();
for(i = 0;i<Num;i++)
{
if(SonarID == 0)
{
if(Soanr1_CAPTURE_STA&0X80) //成功捕获到了一次高电平
{
temp1=Soanr1_CAPTURE_STA&0X3F;
temp1*=0XFFFF; //溢出时间总和
temp1+=Soanr1_CAPTURE_VAL; //得到总的高电平时间
Soanr1MeanVal +=temp1 ;
temp1 = 0;
Soanr1_CAPTURE_STA=0; //开启下一次捕获
Sonar_1_TRIG_Enabled();
}
if(Soanr2_CAPTURE_STA&0X80) //成功捕获到了一次高电平
{
temp2=Soanr2_CAPTURE_STA&0X3F;
temp2*=0XFFFF; //溢出时间总和
temp2+=Soanr2_CAPTURE_VAL; //得到总的高电平时间
Soanr2MeanVal +=temp2 ;
temp2 = 0;
Soanr2_CAPTURE_STA=0; //开启下一次捕获
Sonar_2_TRIG_Enabled();
}
}
if(SonarID == 1)
{
if(Soanr1_CAPTURE_STA&0X80) //成功捕获到了一次高电平
{
temp1=Soanr1_CAPTURE_STA&0X3F;
temp1*=0XFFFF; //溢出时间总和
temp1+=Soanr1_CAPTURE_VAL; //得到总的高电平时间
Soanr1MeanVal +=temp1 ;
Soanr1_CAPTURE_STA=0; //开启下一次捕获
Sonar_1_TRIG_Enabled();
}
}
if(SonarID == 2)
{
if(Soanr2_CAPTURE_STA&0X80) //成功捕获到了一次高电平
{
temp2=Soanr2_CAPTURE_STA&0X3F;
temp2*=0XFFFF; //溢出时间总和
temp2+=Soanr2_CAPTURE_VAL; //得到总的高电平时间
Soanr2MeanVal +=temp2 ;
temp2 = 0;
Soanr2_CAPTURE_STA=0; //开启下一次捕获
Sonar_2_TRIG_Enabled();
}
}
delay(100);
}
switch(SonarID)
{
case 0:
DateS.Sonar1 = ((float)((Soanr1MeanVal/Num)/58.0));
DateS.Sonar2 = ((float)((Soanr2MeanVal/Num)/58.0));
break;
case 1:
DateS.Sonar1 = ((float)((Soanr1MeanVal/Num)/58.0));
DateS.Sonar2 = 0.0;
break;
case 2:
DateS.Sonar1 = 0.0;
DateS.Sonar2 = ((float)((Soanr2MeanVal/Num)/58.0));
break;
}
Soanr1MeanVal = 0;
Soanr2MeanVal = 0;
return DateS;
}只需要调用Sonar_2_TRIG_Enabled（）和 SonarDate GetSonarValue(uint8_t Num,uint8_t SonarID)函数即可获取超声波数据，为了把数据上传给ROS，我们需要定义一个消息类型和节点：
starrobot_msgs::Sonar raw_sonar_msg;
ros::Publisher raw_sonar_pub("sonar", &raw_sonar_msg);
void pub_sonar(void)
{
SonarDate GetSonarData;
if(is_sonar1 && is_sonar2)
{
GetSonarData = GetSonarValue(3,0);
raw_sonar_msg.sonar1 = GetSonarData.Sonar1/100.0;
raw_sonar_msg.sonar2 = GetSonarData.Sonar2/100.0;
}
else if(is_sonar1)
{
GetSonarData = GetSonarValue(3,1);
raw_sonar_msg.sonar1 = GetSonarData.Sonar1/100.0;
raw_sonar_msg.sonar2 = 0.0;
}
else if(is_sonar2)
{
GetSonarData = GetSonarValue(3,2);
raw_sonar_msg.sonar1 = 0.0;
raw_sonar_msg.sonar2 = GetSonarData.Sonar2/100.0;
}
if(is_sonar1 || is_sonar2)
{
raw_sonar_pub.publish(&raw_sonar_msg);
}
} 总结
超声波模块也还是用到了STM32的定时器，STM32定时器还有其他的功能，大家可以自行去了解。超声波数据获取就讲解到这里，如果你也在自己动手制作ROS机器人小车的话就扫描一下二维码进群把:
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
