STM32蓝牙控制循迹避障小车源代码——3.舵机、超声波测距模块
注意-所需模块：
US-015超声波模块
SG90舵机云台
接线：舵机超声波：
A1–P2.7
B8–Trig
B9–Echo
代码
所有的代码都是直接从工程里面复制的，实测是没有问题的。
参考文章： stm32 智能避障小车（二）之sg90
我这里再简单总结一下：
-
舵机：
橙色信号号线，红色5V，棕色GND;
舵机控制需要一个周期为20ms的方波，方波的高电平部分在0.5ms~2.5ms中，会转动一定的角度。
t=0.5ms——舵机转动0°
t=1.0ms——舵机转动45°
t=1.5ms——舵机转动90°
t=2.0ms——舵机转动135°
t=2.5ms——舵机转动180°
设置在转动90°时为舵机的正前方，这样就可以左右转动了。 -
代码思路：利用定时器输出一个占空比可调的PWM，且PWM周期为20ms
-
A1接舵机的橙色线
SG90.c
#include "SG90.H"
void SG90_PWM_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
//开启定时器2，GPIOA,AFIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_AFIO,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
//初始化A1
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_1;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
//初始化定时器2
TIM_TimeBaseInitStruct.TIM_Period=199;
TIM_TimeBaseInitStruct.TIM_Prescaler=7199;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_ClockDivision=0;
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);//PWM周期为：（7200*200）/72000000=0.02=20ms
//定时器2通道二初始化
TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//当计时器值小于比较器设定值时输出有效低电平；大于等于时输出有效高电平
TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_Low;//初试极性为Low
TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;
TIM_OC2Init(TIM2,&TIM_OCInitStruct);
TIM_OC2PreloadConfig(TIM2,TIM_OCPreload_Enable);
TIM_Cmd(TIM2,ENABLE);
}
正在学习STM32的同学可以跟着下面的步骤自己配置一下
SG90_PWM_Init()函数配置：
1.开启时钟，定时器2：
RCC_APB1PeriphClockCmd();
开启IO口时钟、AFIO:
RCC_APB2PeriphClockCmd();//GPIOA
2.初始化A1:
GPIO_Init();//复用推挽输出 AF_PP
3.定时器2初始化：
TIM_TimeBaseInit();//周期199，预分频系数7199，计数器模式 up,时钟分割 0
4.初始化定时器2的通道2：并使能定时器：
TIM_OC2Init();//模式：PWM1,极性：low，输出状态：ENABLE
TIM_OC2PreloadConfig();
TIM_Cmd();
SG90.h:
#ifndef __SG90_H
#define __SG90_H
#include "stm32f10x.h"
#include "delay.h"
#define SG90_Right_90 TIM_SetCompare2(TIM2,195)
#define SG90_Right_45 TIM_SetCompare2(TIM2,190)//右转45
#define SG90_Front TIM_SetCompare2(TIM2,185)//方向摆正,(15/200)*20ms=1.5ms
#define SG90_Left_45 TIM_SetCompare2(TIM2,180)
#define SG90_Left_90 TIM_SetCompare2(TIM2,175)//左转45
void SG90_PWM_Init(void);
#endif
想要测试舵机模块是否正确的可以自己在主函数里直接调用SG90_Front，SG90_Left_45，SG90_Right_45等，注意加上延迟。
接下来是超声波测距，这个程序是当时我花了最长时间才调试好的
参考资料：STM32的超声波测距程序
-
超声波模块工作原理：
采用IO口TRIG触发测距，需要给最少10us的高电平信呈；
模块自动发送8个40KHZ的方波，自动检测是否有信号返回；
有信号返回，通过IO口ECHO输出高电平，高电平持续时间就是超声波从发射到返回的时间；
测试距离=高电平持续时间*声速/2 -
我是用定时器计数来获取时间的。小伙伴最好已经熟练掌握STM32的定时器中断实验。代码的原理一模一样，看不懂代码的可以看看原子哥的视频讲解。我这里不再详细讲解了。
代码直接给你们
cj.c
#include "CJ.H"
#include "stm32f10x.h"
#include "usart.h"
#include "delay.h"
//配置中断优先级
void NVIC_Config(void)
{
NVIC_InitTypeDef NVIC_InitStruct;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
NVIC_InitStruct.NVIC_IRQChannel=TIM4_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=2;//抢占优先级
NVIC_InitStruct.NVIC_IRQChannelSubPriority=0;//响应优先级
NVIC_Init(&NVIC_InitStruct);
}
void CH_SR04_Init(u16 reload,u16 psr)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
//开启定时器4，GPIOB
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
//PB8 TRIG触发信号
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct); //PB8接TRIG
GPIO_ResetBits(GPIOB,GPIO_Pin_8);
//PB9 ECHO回响信号
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPD;
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_9;
GPIO_Init(GPIOB,&GPIO_InitStruct);
//定时器4初始化 设置分频系数
TIM_TimeBaseInitStruct.TIM_Period=reload;
TIM_TimeBaseInitStruct.TIM_Prescaler=psr;
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
TIM_ICInitStruct.TIM_Channel=TIM_Channel_4; //通道4
TIM_ICInitStruct.TIM_ICFilter=0x00; //不滤波
TIM_ICInitStruct.TIM_ICPolarity=TIM_ICPolarity_Rising; //捕获模式，这里设置为上升沿捕获
TIM_ICInitStruct.TIM_ICPrescaler=TIM_ICPSC_DIV1; //配置输入分频,不分频
TIM_ICInitStruct.TIM_ICSelection=TIM_ICSelection_DirectTI;
TIM_ICInit(TIM4,&TIM_ICInitStruct);
TIM_ITConfig(TIM4,TIM_IT_Update|TIM_IT_CC4,ENABLE);
NVIC_Config();
}
cj.h
#ifndef __CJ_H
#define __CJ_H
#include "stm32f10x.h"
#include "delay.h"
#include "sys.h"
#define uint unsigned int
#define TRIG_Send PBout(8)
#define ECHO_Receive PBout(9)
void CH_SR04_Init(u16 reload,u16 psr);//超声波模块配置
void NVIC_Config(void);//中断配置
#endif
- 我将中断溢出放在主函数里面了。
main.c
#include "stm32f10x.h"
#include "moter.h"
#include "delay.h"
#include "SG90.H"
#include "cj.h"
#include "usart.h"
#include "sys.h"
#include "stdio.h"
#define CLK_TIME 1.0/8000000.0
u8 TIM4CH4_CAPTURE_STA=0; //输入捕获状态
u16 TIM4CH4_CAPTURE_VAL; //输入捕获值
u8 cap_flag=0;
float length_res[5]; //用来存放测距结果
u32 time_cnt;
double long_chang;
float sensor(void)
{
PBout(8)=1; //拉高信号，作为触发信号
delay_us(20); //高电平信号超过10us
PBout(8)=0;
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_9)==RESET);
TIM_Cmd(TIM4,ENABLE);//回响信号到来，开启定时器计数
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_9)==SET);//回响信号消失
TIM_Cmd(TIM4,DISABLE);//关闭定时器
if(cap_flag)
{
time_cnt=(TIM4CH4_CAPTURE_STA&0x3f)*65536;
time_cnt+=TIM4CH4_CAPTURE_VAL;
//printf("%d\r\n",time_cnt);
long_chang=time_cnt*CLK_TIME*340/2.0*100.0;
//printf("%f cm\r\n",long_chang);
cap_flag=0;
TIM4CH4_CAPTURE_STA=0;
TIM4CH4_CAPTURE_VAL=0;
delay_ms(10);
}
return long_chang;
}
int main(void)
{
// NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置中断优先级分组为组2：2位抢占优先级，2位响应优先级
// uart_init(115200);
delay_init(); //延时初始化
TIM3_PWM_Init(); //电机pwm TIM3
SG90_PWM_Init(); //舵机初始化
CH_SR04_Init(0xffff,8); //超声波定时器初始化
TIM_Cmd(TIM4,ENABLE);//开启超声波测距定时器
while(1)
{
SG90_Front;
delay_ms(700);
length_res[0]=sensor();
delay_ms(10);
//printf("fornt:%f\r\n",length_res[0]);
if(length_res[0]>40.0000) //如果前方距离大于40cm 前进
{
Forward();
}
else if(length_res[0]<40.0000) //如果前方距离小于40厘米 停车测左右距离
{
STOP();
SG90_Left_45; //舵机左转45度测距
delay_ms(700);
length_res[1] =sensor(); //把测量结果放进数组
delay_ms(10);
//printf("left:%fcm left45 \r\n",length_res[1]);
SG90_Right_45; //舵机右转45度测距
delay_ms(700);
length_res[4] =sensor(); //把测量结果放进数组
delay_ms(10);
//printf("right:%fcm right 45\r\n",length_res[4]);
SG90_Front; //舵机摆正
//delay_ms(700);
if(length_res[1]<40.00&&length_res[4]<40.00&&length_res[1]>length_res[4])
{
SG90_Front; //舵机摆正
delay_ms(700);
length_res[0] =sensor(); //重复测前方的距离同时
delay_ms(10);
Backward();
delay_ms(700);
Turn_left();
// delay_ms(500);
}
if(length_res[1]<40.00&&length_res[4]<40.00&&length_res[1]<length_res[4])
{
SG90_Front; //舵机摆正
delay_ms(700);
length_res[0] =sensor(); //重复测前方的距离同时
delay_ms(10);
Backward();
delay_ms(700);
Turn_right();
// delay_ms(500);
}
if(length_res[1]>length_res[4]) //如果左边的距离大于右边的距离
{
SG90_Front; //舵机摆正
delay_ms(700);
length_res[0] =sensor(); //重复测前方的距离同时左转
delay_ms(10);
Turn_left();
if(length_res[0]>35.0000)
Forward();
}
if(length_res[1]<length_res[4]) //如果右边的距离大于左边的距离
{
SG90_Front;
delay_ms(700);
length_res[0] =sensor(); //重复测前方的距离同时右转
delay_ms(10);
Turn_right();
if(length_res[0]>35.0000)
Forward();
}
}
}
}
void TIM4_IRQHandler(void)//中断溢出
{
if((TIM4CH4_CAPTURE_STA&0x80)==0)
{
if(TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{ //判断定时器是否溢出
if(TIM4CH4_CAPTURE_STA&0x40)//判断之前是否已经接收到上升沿
{
if((TIM4CH4_CAPTURE_STA&0X3F)==0X3F) //如果计数已经满了 直接停止
{
//cap_flag=1;
TIM4CH4_CAPTURE_STA|=0x80;
TIM4CH4_CAPTURE_VAL=0XFFFF;
}
else TIM4CH4_CAPTURE_STA++; //如果计数没满，继续计数
}
}
}
if(TIM_GetITStatus(TIM4, TIM_IT_CC4) != RESET)
{ //表示捕获到来上升沿
if(TIM4CH4_CAPTURE_STA&0x40)//表明之前已经接收到了一个边沿（这一次捕获边沿表示捕获结束）
{
cap_flag=1;
TIM4CH4_CAPTURE_STA|=0x80;
TIM4CH4_CAPTURE_VAL=TIM_GetCapture4(TIM4); //读取捕获的值
TIM_OC4PolarityConfig(TIM4,TIM_ICPolarity_Rising); //重新设置为上升沿捕获
}
else
{ //表示首次进入捕获边沿
cap_flag=0;
TIM4CH4_CAPTURE_STA=0;
TIM4CH4_CAPTURE_VAL=0;
TIM4CH4_CAPTURE_STA|=0X40; //标记为捕获了一次上升沿
TIM_SetCounter(TIM4,0);
TIM_OC4PolarityConfig(TIM4,TIM_ICPolarity_Falling);
}
}
TIM_ClearITPendingBit(TIM4, TIM_IT_CC4|TIM_IT_Update); //清除中断标志位
}
将以上程序写入工程里，下载到板子上，实验现象为：
舵机摆正，不断测量前方的距离；
当前方距离大于40cm时，小车前进；
当前方距离小于40cm时，停车；舵机左右摆，测两边的距离，同时判断情况。
? ---------当左右两边都小于40cm，但左边距离大于右边时，后退，然后左转。
? --------- 当左右两边都小于40cm，但右边距离大于左边时，后退，然后右转。
? ---------当左边距离大于右边时，左转。
? ---------当左边距离小于右边时，右转。
任意时刻，当前方距离大于40cm时，小车前进。
想要观察测量距离的同学，可以把我主函数里的串口通信的代码取消注释，在串口调试助手里面可以随时看到测试的数据。（这些都是我自己调试过程中保留的代码，我没有删掉。你们也可以根据实际小车的运动来改一下参数）
代码配置不成功的伙伴，可以直接下载这个工程，全部实测过没有问题：
STM32小车-超声波避障-超声波测距.zip-嵌入式文档类资源-CSDN文库
下一节写蓝牙控制程序