两路超声波模块代码示例基于stm32f407

两路超声波模块代码示例

US-015.H
#ifndef _US015_H
#define _US015_H
#include "stm32f4xx.h"
#define Echo PAin(2)
#define Trig PAout(4)
#define Trig1 PAout(5)
#define Echo1 PAin(0)

void MY_TIME5_INIT(u32 arr);
void MY_TIME4_INIT(u32 arr);
void Us_015_Init(void);
void Distance_read(void);
void Distance1_read(void);
extern u8 TIM5CH1_CAPTURE_STA;
extern u32 TIM5CH1_CAPTURE_VAL;
extern u32 TIME_us[2];
#endif
US-015.C
#include "my_stm32xx.h"
//#include "led.h"
//u8 TIM5CH1_CAPTURE_STA=0;//用于检测定时器定时是否准确1s自增1
u32 TIM5CH1_CAPTURE_VAL=0;//计时10us用于检测高电平时间
u32 TIM5CH1_CAPTURE_VAL1=0;
u32 TIME_us[2];//用于储存此次高电平持续时间
void MY_TIME5_INIT(u32 arr)
{
	TIM_TimeBaseInitTypeDef	TIM_TimeBaseInitSTRUCT;
	NVIC_InitTypeDef	NVIC_InitSTRUCT;
	GPIO_InitTypeDef	GPIO_InitStruct;
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5,ENABLE);
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN;
	GPIO_InitStruct.GPIO_OType=GPIO_OType_PP;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_2;
	GPIO_InitStruct.GPIO_PuPd=GPIO_PuPd_DOWN;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_100MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_OUT;
	GPIO_InitStruct.GPIO_OType=GPIO_OType_PP;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_4;
	GPIO_InitStruct.GPIO_PuPd=GPIO_PuPd_DOWN;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_100MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseInitSTRUCT.TIM_Period=arr;
	TIM_TimeBaseInitSTRUCT.TIM_Prescaler=83;//1US
	TIM_TimeBaseInitSTRUCT.TIM_CounterMode=TIM_CounterMode_Down;
	TIM_TimeBaseInitSTRUCT.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInit(TIM5,&TIM_TimeBaseInitSTRUCT);
	TIM_ITConfig(TIM5,TIM_IT_Update,ENABLE);
	
	NVIC_InitSTRUCT.NVIC_IRQChannel=TIM5_IRQn;
	NVIC_InitSTRUCT.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitSTRUCT.NVIC_IRQChannelPreemptionPriority=2;
	NVIC_InitSTRUCT.NVIC_IRQChannelSubPriority=2;
	NVIC_Init(&NVIC_InitSTRUCT);
	TIM_Cmd(TIM5,DISABLE);
}
void MY_TIME4_INIT(u32 arr)
{
	TIM_TimeBaseInitTypeDef	TIM_TimeBaseInitSTRUCT;
	NVIC_InitTypeDef	NVIC_InitSTRUCT;
	GPIO_InitTypeDef	GPIO_InitStruct;
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN;
	GPIO_InitStruct.GPIO_OType=GPIO_OType_PP;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0;
	GPIO_InitStruct.GPIO_PuPd=GPIO_PuPd_DOWN;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_100MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_OUT;
	GPIO_InitStruct.GPIO_OType=GPIO_OType_PP;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5;
	GPIO_InitStruct.GPIO_PuPd=GPIO_PuPd_DOWN;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_100MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseInitSTRUCT.TIM_Period=arr;
	TIM_TimeBaseInitSTRUCT.TIM_Prescaler=83;//1US
	TIM_TimeBaseInitSTRUCT.TIM_CounterMode=TIM_CounterMode_Down;
	TIM_TimeBaseInitSTRUCT.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitSTRUCT);
	TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
	
	NVIC_InitSTRUCT.NVIC_IRQChannel=TIM4_IRQn;
	NVIC_InitSTRUCT.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitSTRUCT.NVIC_IRQChannelPreemptionPriority=2;
	NVIC_InitSTRUCT.NVIC_IRQChannelSubPriority=1;
	NVIC_Init(&NVIC_InitSTRUCT);
	TIM_Cmd(TIM4,DISABLE);
}
void Distance_read(void)
{
	TIM5CH1_CAPTURE_VAL=0;
	TIM_Cmd(TIM4,DISABLE);
	TIM_ClearFlag(TIM4,TIM_IT_Update);
	TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
	while(Echo==0);
	TIM_Cmd(TIM4,ENABLE);
	while(Echo==1);
	TIM_Cmd(TIM4,DISABLE);
	TIME_us[0]=TIM5CH1_CAPTURE_VAL;
	TIM_ClearFlag(TIM4,TIM_IT_Update);
	TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
}
void Distance1_read(void)
{
	TIM5CH1_CAPTURE_VAL1=0;
	TIM_Cmd(TIM5,DISABLE);
	TIM_ClearFlag(TIM5,TIM_IT_Update);
	TIM_ClearITPendingBit(TIM5,TIM_IT_Update);
	while(Echo1==0);
	TIM_Cmd(TIM5,ENABLE);
	while(Echo1==1);
	TIM_Cmd(TIM5,DISABLE);
	TIME_us[1]=TIM5CH1_CAPTURE_VAL1;
	TIM_ClearFlag(TIM5,TIM_IT_Update);
	TIM_ClearITPendingBit(TIM5,TIM_IT_Update);
}
void TIM5_IRQHandler(void)
{ 	
	if(TIM_GetITStatus(TIM5,TIM_IT_Update)!=RESET)
	TIM5CH1_CAPTURE_VAL1++;
	TIM_ClearITPendingBit(TIM5,TIM_IT_Update);
}
void TIM4_IRQHandler(void)
{ 	
	if(TIM_GetITStatus(TIM4,TIM_IT_Update)!=RESET)
	TIM5CH1_CAPTURE_VAL++;
	TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
}
KALMAN.H
#ifndef _KALMAN_H
#define _KALMAN_H
#include "stm32f4xx.h"
typedef struct{
	float Xk;
	float Xk_1;
	float Dk;
	
	float kg;
	float Pk;
	float Pk_1;
	float R;
	float Cov;
}KALMANDATA;
extern KALMANDATA Kalmandata1[2];
extern KALMANDATA Kalmandata2[2];
extern u16 distance[2];
void kalman_init(KALMANDATA *Kalmandata,float dat,float p,float r);
float kalmanCalc(KALMANDATA *Kalmandata,float dat);
u16 Distance1_CM_get(void);
void Distance_CM_get(void);
#endif
KALMAN.C
#include "kalman.h"
#include "math.h"
#include "us015.h"
#include "delay.h"
KALMANDATA Kalmandata1[2];
KALMANDATA Kalmandata2[2];
u16 distance[2];
void kalman_init(KALMANDATA *Kalmandata,float dat,float p,float r)
{
	Kalmandata->Xk_1=dat;
	Kalmandata->Pk_1=p;
	Kalmandata->R=r*r;
}
float kalmanCalc(KALMANDATA *Kalmandata,float dat)
{
	Kalmandata->Dk=dat;
	Kalmandata->Cov=_sqrt(Kalmandata->Pk_1*Kalmandata->Pk_1+Kalmandata->R);
	Kalmandata->kg=sqrt(Kalmandata->Cov*Kalmandata->Cov/(Kalmandata->Cov*Kalmandata-	>Cov+Kalmandata->R));
	Kalmandata->Pk=sqrt((1-Kalmandata->kg)*Kalmandata->R);
	Kalmandata->Xk=Kalmandata->Xk_1+Kalmandata->kg*(Kalmandata->Dk-Kalmandata->Xk_1);
	
	Kalmandata->Xk_1=Kalmandata->Xk;
	Kalmandata->Pk_1=Kalmandata->Pk;
	return Kalmandata->Xk;
}
void Distance_CM_get(void)
{
		u8 i;
		Distance_read();
		delay_us(500);
		Distance1_read();
		for(i=0;i<2;i++)
		{
			kalmanCalc(&Kalmandata1[i],TIME_us[i]);
			distance[i]=((float)(Kalmandata1[i].Xk*34))/1000;
			kalmanCalc(&Kalmandata2[i],distance[i]);
			distance[i]=Kalmandata2[i].Xk;
		}
}

