两路超声波模块代码示例
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(Echo0);
TIM_Cmd(TIM4,ENABLE);
while(Echo1);
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(Echo10);
TIM_Cmd(TIM5,ENABLE);
while(Echo11);
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=rr;
}
float kalmanCalc(KALMANDATA Kalmandata,float dat)
{
Kalmandata->Dk=dat;
Kalmandata->Cov=_sqrt(Kalmandata->Pk_1Kalmandata->Pk_1+Kalmandata->R);
Kalmandata->kg=sqrt(Kalmandata->CovKalmandata->Cov/(Kalmandata->CovKalmandata- >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_count1){
Trig=1;
delay_us(15);
Trig=0;
}
if(TIM3_count10)
{
Trig1=1;
delay_us(15);
Trig1=0;
}
else {Trig=0;Trig1=0;}
if(TIM3_count70)TIM3_count=0;//70ms发送一次
if(TIM3_count11000){LED1=!LED1;TIM3_count1=0;}
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}