MAIN.C
#include "us015.h"
#include "led.h"
#include "lcd.h"
#include "kalman.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "kalman.h"
#include "my_tim.h"
#include "my_usart.h"
#include "my_math.h"
u16 distance_line[100]={0};
int main(void)
{ 
 	u16 x=0;
	u8 lcd_id[12];//存放LCD ID字符串
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
	delay_init(168);      //初始化延时函数
	uart_init(115200);		//初始化串口波特率为115200
	MY_TIME4_INIT(1);
	MY_TIME5_INIT(1);//2us定时
	MY_TIME3_INIT(9);//定时1ms
	LED_Init();					  //初始化LED
 	LCD_Init();           //初始化LCD FSMC接口
	POINT_COLOR=RED;      //画笔颜色：红色
	sprintf((char*)lcd_id,"LCD ID:%04X",lcddev.id);//将LCD ID打印到lcd_id数组。
	Distance_CM_get();
	kalman_init(&Kalmandata1[0],distance[0],4,3);	
	kalman_init(&Kalmandata2[0],2,4,3);
	kalman_init(&Kalmandata1[1],distance[1],4,3);	
	kalman_init(&Kalmandata2[1],2,4,3);
  	while(1) 
	{	
		x++;
		LCD_DrawRectangle(28,35,228,65);
		LCD_ShowString(30,40,210,24,24,"Liu Wei's Car");
		LCD_ShowString(30,70,90,16,16,"Time_us:");		
		LCD_ShowNum(110,70,TIME_us[0],num_len_get(TIME_us[0]),16); 
		LCD_ShowString(30,90,90,16,16,"Distance:");	
		LCD_ShowNum(110,90,distance[0],num_len_get(distance[0]),16); 	
		LCD_ShowString(30,110,90,16,16,"Time_us[1]:");		
		LCD_ShowNum(115,110,TIME_us[1],num_len_get(TIME_us[1]),16); 
		LCD_ShowString(30,130,90,16,16,"Distance[1]:");	
		LCD_ShowNum(115,130,distance[1],num_len_get(distance[1]),16); 			
 		//LCD_ShowString(30,110,200,16,16,lcd_id);		//显示LCD ID	      					 	
		Distance_CM_get();
		//distance1=Distance1_CM_get();
		VisualScope_Output(distance[0],distance[1],0,0);
		if(x==5){LCD_Clear(WHITE);x=0;}
	} 
}
注意事项：两个超声波模块的测量要分两次进行，两次之间要有一定的延时
注：
void MY_TIME3_INIT(u16 arr)//arr=9
{
	TIM_TimeBaseInitTypeDef	TIM_TimeBaseInitSTRUCT;
	NVIC_InitTypeDef	NVIC_InitSTRUCT;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	TIM_TimeBaseInitSTRUCT.TIM_Period=arr;
	TIM_TimeBaseInitSTRUCT.TIM_Prescaler=8400-1;
	TIM_TimeBaseInitSTRUCT.TIM_CounterMode=TIM_CounterMode_Down;
	TIM_TimeBaseInitSTRUCT.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitSTRUCT);
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);
	
	NVIC_InitSTRUCT.NVIC_IRQChannel=TIM3_IRQn;
	NVIC_InitSTRUCT.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitSTRUCT.NVIC_IRQChannelPreemptionPriority=2;
	NVIC_InitSTRUCT.NVIC_IRQChannelSubPriority=2;
	NVIC_Init(&NVIC_InitSTRUCT);
	TIM_Cmd(TIM3,ENABLE);
}
void TIM3_IRQHandler()
{
	static u16 TIM3_count,TIM3_count1;
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)!=RESET)
	{
		TIM3_count++;
		TIM3_count1++;
		if(TIM3_count==1){
			Trig=1;
			delay_us(15);
			Trig=0;
		}
		if(TIM3_count==10)
		{
			Trig1=1;
			delay_us(15);
			Trig1=0;
		}
		else {Trig=0;Trig1=0;}
		if(TIM3_count==70)TIM3_count=0;//70ms发送一次
		if(TIM3_count1==1000){LED1=!LED1;TIM3_count1=0;}
		}
		TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}