基于STM32的智能小车

爱学电子的刻刻帝

已于 2024-08-11 00:57:11 修改

阅读量1.2k

点赞数 11

文章标签： stm32 嵌入式硬件单片机智能小车 PID算法

于 2024-08-11 00:57:08 首次发布

本文链接：https://blog.csdn.net/m0_74676415/article/details/140989176

版权

简介

小车包含如下功能：PWM驱动电机、编码器测速、PID速度控制、OLED显示、循迹、手机遥控、超声波避障和跟随、MPU6050指定轨迹、扩展排针（方便后续扩展）、摄像头模块（可以搭配openmv）、FreeRTOS定时器（我们保留着，之后我会扩展）。

硬件部分

材料选购

电源

1、12v锂电池组18650充电带保护板大容量电瓶通用移动电源便携蓄电池。

小车重要结构类似三轮或四轮结构均可

1、常用M3螺丝包螺母 3*5 3*8 3*10 3*12 3*12 圆头平头螺丝。

2、常用M3 铜柱包 M3*5 6 8 10 12 15 +6 双通 M3螺母螺丝。

3、STM32小车车体亚克力板。（亚克力板要能安装对应电机和万向轮）

4、JGA25-370直流减速电机。

5、真空W420 万向轮或者是牛眼轮滚珠万向球CY-15-25A型号:CY-12A(全碳钢）

线材

1、XH2.54 3P 单双头电子线红白排线长20CM。

2、XH2.54 4P 单头电子线红白排线长20CM。

3、杜邦线母对母公对母公对公40P彩色排线连接线10/15/30/20/40CM。

PCB元件

1、XH2.54接插件 2.54mm 。

2、XH2.54接插件 2.54mm 。

3、单排针间距2.54MM 。（买长的，短的直接锯）

4、间距2.54MM 单排直插母座。（买长的，短的直接锯）

5、DC-005电源座 DC电源插座5.5*2.1MM DC2.1。

6、滑动开关插件大电流拨动开关 SS-12D10L2.5。

7、0603封装20K电阻和0603封装10K电阻。

8、跳线帽短路帽间距2.54MM。

电子元件

1、USB转TTL CH340模块 USB转串口 .

2、ST-LINK V2 STM8/STM32仿真器编程器。

3、DC-DC 3A迷你降压模块车载电源6V9V12V-30V转3.3V/5V输出。

4、A4950直流有刷电机驱动板模块双路电机驱动模块。

5、STM32f103c8t6最小系统板。

6、OLED显示屏模块 0.96寸 IIC/SPI 。

7、HC-SR04 超声波测距模块。

8、寻迹传感器 TCRT5000红外反射传感器。

9、HC-05 主从机一体蓝牙串口透传模块。

10、MPU-6050模块三轴加速度陀螺仪6DOF GY-521

原理图以及PCB

结构与组装

焊接电子元件

插上各个模块

将板子用铜柱固定在小车底盘，并把模块的线接好

软件部分

按键

PB4--KEY1 单片机设置下拉输入 - 、上降沿触发

PA12--KEY2 单片机设置上拉输入、下降沿触发

原理图

开始配置

主要代码

void HAL_GPIO_EXTI_Callback ( uint16_t GPIO_Pin )

{

        if ( GPIO_Pin == KEY1_Pin ){ // 判断一下那个引脚触发中断

        //这里编写触发中断后要执行的程序

        HAL_GPIO_TogglePin ( LED_GPIO_Port , LED_Pin ); // 切换 LED GPIO 状态

        }

        if ( GPIO_Pin == KEY2_Pin ){ // 判断一下那个引脚触发中断

        //这里编写触发中断后要执行的程序

        HAL_GPIO_TogglePin ( LED_GPIO_Port , LED_Pin ); // 切换 LED GPIO 状态

        }

}

PWM控制电机

认识PWM

原理图

PWM配置

根据我们小车原理图我们知道是 PA11 和 PA8 两个引脚要设置为 PWM 输出

主要代码

PWM输出的配置就已经完成了，但是不能输出产生 PWM 波，因为 Cube 在生成代码时，有很多外设初始化完后默认是关闭的，需要我们手动开启。

HAL_TIM_PWM_Start ( & htim1 , TIM_CHANNEL_1 ); // 开启定时器 1 通道 1 PWM 输出

HAL_TIM_PWM_Start ( & htim1 , TIM_CHANNEL_4 ); // 开启定时器 1 通道 4 PWM 输出

我们可以使用这个宏来修改占空比

__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 40);

__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 40);

电机驱动和PWM

认识电机驱动

项目使用电机驱动芯片为A4950、下面是电机驱动的相关介绍

我们按照这种使用方法

这我们使用一个图介绍

使用电机驱动(独立工程)

分析和编写代码

综合电机使用方法、 C8T6 单片机硬件资源、小车原理图我们要进行如下配置

PA11-TIM1_CH4 定时器 PWM 输出 -PWMA 前面已经完成

PB13-GPIO 输出 -AIN1

PA8-TIM1_CH1 定时器 PWM 输出 -PWMB 前面已经完成

PB3-GPIO 输出 -BIN1

还有两个管脚没有初始化

主要代码

电机正反转测试

HAL_GPIO_WritePin ( AIN1_GPIO_Port , AIN1_Pin , GPIO_PIN_RESET ); // 设置 AIN1 PB13 为低

电平

HAL_GPIO_WritePin ( BIN1_GPIO_Port , BIN1_Pin , GPIO_PIN_SET ); // 设置 BIN1 PB3 为高电

平

HAL_Delay ( 1000 );

// 两次会使得电机反向。

HAL_GPIO_WritePin ( AIN1_GPIO_Port , AIN1_Pin , GPIO_PIN_SET ); // 设置 AIN1 PB13 为高电

平

HAL_GPIO_WritePin ( BIN1_GPIO_Port , BIN1_Pin , GPIO_PIN_RESET ); // 设置 BIN1 PB3 为低

电平

编写电机转速开环控制函数

/*******************

* @brief 设置两个电机转速和方向

* @param motor1: 电机 B 设置参数、 motor2: 设置参数

* @param motor1: 输入 1~100 对应控制 B 电机正方向速度在 1%-100% 、输入 -1~-100 对应控制 B 电机

反方向速度在 1%-100% 、 motor2 同理

* @return 无

*

*******************/

void Motor_Set ( int motor1 , int motor2 )

{

        //根据参数正负设置选择方向

        if ( motor1 < 0 ) BIN1_SET ;

        else BIN1_RESET ;

        if ( motor2 < 0 ) AIN1_SET ;

        else AIN1_RESET ;

        //motor1 设置电机 B 的转速

        if ( motor1 < 0 )

        {

                if ( motor1 < - 99 ) motor1 = - 99 ; // 超过 PWM 幅值

                //负的时候绝对值越小 PWM 占空比越大

                //现在的motor1 -1 -99

                //给寄存器或者函数 99 1

                __HAL_TIM_SET_COMPARE ( & htim1 , TIM_CHANNEL_1 , ( 100 + motor1 )); // 修改定时器 1通道1 PA8 Pulse改变占空比

        }

        else

         {

                if ( motor1 > 99 ) motor1 = 99 ;

        //现在是 0 1 99

        //我们赋值 0 1 99

        __HAL_TIM_SET_COMPARE ( & htim1 , TIM_CHANNEL_1 , motor1 ); // 修改定时器 1 通道 1 PA8 Pulse改变占空比

        }

        //motor2 设置电机 A 的转速

        if ( motor2 < 0 )

        {

                if ( motor2 < - 99 ) motor2 = - 99 ; // 超过 PWM 幅值

                //负的时候绝对值越小 PWM 占空比越大

                //现在的motor2 -1 -99

                //给寄存器或者函数 99 1

                __HAL_TIM_SET_COMPARE ( & htim1 , TIM_CHANNEL_4 , ( 100 + motor2 )); // 修改定时器 1 通道4 PA11 Pulse 改变占空比

        }

        else

         {

                if ( motor2 > 99 ) motor2 = 99 ;

                //现在是 0 1 99

                //我们赋值 0 1 99

                __HAL_TIM_SET_COMPARE ( & htim1 , TIM_CHANNEL_4 , motor2 ); // 修改定时器 1 通道 4 PA11 Pulse改变占空比

        }

}

然后我们连接电机主函数进行测试

HAL_Delay ( 500 );

Motor_Set ( 0 , 0 );

编码器测速

认识编码器

编码器 : 一般按照电机尾部、用于测量电机转速、方向、位置。

那么编码器的输出信号具体是什么？我们如何根据输出信号测量转速和方向？

转速: 单位时间测量到的脉冲数量(比如根据每秒测量到多少个脉冲来计算转速)。

旋转方向: 两通道信号的相对电平关系。

单片机定时器的编码器功能

那么我们已经知道编码器输出的波形，我们如何通过单片机读取波形，然后计算出速度那?

这里STM32 单片机的定时器和通用定时器具有 编码器接口模式 、在 STM32 中文参考手册 13 章中有详细介绍。

STM32中文参考手册 - 第 200 页

这个是计数方向与编码器信号的关系、我们拆开来看

仅在TI1计数、电机正转、对原始数据二倍频

仅在TI1计数、电机反转、对原始数据二倍频

在TI1和TI2都计数

可以看到这样就对原始数据四倍频

计数方向

获得单位时间计数器值变化量

这里很巧妙，因为如果编码器的方向如果是反的，那么16位数的CNT计数器就会变成63335，之后不断的减1，然后变成short类型的数据之后，那么就会变成-1，我们就只要转换一下数据类型，就可以知道编码器的方向。（但是切记，不能溢出，但是也不需要考虑，电机不可能转的能这么快，超过32768）

测变化量有两种方法：

方法一：

这次编码器计数值 = 计数器值+计数溢出次数 * 计数最大器计数最大值

计数器两次变化值 = 这次编码器计数值 - 上次编码器计数值

然后根据这个单位变化量计算速度

方法二：

计数器变化量 = 当前计数器值

每次计数值清空

然后根据这个变化量计算速度

然后我们再看具体到哪一款电机和编码器上如何测速

我们采用方法二，因为他的转速不足以让计数器溢出。

设置TIM2

设置TIM2滤波器

同理设置TIM4。

设置引脚上拉

生成代码

开启定时器和定时中断

HAL_TIM_Encoder_Start ( & htim2 , TIM_CHANNEL_ALL ); // 开启定时器 2

HAL_TIM_Encoder_Start ( & htim4 , TIM_CHANNEL_ALL ); // 开启定时器 4

HAL_TIM_Base_Start_IT ( & htim2 ); // 开启定时器 2 中断

HAL_TIM_Base_Start_IT ( & htim4 ); // 开启定时器 4 中断

在定义两个变量保存计数器值

short Encoder1Count = 0 ; // 编码器计数器值

short Encoder2Count = 0 ;

每2ms读取计数器值-> 清零计数器

Motor_Set ( 0 , 0 );

//1. 保存计数器值

Encoder1Count = ( short ) __HAL_TIM_GET_COUNTER ( & htim4 );

Encoder2Count = ( short ) __HAL_TIM_GET_COUNTER ( & htim2 );

//2. 清零计数器值

__HAL_TIM_SET_COUNTER ( & htim4 , 0 );

__HAL_TIM_SET_COUNTER ( & htim2 , 0 );

printf ( "Encoder1Count:%d\r\n" , Encoder1Count );

printf ( "Encoder2Count:%d\r\n" , Encoder2Count );

HAL_Delay ( 2 );

接好电池、烧录代码、串口一连接电脑

用手转动电机 1 或者电机 2 、串口助手可以看到输出信息了

主函数周期测量转速

上面我们测量出来了溢出值，我们再根据当前计数器值就可以测量出计数器变化量，我们通过单位时间变量就可以计算出转速

下面是电机和编码器的参数

我们先测试的结论是否有问题？

1. 编码器计数器会不会在计数时间内溢出？

2. 车轮旋转一周，单片机编码器计数器计数多少？ 9.6 乘 11 乘 4

3. 根据计算方法计算电机转速

//定义两个 float 变量

float Motor1Speed = 0.00 ;

float Motor2Speed = 0.00 ;

// 下面是代码 ( 一定要把主函数没有用的删除掉 )

// 计算速度

Motor1Speed = ( float ) Encode1Count * 100 / 9.6 / 11 / 4 ;

Motor2Speed = ( float ) Encode2Count * 100 / 9.6 / 11 / 4 ;

printf ( "Motor1Speed:%.2f\r\n" , Motor1Speed );

printf ( "Motor2Speed:%.2f\r\n" , Motor2Speed );

编译烧录代码就会输出结果

结论：前面算计数器变换值没有问题，我转了1/4圈，读出来也挺精准。实际上，按照公式算也不可能溢出。假设每秒能转n圈，那么就会有n*9.6*11个脉冲，我们计数值允许我们有32768个脉冲，我们十毫秒读一次，也只会读到n*9.6*11个脉冲，算一下，每秒你得转32768*100/9.6/11/4=7757圈，不是哥们，你要超过宇宙第一速度啊。

定时器中断定时测量速度

上面我们实现: 在主函数周期，读取计数器值然后计算速度，但是如果函数加入其他内容这个周期时间就很难保证。

设置内部时钟源

使能自动重装载

开启定义更新中断

代码开启定时器1 中断

HAL_TIM_Base_Start_IT(&htim1); //开启定时器1 中断

定时器回调函数中添加速度计算内容

/*******************

* @brief 定时器回调函数

* @param

* @return

*

*******************/

void HAL_TIM_PeriodElapsedCallback ( TIM_HandleTypeDef * htim )

{

        if ( htim == & htim1 ) //htim1 500HZ 2ms 中断一次

        {

                TimerCount ++ ;

                if ( TimerCount % 5 == 0 ) // 每 10ms 执行一次

                {

                        Encode1Count = ( short ) __HAL_TIM_GET_COUNTER ( & htim4 );

                        Encode2Count = ( short ) __HAL_TIM_GET_COUNTER ( & htim2 );

                        __HAL_TIM_SET_COUNTER ( & htim4 , 0 );

                        __HAL_TIM_SET_COUNTER ( & htim2 , 0 );

                        Motor1Speed = ( float ) Encode1Count * 100 / 9.6 / 11 / 4 ;

                        Motor2Speed = ( float ) Encode2Count * 100 / 9.6 / 11 / 4 ;

                        TimerCount = 0 ;

                }

        }

}

主函数就输出速度大小就可以了

printf ( "Motor1Speed:%.2f\r\n" , Motor1Speed );

printf ( "Motor2Speed:%.2f\r\n" , Motor2Speed );

然后打开串口助手

PID-速度控制

准备工作-匿名上位机曲线显示速度波形方便观察数据

为了方便观察电机速度数据，我们通过上位机曲线显示一下，我使用的版本是 : 匿名上位机 V7.2.2.8版本，匿名上位机官方下载链接

我们要把 STM32数据发送到匿名上位机，就要满足匿名上位机的数据协议要求，这里有个大佬的，可以去看看

CSDN 慕羽★ 大佬写的协议解析教程博客 : https://blog.csdn.net/qq_44339029/article/details/106

004997

1. 先补充一下大小端模式

这是因为在计算机系统中，我们是以字节为单位的，每个地址单元都对应着一个字节，一个字节为 8bit。但是在 C 语言中除了 8bit 的 char 之外，还有 16bit 的 short 型， 32bit 的 long 型（要看具体的编译器），另外，对于位数大于8 位的处理器，例如 16 位或者 32 位的处理器，由于寄存器宽度大于一个字节，那么必然存在着一个如和将多个字节安排的问题 。因此就导致了大端存储模式和小端存储模式。例如一个16bit 的 short 型 x ，在内存中的地址为 0x0010 ， x 的值为 0x1122 ，那么 0x11 为高字节， 0x22 为低字节。对于大端模式，就将0x11 放在低地址中，即 0x0010 中， 0x22 放在高地址中，即 0x0011 中。

所谓的大端模式（BE big-endian ），是指数据的低位保存在内存的高地址中，而数据的高位，保 存在内存的低地址中（低对高，高对低）；

所谓的小端模式（LE little-endian ），是指数据的低位保存在内存的低地址中，而数据的高位保 存在内存的高地址中（低对低，高对高）。

常见的 单片机 大小端模式：（ 1 ） KEIL C51 中，变量都是大端模式的，而 KEIL MDK 中，变量是小 端模式的。（ 2 ） SDCC-C51 是小端寻址， AVRGCC 小端寻址 . （ 3 ） PC 小端，大部分 ARM 是小端 （ 4 ）总起来说 51 单片机一般是大端模式， 32单片机一般是小端模式。

2. 看一下上位机要求的协议

灵活格式帧(用户自定义帧)

前面我们好理解

0xAA: 一个字节表示开始

0xFF: 一个字节表示目标地址

0xF1: 一个字节表示发送功能码

1-40 ：一个字节表示数据长度

数据内容有多个字节如何发送

因为串口每次发送一个字节，但是数据可能是int16_t 16 位的数据，或者 int32_t 32 位数据，每次发送 16 位数据 , 先发送数据低八位，还是先发送数据高八位那 ?

匿名协议通信介绍给出：DATA 数据内容中的数据，采用小端模式传送，低字节在前，高字节在后。

那么就要求，比如我们在发送16 位数据 0x2314 我们要先发送低字节 0x14, 然后发送高字节 0x23。

那么如何解析出低字节或者高字节，就需要知道多字节数据在单片机里面是怎么存的，因为STM32 是小端存储，所以低字节就在低位地址中，高字节高位地址中。

如果使用32 单片机小端模式， 0x23 高地址， 0x14 在低地址，所以我们要先发低地址，再发高地址。

下面就是对16位数据，或者32位数据的拆分。

// 需要发送 16 位 ,32 位数据，对数据拆分，之后每次发送单个字节

// 拆分过程：对变量 dwTemp 去地址然后将其转化成 char 类型指针，最后再取出指针所指向的内容

#define BYTE0(dwTemp) (*(char *)(&dwTemp))

#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))

#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))

#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))

拆分后我们按照协议要求发送数据就可以了

uint8_t data_to_send[100];
//通过F1帧发送4个uint16类型的数据
void ANO_DT_Send_F1(uint16_t _a, uint16_t _b, uint16_t _c, uint16_t _d)
{
   uint8_t _cnt = 0; //计数值
   uint8_t sumcheck = 0; //和校验
   uint8_t addcheck = 0; //附加和校验
   uint8_t i = 0;
   data_to_send[_cnt++] = 0xAA;//帧头
   data_to_send[_cnt++] = 0xFF;//目标地址
   data_to_send[_cnt++] = 0xF1;//功能码
   data_to_send[_cnt++] = 8; //数据长度
   //单片机为小端模式-低地址存放低位数据，匿名上位机要求先发低位数据，所以先发低地址
   data_to_send[_cnt++] = BYTE0(_a);
   data_to_send[_cnt++] = BYTE1(_a);
   data_to_send[_cnt++] = BYTE0(_b);
   data_to_send[_cnt++] = BYTE1(_b);
   data_to_send[_cnt++] = BYTE0(_c);
   data_to_send[_cnt++] = BYTE1(_c);
   data_to_send[_cnt++] = BYTE0(_d);
   data_to_send[_cnt++] = BYTE1(_d);
   for ( i = 0; i < data_to_send[3]+4; i++)
   {
       sumcheck += data_to_send[i];//和校验
       addcheck += sumcheck;//附加校验
   }
   data_to_send[_cnt++] = sumcheck;
   data_to_send[_cnt++] = addcheck;
   HAL_UART_Transmit(&huart1,data_to_send,_cnt,0xFFFF);//这里是串口发送函数
}
//通过F2帧发送4个int16类型的数据
void ANO_DT_Send_F2(int16_t _a, int16_t _b, int16_t _c, int16_t _d) //F2帧 4个int16 参数
{
   uint8_t _cnt = 0;
   uint8_t sumcheck = 0; //和校验
   uint8_t addcheck = 0; //附加和校验
   uint8_t i=0;
   data_to_send[_cnt++] = 0xAA;
   data_to_send[_cnt++] = 0xFF;
   data_to_send[_cnt++] = 0xF2;
   data_to_send[_cnt++] = 8; //数据长度
   //单片机为小端模式-低地址存放低位数据，匿名上位机要求先发低位数据，所以先发低地址
   data_to_send[_cnt++] = BYTE0(_a);
   data_to_send[_cnt++] = BYTE1(_a);
   data_to_send[_cnt++] = BYTE0(_b);
   data_to_send[_cnt++] = BYTE1(_b);
   data_to_send[_cnt++] = BYTE0(_c);
   data_to_send[_cnt++] = BYTE1(_c);
   data_to_send[_cnt++] = BYTE0(_d);
   data_to_send[_cnt++] = BYTE1(_d);
   for ( i = 0; i < data_to_send[3]+4; i++)
   {
       sumcheck += data_to_send[i];
       addcheck += sumcheck;
   }
   data_to_send[_cnt++] = sumcheck;
   data_to_send[_cnt++] = addcheck;
   HAL_UART_Transmit(&huart1,data_to_send,_cnt,0xFFFF);//这里是串口发送函数
}
//通过F3帧发送2个int16类型和1个int32类型的数据
void ANO_DT_Send_F3(int16_t _a, int16_t _b, int32_t _c ) //F3帧 2个 int16 参数1个 int32 参数
{
   uint8_t _cnt = 0;
   uint8_t sumcheck = 0; //和校验
   uint8_t addcheck = 0; //附加和校验
   uint8_t i=0;
   data_to_send[_cnt++] = 0xAA;
   data_to_send[_cnt++] = 0xFF;
   data_to_send[_cnt++] = 0xF3;
   data_to_send[_cnt++] = 8; //数据长度
   //单片机为小端模式-低地址存放低位数据，匿名上位机要求先发低位数据，所以先发低地址
   data_to_send[_cnt++] = BYTE0(_a);
   data_to_send[_cnt++] = BYTE1(_a);
   data_to_send[_cnt++] = BYTE0(_b);
   data_to_send[_cnt++] = BYTE1(_b);
   data_to_send[_cnt++] = BYTE0(_c);
   data_to_send[_cnt++] = BYTE1(_c);
   data_to_send[_cnt++] = BYTE2(_c);
   data_to_send[_cnt++] = BYTE3(_c);
   for ( i = 0; i < data_to_send[3]+4; i++)
   {
   sumcheck += data_to_send[i];
   addcheck += sumcheck;
   }
   data_to_send[_cnt++] = sumcheck;
   data_to_send[_cnt++] = addcheck;
   HAL_UART_Transmit(&huart1,data_to_send,_cnt,0xFFFF);//这里是串口发送函数
}

添加测试代码

// 电机速度等信息发送到上位机

// 注意上位机不支持浮点数，所以要乘 100

ANO_DT_Send_F2 ( Motor1Speed * 100 , 3.0 * 100 , Motor2Speed * 100 , 3.0 * 100 );

下面设置上位机-数据解析

P I D 逐个参数理解

加入的现在过去未来概念

p:现在

i:过去

d:未来

一句话就是，我们会根据我们需求，然后结合现在的状态，不断调试出合适的PID参数，看看哪些参数最适合我们的要求。

开始写PID

PID 的结构体类型变量、里面成员都是浮点类型

先在 pid.h 声明一个结构体类型、声明 .c 中的函数

#ifndef __PID_H

#define __PID_H

// 声明一个结构体类型

typedef struct

{

        float target_val ; // 目标值

        float actual_val ; // 实际值

        float err ; // 当前偏差

        float err_last ; // 上次偏差

        float err_sum ; // 误差累计值

        float Kp , Ki , Kd ; // 比例，积分，微分系数

} tPid ;

// 声明函数

float P_realize ( tPid * pid , float actual_val );

void PID_init ( void );

float PI_realize ( tPid * pid , float actual_val );

float PID_realize ( tPid * pid , float actual_val );

#endif

然后在 pid.c 中定义结构体类型变量

//给结构体类型变量赋初值
void PID_init()
{
   pid1_speed.actual_val=0.0;
   pid1_speed.target_val=0.00;
   pid1_speed.err=0.0;
   pid1_speed.err_last=0.0;
   pid1_speed.err_sum=0.0;
   pid1_speed.Kp=15;
   pid1_speed.Ki=5;
   pid1_speed.Kd=0;

   pid2_speed.actual_val=0.0;
   pid2_speed.target_val=0.00;
   pid2_speed.err=0.0;
   pid2_speed.err_last=0.0;
   pid2_speed.err_sum=0.0;
   pid2_speed.Kp=15;
   pid2_speed.Ki=5;
   pid2_speed.Kd=0;


}
//比例p调节控制函数
float P_realize(tPid * pid,float actual_val)
{
   pid->actual_val = actual_val;//传递真实值
   pid->err = pid->target_val - pid->actual_val;//当前误差=目标值-真实值
   //比例控制调节输出=Kp*当前误差
   pid->actual_val = pid->Kp*pid->err;
   return pid->actual_val;
}
//比例P 积分I 控制函数
float PI_realize(tPid * pid,float actual_val)
{
   pid->actual_val = actual_val;//传递真实值
   pid->err = pid->target_val - pid->actual_val;//当前误差=目标值-真实值
   pid->err_sum += pid->err;//误差累计值 = 当前误差累计和
   //使用PI控制输出=Kp*当前误差+Ki*误差累计值
   pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->err_sum;
   return pid->actual_val;
}
// PID控制函数
float PID_realize(tPid * pid,float actual_val)
{
   pid->actual_val = actual_val;//传递真实值
   pid->err = pid->target_val - pid->actual_val;当前误差=目标值-真实值
   pid->err_sum += pid->err;//误差累计值 = 当前误差累计和
   //使用PID控制输出 = Kp*当前误差 + Ki*误差累计值 + Kd*(当前误差-上次误差)
   pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->err_sum + pid->Kd*(pid->err - pid->err_last);
   //保存上次误差: 这次误差赋值给上次误差
   pid->err_last = pid->err;
   return pid->actual_val;
}

这里解释一下三个函数

①PI算法：
特点：

从时域上看，只要存在偏差，积分就会不停对偏差积累，因此稳态时误差一定为零
不足：比例与积分动作都是对过去控制误差进行操作，不对未来控制误差进行预测，限制了控制性能。
PI调节将比列调节的快速反应与积分调节消除静差的特点结合，实现好的调节效果。
PI调节适用于控制通道滞后较小、负荷变化不大、工艺参数不允许有静差的系统。
②PD算法：
适用于舵机快速响应。

对于惯性较大的对象，常常希望能加快控制速度，此时可增加微分作用。

特点：

比例控制对于惯性较大对象，控制过程缓慢，控制品质不佳。比例微分控制可提高控制速度，对惯性较大对象，可改善控制质量，减小偏差，缩短控制时间。
理想微分作用持续时间太短，执行器来不及响应。实际使用中，一般加以惯性延迟，称为实际微分。
PD 调节以比例调节为主，微分调节为辅，PD调节是有差调节。
PD 调节具有提高系统稳定性、抑制过渡过程最大动态偏差的作用。
PD 调节有利于提高系统响应速度。
PD 调节抗干扰能力差，一般只能应用于被调参数变化平稳的生产过程。
微分作用太强时，容易造成系统振荡。
③PID算法：
将比例、积分、微分三种调节规律结合在一起，只要三项作用的强度配合适当，既能快速调节，又能消除余差，可得到满意的控制效果。

特点：

PID 控制作用中，比例作用是基础控制；微分作用是用于加快系统控制速度；积分作用是用于消除静差。

只要比例、积分、微分三种控制规律强度配合适当，既能快速调节，又能消除余差，可得到满意控制效果。

Kp 较小时，系统对微分和积分环节的引入较为敏感，积分会引起超调，微分可能会引起振荡，而振荡剧烈的时候超调也会增加。

Kp 增大时，积分环节由于滞后产生的超调逐渐减小，此时如果想要继续减少超调可以适当引入微分环节。继续增大 Kp 系统可能会不太稳定，因此在增加 Kp 的同时引入 Kd 减小超调，可以保证在 Kp 不是很大的情况下也能取得较好的稳态特性和动态性能。

Kp 较小时，积分环节不宜过大，Kp 较大时积分环节也不宜过小（否则调节时间会非常地长），当使用分段PID ，在恰当的条件下分离积分，可以取得更好的控制效果。原因在于在稳态误差即将满足要求时，消除了系统的滞后。因此系统超调会明显减少。

然后在 main 中要调用 PID_init(); 函数

PID_init ();

p 调节函数 函数只根据当前误差进行控制

// 比例 p 调节控制函数

float P_realize ( tPid * pid , float actual_val )

{

        pid -> actual_val = actual_val ; // 传递真实值

        pid -> err = pid -> target_val - pid -> actual_val ; // 目标值减去实际值等于误差值

        //比例控制调节

        pid -> actual_val = pid -> Kp * pid -> err ;

        return pid -> actual_val ;

}

主函数 - 可以估算当 p=10 就有较好的响应速度

先看根据 p 比例控制的效果

p 调节电机稳态后还是存在误差。

下面加入 i 调节也就是加入历史误差

pi 的控制函数

// 比例 P 积分 I 控制函数

float PI_realize ( tPid * pid , float actual_val )

{

        pid -> actual_val = actual_val ; // 传递真实值

        pid -> err = pid -> target_val - pid -> actual_val ; // 目标值减去实际值等于误差值

        pid -> err_sum += pid -> err ; // 误差累计求和

        //使用 PI 控制

        pid -> actual_val = pid -> Kp * pid -> err + pid -> Ki * pid -> err_sum ;

        return pid -> actual_val ;

}

因为实际值 1.6 的时候误差为 1.4 上次偏差 1.4 和这次偏差 1.4 相加 2.8 我们乘 5 等于 10 点多就会有较好控制效果

这是 pi 调节的控制效果

下面是 PID 调节的

// PID 控制函数

float PID_realize ( tPid * pid , float actual_val )

{

        pid -> actual_val = actual_val ; // 传递真实值

        pid -> err = pid -> target_val - pid -> actual_val ; // 目标值减去实际值等于误差值

        pid -> err_sum += pid -> err ; // 误差累计求和

        //使用 PID 控制

        pid -> actual_val = pid -> Kp * pid -> err + pid -> Ki * pid -> err_sum + pid -> Kd * ( pid -> err - pid -> err_last );

        //保存上次误差 : 最近一次赋值给上次

        pid -> err_last = pid -> err ;

        return pid -> actual_val ;

}

所谓PID控制，就是要根据理论，每个参数具体起什么作用，然后结合自己实际情况，不断调试，直到选出最好的参数，这就是我们所谓的PID调控。

加入cJSON方便上位机调参

调大堆栈

软件开启中断

开启接收中断

__HAL_UART_ENABLE_IT ( & huart1 , UART_IT_RXNE ); // 开启串口 1 接收中断

中断回调函数

uint8_t Usart1_ReadBuf [ 256 ]; // 串口 1 缓冲数组

uint8_t Usart1_ReadCount = 0 ; // 串口 1 接收字节计数

if ( __HAL_UART_GET_FLAG ( & huart1 , UART_FLAG_RXNE )) // 判断 huart1 是否读到字节

{

if ( Usart1_ReadCount >= 255 ) Usart1_ReadCount = 0 ;

HAL_UART_Receive ( & huart1 , & Usart1_ReadBuf [ Usart1_ReadCount ++ ], 1 , 1000 );

}

编写函数用于判断串口是否发送完一帧数据

extern uint8_t Usart1_ReadBuf [ 255 ]; // 串口 1 缓冲数组

extern uint8_t Usart1_ReadCount ; // 串口 1 接收字节计数

// 判断否接收完一帧数据

uint8_t Usart_WaitReasFinish ( void )

{

        static uint16_t Usart_LastReadCount = 0 ; // 记录上次的计数值

        if ( Usart1_ReadCount == 0 )

        {

                Usart_LastReadCount = 0 ; return 1 ; // 表示没有在接收数据

        }

        if ( Usart1_ReadCount == Usart_LastReadCount ) // 如果这次计数值等于上次计数值

        {

                Usart1_ReadCount = 0 ;

                Usart_LastReadCount = 0 ;

                return 0 ; // 已经接收完成了

        }

        Usart_LastReadCount = Usart1_ReadCount ;

        return 2 ; // 表示正在接受中

}

然后我们把 cJSON 库放入工程里面

下载 cJSON 新版

gtihub 链接 : https://github.com/DaveGamble/cJSON

和添加其他文件一样，加入工程，然后指定路径

编写解析指令的函数

#include "cJSON.h"

#include <string.h>

cJSON * cJsonData , * cJsonVlaue ;

if ( Usart_WaitReasFinish () == 0 ) // 是否接收完毕

{

         cJsonData = cJSON_Parse (( const char * ) Usart1_ReadBuf );

        if ( cJSON_GetObjectItem ( cJsonData , "p" ) != NULL )

        {

                cJsonVlaue = cJSON_GetObjectItem ( cJsonData , "p" );

                p = cJsonVlaue -> valuedouble ;

                pidMotor1Speed . Kp = p ;

        }

        if ( cJSON_GetObjectItem ( cJsonData , "i" ) != NULL )

        {

                cJsonVlaue = cJSON_GetObjectItem ( cJsonData , "i" );

                i = cJsonVlaue -> valuedouble ;

                pidMotor1Speed . Ki = i ;

        }

        if ( cJSON_GetObjectItem ( cJsonData , "d" ) != NULL )

        {

                cJsonVlaue = cJSON_GetObjectItem ( cJsonData , "d" );

                d = cJsonVlaue -> valuedouble ;

                pidMotor1Speed . Kd = d ;

        }

        if ( cJSON_GetObjectItem ( cJsonData , "a" ) != NULL )

        {

                cJsonVlaue = cJSON_GetObjectItem ( cJsonData , "a" );

                a = cJsonVlaue -> valuedouble ;

                pidMotor1Speed . target_val = a ;

        }

        if ( cJsonData != NULL )

        {

                cJSON_Delete ( cJsonData ); // 释放空间、但是不能删除 cJsonVlaue 不然会出现异常错误

        }

        memset ( Usart1_ReadBuf , 0 , 255 ); // 清空接收 buf ，注意这里不能使用 strlen

}

printf ( "P:%.3f I:%.3f D:%.3f A:%.3f\r\n" , p , i , d , a );

测试发送cJSON数据就会解析收到数据

然后我们赋值改变一个电机的 PID 参数和目标转速

然后我们通过串口发送命令，就会改变 PID 的参数了

不断进行调试，知道调出合适自己的参数即可。

PID整定方法

正如之前所说，现在我们PID 控制函数是在主函数中循环调用，这样的调用方式并不能保证实时性，不能保证周期得到调用。

所以我们要把PID 控制函数放到中断里面定时执行，那么如何放到中断里面执行，执行的周期是多少合适？

烧录测试一下，是否可以改变波形和调整参数

借助上位机调节 PID

在PID控制中，P、I、D各个参数分别代表比例（Proportional）、积分（Integral）、微分（Derivative），它们分别起到以下作用：

1.比例控制（P）：

2.比例项的作用是根据当前偏差的大小来产生一个控制输出。P参数用于调节系统的稳定性和响应速度。增大P参数可以减小超调量，但可能会加大稳定时间。
3.P参数的作用是使系统对误差的响应更为迅速，但会增加系统对误差的抖动和震荡。

4.积分控制（I）：

5.积分项的作用是减小稳态误差，通过对积分累积的偏差进行补偿来消除静差。I参数用于调节系统的静态稳定性，确保系统最终稳定在目标值附近。
6.I参数的作用是消除系统的稳态误差，对于存在恒定偏差的系统，采用I参数可以更好地调节系统。

7.微分控制（D）：

8.微分项的作用是根据误差变化率来预测未来的系统状态，通过预测系统的趋势来提前调整控制输出。D参数用于抑制超调和稳定系统的过程。
9.D参数的作用是稳定系统的过渡过程，减小超调量，减缓系统的振荡速度，提高系统的稳定性。

综合来看，P、I、D三个参数在PID控制中协同工作，通过不同比例的调节，可以使系统达到稳定、响应迅速且准确的控制效果。在实际应用中，根据系统的特性和要求，需要合理调节这三个参数以实现最佳的控制效果。

1. 调节 P 把 I=0 、 D=0 先给正值或负值值测试 P 正负、然后根据 PID 函数输入和输出估算 P 大小，然后I=0 D=0去测试，调节一个较大值。

2. 调节 I 把 P 等于前面的值然后测试 I 给较大正值和负值测试出 I 正负，然后 I 从小值调节，直到没有偏差存在。

3. 一般系统不使用 D。

然后当前系统特点 :I 对于系统更重要

下面我们调节 I

给一个较小的 i 发现有一个大的超调，我们就减少 p 、减小一半 p

下面是减少一半 p 的效果

这个效果还可以

整理双电机速度控制

首先我们的需要是控制两个电机，那么这两个电机的特点不同，他们的P I D 参数不同，要控制不同的目标速度，那么他们的目标值、实际值、偏差等都会不同，所以我们的PID 函数就要能够根据输入参数控制

电机我们增加tPid 类型函数的定义用于控制电机。

tPid pid1_speed ; // 电机 1 的转速控制

tPid pid2_speed ; // 电机 2 的转速控制

// 初始化 PID 参数

void PID_init ()

{

pid1_speed . actual_val = 0.0 ; // 初始化电机 1 转速 PID 结构体

pid1_speed . target_val = 0.0 ;

pid1_speed . err = 0.0 ;

pid1_speed . err_last = 0.0 ;

pid1_speed . err_sum = 0.0 ;

pid1_speed . Kp = 0.0 ;

pid1_speed . Ki = 0.0 ;

pid1_speed . Kd = 0.0 ;

pid2_speed . actual_val = 0.0 ; // 初始化电机 2 转速 PID 结构体

pid2_speed . target_val = 0.0 ;

pid2_speed . err = 0.0 ;

pid2_speed . err_last = 0.0 ;

pid2_speed . err_sum = 0.0 ;

pid2_speed . Kp = 0.0 ;

pid2_speed . Ki = 0.0 ;

pid2_speed . Kd = 0.0 ;

}

更改一下 PID 函数，这里我们使用结构体作为函数地址

访问因为是地址，访问结构体变量要用 ->

更改定时器回调函数，对PID函数的使用

       if(Timer2Count %10 ==0)//每20ms一次
       {                 Motor_Set(PID_realize(&pid1_speed,Motor1Speed),PID_realize(&pid2_speed,Motor2Speed));
           Timer2Count=0;
       }

然后可以分别调节电机 1 的参数和电机二的参数

把测试好的 PID 参数分别写在 PID_init 里面

以上是入门篇

通过上面的学习与实操，大家对 :PWM 、电机驱动、 PID 闭环控制、串口通信等有了一定掌握。

后面的内容就是偏应用比较简单了。

小车跑一跑

如何实现小车的前、后、左、右、停

控制电机速度就可以控制小车运动。

如何控制电机速度？

改变小车速度 PID 的目标值，然后定时器里面的 PID 控制函数就会计算输占空比然后控制小车。

代码如下 :

定时器里面有电机控制，我们这里还增加

Motor_Set(PID_realize(&pidMotor1Speed,Motor1Speed),PID_realize(&pidMotor2Speed,Motor2Speed));

是为了提高实时性。

/*******************

* @brief 通过 PID 控制电机转速

* @param Motor1Speed: 电机 1 目标速度、 Motor2Speed: 电机 2 目标速度

* @return 无

*

*******************/

void motorPidSetSpeed ( float Motor1SetSpeed , float Motor2SetSpeed )

{

        //改变电机 PID 参数的目标速度

        pidMotor1Speed . target_val = Motor1SetSpeed ;

        pidMotor2Speed . target_val = Motor2SetSpeed ;

        //根据 PID 计算输出作用于电机

        Motor_Set ( PID_realize ( & pidMotor1Speed , Motor1Speed ), PID_realize ( & pidMotor2Speed , Motor2Speed ));

}

很容易得到一下控制方式

// motorPidSetSpeed(1,2);// 向右转弯

// motorPidSetSpeed(2,1);// 向左转弯

// motorPidSetSpeed(1,1);// 前进

// motorPidSetSpeed(-1,-1);// 后退

// motorPidSetSpeed(0,0);// 停止

加速减速函数

// 向前加速函数

void motorSpeedUp ( void )

{

        static float MotorSetSpeedUp = 0.5 ; // 静态变量函数结束变量不会销毁

        if ( MotorSetSpeedUp <= MAX_SPEED_UP ) MotorSetSpeedUp += 0.5 ; // 如果没有超过最大值就增加0.5

        motorPidSetSpeed ( MotorSetSpeedUp , MotorSetSpeedUp ); // 设置到电机

}

// 向前减速函数

void motorSpeedCut ( void )

{

        static float MotorSetSpeedCut = 3 ; // 静态变量函数结束变量不会销毁

        if ( MotorSetSpeedCut >= 0.5 ) MotorSetSpeedCut -= 0.5 ; // 判断是否速度太小

        motorPidSetSpeed ( MotorSetSpeedCut , MotorSetSpeedCut ); // 设置到电机

}

OLED速度与历程显示

这节我们显示两轮速度和里程

两轮速度很简单之前已经计算过，那么如何计算里程那？

里程 : 小车行驶的路程长度。

这里我们只要计算出每个单位时间小车行驶的长度然后一直相加，就是这一段时间行驶的总里程长度

了。

我们 20ms 计算一次 ,20ms 走过了多少距离，然后一直相加，就是走的总距离，就是里程。这里我们使用

使用电机 1 车轮 1 进行计算。你也可以电机 1 和电机 2 相加然后除 2 。

/* 里程数 (cm) += 时间周期（ s ） * 车轮转速 ( 转 /s)* 车轮周长 (cm)*/

Mileage += 0.02 * Motor1Speed * 22 ;

然后主函数我们通过OLED显示电机速度和小车里程

sprintf (( char * ) OledString , "V1:%.2fV2:%.2f" , Motor1Speed , Motor2Speed ); // 显示两个电机速度

OLED_ShowString ( 0 , 0 , OledString , 12 ); // 这个是 oled 驱动里面的，是显示位置的一个函数

sprintf (( char * ) OledString , "Mileage:%.2f " , Mileage ); // 显示里程数

OLED_ShowString ( 0 , 1 , OledString , 12 ); // 这个是 oled 驱动里面的，是显示位置的一个函数

ADC采集电压和显示

什么是 ADC

百度百科介绍：

我们知道万用表电压表可以测量电池，或者电路电压。那么我们是否可以通过单片机获得电压，方便我们监控电池状态。

如何测量我们的锂电池电压那？锂电池电压 12V 左右，单片机 ADC 最大测量电压 3.3V ，这里我们需要分压电路分压。

然后我们通过电阻分压，显而易见 ADC 点的电压是 VBAT_IN 的五分之一

1. 软件初始化一下 ADC 。

2. 然后注意调长一点采样时间、这样精度才会更高一点

在adc.c文件添加ADC相关函数

/*******************

* @brief 电池电压测量计算函数

* @param 无

* @return 小车电池电压

*

*******************/

float adcGetBatteryVoltage ( void )

{

        HAL_ADC_Start ( & hadc2 ); // 启动 ADC 转化

        if ( HAL_OK == HAL_ADC_PollForConversion ( & hadc2 , 50 )) // 等待转化完成、超时时间 50ms

        return ( float ) HAL_ADC_GetValue ( & hadc2 ) / 4096 * 3.3 * 5 ; // 计算电池电压

        return - 1 ;

}

在main中调用显示函数显示电压

sprintf (( char* ) OledString , "U:%.2fV" , adcGetBatteryVoltage ());

OLED_ShowString ( 0 , 2 , OledString , 12 ); // 这个是 oled 驱动里面的，是显示位置的一个函数，

循迹功能

非PID循迹功能完成

先红外对管调试

我们这里学习一下，如何实现循迹功能。

如何才能让小车沿着黑线运动、要让小车感知到黑线的位置，使用这种传感器就可以反馈黑线是否存在。

根据传感器特性，我们检测红外对管 DO 引脚的电压就可以知道，下面有没有黑线。

DO 高电平 -> 有黑线小灯灭。

DO 低电平 -> 没有黑线小灯亮。

这是好多地方对这个产品的说明。

下面我们通过单片机读取红外对管DO口的电压，就知道黑线在小车下面的位置了。

STM32 初始化

先看原理图需要初始化那些引脚

重新生成

然后我们在 gpio.h 添加读取 GPIO 的宏，使得程序更简洁。

#define READ_HW_OUT_1 HAL_GPIO_ReadPin(HW_OUT_1_GPIO_Port,HW_OUT_1_Pin) // 读取红外对管GPIO 电平 #define READ_HW_OUT_2 HAL_GPIO_ReadPin(HW_OUT_2_GPIO_Port,HW_OUT_2_Pin)

#define READ_HW_OUT_3 HAL_GPIO_ReadPin(HW_OUT_3_GPIO_Port,HW_OUT_3_Pin)

#define READ_HW_OUT_4 HAL_GPIO_ReadPin(HW_OUT_4_GPIO_Port,HW_OUT_4_Pin)

根据红外对管状态控制电机速度

注意：整个主函数不要加入延时，这样实时性更高，可以根据红外对管状态做出及时控制。

if ( READ_HW_OUT_1 == 0 && READ_HW_OUT_2 == 0 && READ_HW_OUT_3 == 0 && READ_HW_OUT_4 == 0 )

{

        printf ( " 应该前进 \r\n" );

        motorPidSetSpeed ( 1 , 1 ); // 前运动

}

if ( READ_HW_OUT_1 == 0 && READ_HW_OUT_2 == 1 && READ_HW_OUT_3 == 0 && READ_HW_OUT_4 == 0 )

{

        printf ( " 应该右转 \r\n" );

        motorPidSetSpeed ( 0.5 , 2 ); // 右边运动

}

if ( READ_HW_OUT_1 == 1 && READ_HW_OUT_2 == 0 && READ_HW_OUT_3 == 0 && READ_HW_OUT_4 == 0 )

{

        printf ( " 快速右转 \r\n" );

        motorPidSetSpeed ( 0.5 , 2.5 ); // 快速右转

}

if ( READ_HW_OUT_1 == 0 && READ_HW_OUT_2 == 0 && READ_HW_OUT_3 == 1 && READ_HW_OUT_4 == 0 )

{

        printf ( " 应该左转 \r\n" );

        motorPidSetSpeed ( 2 , 0.5 ); // 左边运动

}

if ( READ_HW_OUT_1 == 0 && READ_HW_OUT_2 == 0 && READ_HW_OUT_3 == 0 && READ_HW_OUT_4 == 1 )

{

        printf ( " 快速左转 \r\n" );

        motorPidSetSpeed ( 2.5 , 0.5 ); // 快速左转

}

然后测试

1. 测试红外对管灵敏度，放在有黑线的地上或者纸上，然后把小车黑线比如放到最右边及第一个红外对管，观察红外对管小灯变化情况和串口输出情况，如果小灯没有灭，就调节红外对管灵敏度和室内灯光，直到每个红外对管都可以感应到小灯。

2. 然后在黑线上让小车循迹

然后放到地上

加入循迹PID

我们这里规定小车位于循迹路上有5个位置（因为我们有四个红外对管），这样子我们可以生成七个状态。（快速左转x2、快速右转x2、左转、右转、前进）

前面的代码我们对循迹是判断的几个状态，然后PID 控制电机不同速度，但是我们可以使用红外对管状态作为PID 控制的输入然后再控制电机。

PID的输入是红外对管状态，我们设计 PID输入是红外对管的状态、然后输出一个速度值，然后左右电机去加或者减这个值，就可以完成根据红外对管输入对电机的差速控制。这里就需要自己取摸索了，自己慢慢调参。

主函数添加：

extern tPid pidHW_Tracking ; // 红外循迹的 PID

uint8_t g_ucaHW_Read [ 4 ] = { 0 }; // 保存红外对管电平的数组

int8_t g_cThisState = 0 ; // 这次状态

int8_t g_cLastState = 0 ; // 上次状态

float g_fHW_PID_Out ; // 红外对管 PID 计算输出速度

float g_fHW_PID_Out1 ; // 电机 1 的最后循迹 PID 控制速度

float g_fHW_PID_Out2 ; // 电机 2 的最后循迹 PID 控制速度

然后实现 PID 循迹控制、注意为了更加快，要减少没有必要的程序和优化判断、将没有必要的输出都注释掉。

       g_ucaHW_Read[0] = READ_HW_OUT_1;//读取红外对管状态、这样相比于写在if里面更高效
       g_ucaHW_Read[1] = READ_HW_OUT_2;
       g_ucaHW_Read[2] = READ_HW_OUT_3;
       g_ucaHW_Read[3] = READ_HW_OUT_4;

       if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )
       {
           g_cThisState = 0;//前进
       }
       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )//使用else if更加合理高效
       {
           g_cThisState = -1;//应该右转
       }
       else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )
       {
           g_cThisState = -2;//快速右转
       }
       else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0)
       {
           g_cThisState = -3;//快速右转
       }
       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 0 )
       {
           g_cThisState = 1;//应该左转
       }
       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 1 )
       {
           g_cThisState = 2;//快速左转
       }
       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 1)
       {
           g_cThisState = 3;//快速左转
       }
       g_fHW_PID_Out = PID_realize(&pidHW_Tracking,g_cThisState);//PID计算输出目标速度这个速度，会和基础速度加减
       g_fHW_PID_Out1 = 3 + g_fHW_PID_Out;//电机1速度=基础速度+循迹PID输出速度
       g_fHW_PID_Out2 = 3 - g_fHW_PID_Out;//电机1速度=基础速度-循迹PID输出速度
       if(g_fHW_PID_Out1 >5) g_fHW_PID_Out1 =5;//进行限幅限幅速度在0-5之间
       if(g_fHW_PID_Out1 <0) g_fHW_PID_Out1 =0;
       if(g_fHW_PID_Out2 >5) g_fHW_PID_Out2 =5;//进行限幅限幅速度在0-5之间
       if(g_fHW_PID_Out2 <0) g_fHW_PID_Out2 =0;
       if(g_cThisState != g_cLastState)//如何这次状态不等于上次状态、就进行改变目标速度和控制电机、在定时器中依旧定时控制电机
       {
           motorPidSetSpeed(g_fHW_PID_Out1,g_fHW_PID_Out2);//通过计算的速度控制电机
       }
       g_cLastState = g_cThisState;//保存上次红外对管状态

在pid.中

pidHW_Tracking . actual_val = 0.0 ;

pidHW_Tracking . target_val = 0.00 ; // 红外循迹 PID 的目标值为 0

pidHW_Tracking . err = 0.0 ;

pidHW_Tracking . err_last = 0.0 ;

pidHW_Tracking . err_sum = 0.0 ;

pidHW_Tracking . Kp =- 1.50 ;

pidHW_Tracking . Ki = 0 ;

pidHW_Tracking . Kd = 0.80 ;

然后就可以跑一下试试了。

可以改进的地方

1. 红外对管影响差速转向，也影响基础直行的速度，会有更好控制效果，所以可以加入每种红外对管状态下对基础速度的影响。

2. 红外对管的数量越多，效果会越好。

手机遥控功能

我们要实现蓝牙遥控功能，蓝牙遥控功能要使用 :1. 单片机的串口、 2. 蓝牙通信模块

所以我们先调试好 : 单片机的串口 -> 蓝牙模块 -> 接到一起联调

电脑控制小车

完成功能:电脑连接单片机串口三控制小车前进后退

通过原理图可以看出这是使用的串口3 在使用的时候注意把跳线帽，跳线到蓝牙通信位置。

打开初始化软件

生成代码

在 main 定义全局变量

uint8_t g_ucUsart3ReceiveData ; // 保存串口三接收的数据

开启串口三中断接收

HAL_UART_Receive_IT ( & huart3 , & g_ucUsart3ReceiveData , 1 ); // 串口三接收数据

然后我们可以在中断回调函数里面中编写遥控命令控制逻辑了

// 串口接收回调函数

void HAL_UART_RxCpltCallback ( UART_HandleTypeDef * huart )

{

        if ( huart == & huart3 ) // 判断中断源

        {

                if ( g_ucUsart3ReceiveData == 'A' ) motorPidSetSpeed ( 1 , 1 ); // 前运动

                if ( g_ucUsart3ReceiveData == 'B' ) motorPidSetSpeed ( - 1 , - 1 ); // 后运动

                if ( g_ucUsart3ReceiveData == 'C' ) motorPidSetSpeed ( 0 , 0 ); // 停止

                if ( g_ucUsart3ReceiveData == 'D' ) motorPidSetSpeed ( 1 , 2 ); // 右边运动

                if ( g_ucUsart3ReceiveData == 'E' ) motorPidSetSpeed ( 2 , 1 ); // 左边运动

                if ( g_ucUsart3ReceiveData == 'F' ) motorPidSpeedUp (); // 加速

                if ( g_ucUsart3ReceiveData == 'G' ) motorPidSpeedCut (); // 减速

                HAL_UART_Receive_IT ( & huart3 , & g_ucUsart3ReceiveData , 1 ); // 继续进行中断接收

        }

}

然后我们更改一下主函数内容，把 PID 红外循迹代码注释掉，然后我们增加串口三的输出，以便我们后面观察数据。

串口不定长输出

我们把转速等信息都可以显示在 OLED 上，那么如何通过串口输出那？

sprintf (( char

* ) Usart3String , "V1:%.2fV2:%.2f\r\n" , Motor1Speed , Motor2Speed ); // 显示两个电机转速单

位：转 / 秒

HAL_UART_Transmit ( & huart3 ,( uint8_t * ) Usart3String , strlen (( const char

* ) Usart3String ), 50 ); // 阻塞式发送通过串口三输出字符 strlen: 计算字符串大小

sprintf (( char * ) Usart3String , "Mileage%.2f\r\n" , Mileage ); // 计算小车里程单位 cm

HAL_UART_Transmit ( & huart3 ,( uint8_t * ) Usart3String , strlen (( const char

* ) Usart3String ), 50 ); // 阻塞式发送通过串口三输出字符 strlen: 计算字符串大小

sprintf (( char * ) Usart3String , "U:%.2fV\r\n" , adcGetBatteryVoltage ()); // 显示电池

电压

HAL_UART_Transmit ( & huart3 ,( uint8_t * ) Usart3String , strlen (( const char

* ) Usart3String ), 50 ); // 阻塞式发送通过串口三输出字符 strlen: 计算字符串大小

HAL_Delay ( 5 );

把之前 PID 初始化时候速度 PID 目标值改成 0。

然后我们测试

硬件连接

我们现在使用 USB-TTL 连接串口三，单片机串口三与电脑通信 ( 底板不需要插入蓝牙 )

然后打开软件

发送指令小车就会对应运动

在电脑串口软件查看输出信息、发送指令控制小车运动

手机蓝牙控制小车

功能 : 蓝牙遥控小车前进、后退、停止、左右转、加速、减速、手机显示数据

蓝牙模块和电脑通信

蓝牙模块 - 硬件介绍

使用： HC-05 主从机一体蓝牙串口透传模块

注意：供电 3.6V-6V( 最好 5V)

引脚顺序 VCC GND TXD RXD

先调试蓝牙模块-设置波特率

如图先把蓝牙模块通过USB-TTL模块相连接，然后

1. 先连接好蓝牙模块的几根线，然后按住蓝牙模块的按键

2. 然后连接电脑，然后几秒后蓝牙小灯慢闪，说明进入 AT 模式

3. 然后串口助手通过 38400 发送设置指令 :AT+UART=115200,0,0

4. 然后收到 OK 数据，说明设置成功。

这个是设置波特率截图

5. 然后重新拔插蓝牙模块 ( 不用按按键 )

6. 在手机系统蓝牙配对 HC-50 密码 1234

7. 串口助手设置波特率 115200 ，然后打开手机 APP 发送任意内容测试

这个是后面通信测试截图

8. 设置按键 - 按照代码设置按下发送的数据

蓝牙模块连接单片机

把蓝牙插入到底板、跳线帽选择蓝牙通信

按下不同按钮小车会对应控制。

超声波避障功能

超声波测距

完成超声波测距功能、测量数据显示在OLED屏幕上。

使用： HC-SR04 超声波测距模块

注意：绘制 PCB 注意四个引脚顺序 Vcc Trig Echo Gnd

供电 3.3V-5V( 最好 5V)

测距原理

不同模式

GPIO模式

查看原理图

通过超声波的硬件介绍我们知道

MCU 给 Trig 脚一个大于 10us 的高电平脉冲；然后读取 Echo 脚的高电平信号时间，通过公式：距离 = T*声速/2 就可以算出来距离。

Trig(PB5) 我们配置为 GPIO 输出

Echo(PA6) 我们配置 GPIO 输入功能

注：这里大家可能会问，为什么不使用定时器捕获功能？

原因 :

1. 留一个定时器方便以后扩展 FreeRTOS 使用

2. 或者扩展其他舵机、电机等

软件初始化

设置 PB5 输出模式然后起别名

设置 PA6 输入模式

然后生成代码

自己新建 HC_SR04.c 和 HC_SR04.h 然后加入工程，指定路径

防止溢出把之前使用的数组调整大一些

因为我们不适用定时器所以我们需要自己写一个 us 级延时函数。

/*******************

* @brief us 级延时

* @param usdelay: 要延时的 us 时间

* @return

*

*******************/

void HC_SR04_Delayus ( uint32_t usdelay )

{

        __IO uint32_t Delay = usdelay * ( SystemCoreClock / 8U /

        1000U / 1000 ); //SystemCoreClock: 系统频率

        do

        {

        __NOP ();

        }

        while ( Delay -- );

}

/*******************
* @brief HC_SR04读取超声波距离
* @param 无
* @return 障碍物距离单位:cm (静止表面平整精度更高)
*
*******************/
float HC_SR04_Read(void)
{
   uint32_t i = 0;
   float Distance;
   HAL_GPIO_WritePin(HC_SR04_Trig_GPIO_Port,HC_SR04_Trig_Pin,GPIO_PIN_SET);//输出15us高电平
   HC_SR04_Delayus(15);
   HAL_GPIO_WritePin(HC_SR04_Trig_GPIO_Port,HC_SR04_Trig_Pin,GPIO_PIN_RESET);//高电平输出结束，设置为低电平

   while(HAL_GPIO_ReadPin(HC_SR04_Echo_GPIO_Port,HC_SR04_Echo_Pin) == GPIO_PIN_RESET)//等待回响高电平
   {
       i++;
       HC_SR04_Delayus(1);
       if(i>100000) return -1;//超时退出循环、防止程序卡死这里
   }
   i = 0;
   while(HAL_GPIO_ReadPin(HC_SR04_Echo_GPIO_Port,HC_SR04_Echo_Pin) == GPIO_PIN_SET)//下面的循环是2us
   {
       i = i+1;
       HC_SR04_Delayus(1);//1us 延时，但是整个循环大概2us左右
       if(i >100000) return -2;//超时退出循环
   }
   Distance = i*2*0.033/2;//这里乘2的原因是上面是2微妙
   return Distance   ;
}

然后就可以读距离了、连上蓝牙可以显示数据

注意 : 两个 HC_SR04_Read() 函数调用的时间间隔要 2ms 及以上，测量范围更大精度更高

sprintf (( char * ) Usart3String , "HC_SR04:%.2fcm\r\n" , HC_SR04_Read ()); // 显示超声波

数据

HAL_UART_Transmit ( & huart3 ,( uint8_t * ) Usart3String , strlen (( const char

* ) Usart3String ), 0xFFFF ); // 通过串口三输出字符 strlen: 计算字符串大小

然后把我们的手机蓝牙和小车蓝牙连接

手机显示

避障逻辑编写

然后我们编写循迹逻辑，我们的逻辑是：

//************** 避障功能 ********************//

// 避障逻辑

if ( HC_SR04_Read () > 25 ) // 前方无障碍物

{

        motorPidSetSpeed ( 1 , 1 ); // 前运动

        HAL_Delay ( 100 );

}

else // 前方有障碍物

{

        motorPidSetSpeed ( - 1 , 1 ); // 右边运动原地

        HAL_Delay ( 500 );

        if ( HC_SR04_Read () > 25 ) // 右边无障碍物

        {

                motorPidSetSpeed ( 1 , 1 ); // 前运动

                HAL_Delay ( 100 );

        }

        else // 右边有障碍物

        {

                motorPidSetSpeed ( 1 , - 1 ); // 左边运动原地

                HAL_Delay ( 1000 );

                if ( HC_SR04_Read () > 25 ) // 左边无障碍物

                {

                        motorPidSetSpeed ( 1 , 1 ); // 前运动

                        HAL_Delay ( 100 );

                }

                else

                 {

                        motorPidSetSpeed ( - 1 , - 1 ); // 后运动

                        HAL_Delay ( 1000 );

                        motorPidSetSpeed ( - 1 , 1 ); // 右边运动

                        HAL_Delay ( 50 );

                }

        }

}

超声波跟随功能

无PID跟随功能

// 超声波跟随

if ( HC_SR04_Read () > 25 )

{

        motorForward (); // 前进

        HAL_Delay ( 100 );

}

if ( HC_SR04_Read () < 20 )

{

        motorBackward (); // 后退

        HAL_Delay ( 100 );

}

PID跟随功能

在pid.c中定义一组PID参数

tPid pidFollow ; // 定距离跟随 PID

pidFollow . actual_val = 0.0 ;

pidFollow . target_val = 22.50 ; // 定距离跟随目标距离 22.5cm

pidFollow . err = 0.0 ;

pidFollow . err_last = 0.0 ;

pidFollow . err_sum = 0.0 ;

pidFollow . Kp =- 0.5 ; // 定距离跟随的 Kp 大小通过估算 PID 输入输出数据，确定大概大小，然后在调试

pidFollow . Ki =- 0.001 ; //Ki 小一些

pidFollow . Kd = 0 ;

//**********PID 跟随功能 ***********//

g_fHC_SR04_Read = HC_SR04_Read (); // 读取前方障碍物距离

if ( g_fHC_SR04_Read < 60 ){ // 如果前 60cm 有东西就启动跟随

g_fFollow_PID_Out = PID_realize ( & pidFollow , g_fHC_SR04_Read ); //PID 计算输出

目标速度这个速度，会和基础速度加减

if ( g_fFollow_PID_Out > 6 ) g_fFollow_PID_Out = 6 ; // 对输出速度限幅

if ( g_fFollow_PID_Out < - 6 ) g_fFollow_PID_Out = - 6 ;

motorPidSetSpeed ( g_fFollow_PID_Out , g_fFollow_PID_Out ); // 速度作用与电机上

}

else motorPidSetSpeed ( 0 , 0 ); // 如果前面 60cm 没有东西就停止

HAL_Delay ( 10 ); // 读取超声波传感器不能过快

最后可以明显发现，PID跟随功能明显跟随的更加快，不容易被一下子贴近碰到以及一下子距离拉大而跟丢。

用MPU6050走直线和转弯90度

使用软件初始化两个引脚

PB8- 输出模式 - 起始输出高电平

PB9 输出模式起始状态高电平

生成代码

打开我们的代码，是通过模拟 IIC 读取 6050 数据的，我们知道 SDA 是模拟 IIC 的数据线所以通信过程中是 再输入和输出模式中切换的，但是我们的 STM32CubeMX 是设置的输出，是在哪里更改的模式？

//IO 方向设置设置 SDA-PB9 为输入或者输出

#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFFFF0F;GPIOB->CRH|=8<<4;}

#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFFFF0F;GPIOB->CRH|=3<<4;}

1. 这是通过 按位与后赋值 &= 和 按位或后赋值 |=

2. 设置 端口配置高寄存器 指定位。

先看一个例子

更改设置SDA与SCL电平的宏

//IO 操作函数

#define MPU_IIC_SCL_Hige

HAL_GPIO_WritePin(SCL_6050_GPIO_Port,SCL_6050_Pin,GPIO_PIN_SET) // 设置 SCL 高电平

#define MPU_IIC_SCL_Low

HAL_GPIO_WritePin(SCL_6050_GPIO_Port,SCL_6050_Pin,GPIO_PIN_RESET) // 设置 SCL 低电平

#define MPU_IIC_SDA_Hige

HAL_GPIO_WritePin(SDA_6050_GPIO_Port,SDA_6050_Pin,GPIO_PIN_SET) // 设置 SDA 高电平

#define MPU_IIC_SDA_Low

HAL_GPIO_WritePin(SDA_6050_GPIO_Port,SDA_6050_Pin,GPIO_PIN_RESET) // 设置 SDA 低电平

#define MPU_READ_SDA HAL_GPIO_ReadPin(SDA_6050_GPIO_Port,SDA_6050_Pin)

// 读 SDA 电平

当然，大家应该对I2C协议应该很了解了，我们这里用GPIO来模拟通信，封装了写数据和读数据的函数，这里就不去详细列举了。

uint8_t MPU_Init(void);                                //初始化MPU6050
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf);//IIC连续写
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf); //IIC连续读
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data);               //IIC写一个字节
uint8_t MPU_Read_Byte(uint8_t reg);                       //IIC读一个字节

uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr);
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr);
uint8_t MPU_Set_LPF(uint16_t lpf);
uint8_t MPU_Set_Rate(uint16_t rate);
uint8_t MPU_Set_Fifo(uint8_t sens);

short MPU_Get_Temperature(void);
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az);

然后在 main.c 中定义变量和添加同文件

float pitch , roll , yaw ; // 俯仰角横滚角航向角

#include "mpu6050.h"

#include "inv_mpu.h"

#include "inv_mpu_dmp_motion_driver.h"

初始化6050

HAL_Delay ( 500 ); // 延时 0.5 秒 6050 上电稳定后初始化

MPU_Init (); // 初始化 MPU6050

while ( MPU_Init () != 0 );

while ( mpu_dmp_init () != 0 )

我们通过下面的代码获得数据

sprintf (( char * ) Usart3String , "pitch:%.2f roll:%.2f

yaw:%.2f\r\n" , pitch , roll , yaw ); // 显示 6050 数据

HAL_UART_Transmit ( & huart3 ,( uint8_t * ) Usart3String , strlen (( const char

* ) Usart3String ), 0xFFFF ); // 通过串口三输出字符 strlen: 计算字符串大小

//mpu_dmp_get_data(&pitch,&roll,&yaw);// 返回值 :0,DMP 成功解出欧拉角

while ( mpu_dmp_get_data ( & pitch , & roll , & yaw ) != 0 ){} // 这个可以解决经常读不出数据的问

题

编译、烧录、然后就可以连接手机蓝牙，在蓝牙软件查看数据了

利用6050直线和90度(有代码)

为什么小车还是不能走直线

为什么两个电机转速一样不能走非常正直线，如何控制小车转弯 90 度。

当然，我们可以开环控制，但是控制效果可能不好，受外界影响比较大。

如果我们使用闭环控制，就要使用一个传感器来获得现在小车角度。

走直线(控制朝一个方向运动)

在 pid.c 中定义一个姿态控制使用的 PID

tPid pidMPU6050YawMovement ; // 利用 6050 偏航角进行姿态控制的 PID

pidMPU6050YawMovement . actual_val = 0.0 ;

pidMPU6050YawMovement . target_val = 0.00 ; // 设定姿态目标值

pidMPU6050YawMovement . err = 0.0 ;

pidMPU6050YawMovement . err_last = 0.0 ;

pidMPU6050YawMovement . err_sum = 0.0 ;

pidMPU6050YawMovement . Kp = 2 ; // 定距离跟随的 Kp 大小通过估算 PID 输入输出数据，确定大概大

小，然后在调试

pidMPU6050YawMovement . Ki = 0 ;

pidMPU6050YawMovement . Kd = 0 ;

仿照之前红外循迹代码编写姿态控制函数

float g_fMPU6050YawMovePidOut = 0.00f ; // 姿态 PID 运算输出

float g_fMPU6050YawMovePidOut1 = 0.00f ; // 第一个电机控制输出

float g_fMPU6050YawMovePidOut2 = 0.00f ; // 第一个电机控制输出

走直线程序如下 ( 因为上电初始化时候航向角是 0 、而且 pidMPU6050YawMovementPID 结构体的目标值target_val 也是 0)

//*************MPU6050 航向角 PID 转向控制 *****************//

sprintf (( char * ) Usart3String , "pitch:%.2f roll:%.2f

yaw:%.2f\r\n" , pitch , roll , yaw ); // 显示 6050 数据俯仰角横滚角航向角

HAL_UART_Transmit ( & huart3 ,( uint8_t * ) Usart3String , strlen (( const char

* ) Usart3String ), 0xFFFF ); // 通过串口三输出字符 strlen: 计算字符串大小

//mpu_dmp_get_data(&pitch,&roll,&yaw);// 返回值 :0,DMP 成功解出欧拉角

while ( mpu_dmp_get_data ( & pitch , & roll , & yaw ) != 0 ){} // 这个可以解决经常读不出数据的问

题

g_fMPU6050YawMovePidOut = PID_realize ( & pidMPU6050YawMovement , yaw ); //PID 计算输

出目标速度这个速度，会和基础速度加减

g_fMPU6050YawMovePidOut1 = 1.5 + g_fMPU6050YawMovePidOut ; // 基础速度加减 PID 输出速

度

g_fMPU6050YawMovePidOut2 = 1.5 - g_fMPU6050YawMovePidOut ;

if ( g_fMPU6050YawMovePidOut1 > 3.5 ) g_fMPU6050YawMovePidOut1 = 3.5 ; // 进行限幅

if ( g_fMPU6050YawMovePidOut1 < 0 ) g_fMPU6050YawMovePidOut1 = 0 ;

if ( g_fMPU6050YawMovePidOut2 > 3.5 ) g_fMPU6050YawMovePidOut2 = 3.5 ;

if ( g_fMPU6050YawMovePidOut2 < 0 ) g_fMPU6050YawMovePidOut2 = 0 ;

motorPidSetSpeed ( g_fMPU6050YawMovePidOut1 , g_fMPU6050YawMovePidOut2 );

然后调节 PID 参数

顺序先确定 P 正负然后 P 大小

然后 D 正负然后 D 大小

最后调节的参数如下

pidMPU6050YawMovement . Kp = 0.02 ; //6050 航向角 PID 运动控制

pidMPU6050YawMovement . Ki = 0 ;

pidMPU6050YawMovement . Kd = 0.1 ;

然后我们把小车放在地上就可以完成一直朝着初始方向前进，如果往侧面推也会马上矫正。

转弯90度功能(控制转弯角度)

然后我们增加一下，如何旋转 90 度程序

在串口接收回调函数表姿态 PID 的目标值

extern tPid pidMPU6050YawMovement ; // 利用 6050 偏航角进行姿态控制的 PID

if ( g_ucUsart3ReceiveData == 'H' ) // 转向 90 度

{

        if ( pidMPU6050YawMovement . target_val <= 180 )

         pidMPU6050YawMovement . target_val += 90 ; // 目标值

}

if ( g_ucUsart3ReceiveData == 'I' ) // 转回 90 度

{

        if ( pidMPU6050YawMovement . target_val >= - 180 )

        pidMPU6050YawMovement . target_val -= 90 ; // 目标值

}

然后我们的蓝牙APP增加两个发送按钮的设置

现象是上电小车向初始方向直行，如果推小车车头方向，小车能够立马矫正。

然后连接蓝牙发送转 90 度小车会转 90 度，按下转回 90 度小车回转回。

按键和APP按钮切换功能

根据上面介绍，我们的模式可以有：

OLED 显示模式 : 速度、里程、电压、超声波数据、 MPU6050 俯仰角、横滚角、航向角数据显示在

OLED 上和通过串口发送蓝牙 APP

PID 循迹模式 : 红外对管 PID 循迹

手机遥控普通运动模式 : 遥控前、后、左、右加速运动

超声波避障模式

PID 跟随模式 : 超声波 PID 定距离跟随

手机遥控角度闭环模式： MPU6050 角度 PID 控制

可以设置标志位通过按键改变标志位，以实现功能切换。

定义一个全局变量：

uint8_t g_ucMode = 0 ;

// 小车运动模式标志位 0: 显示功能、 1:PID 循迹模式、 2: 手机遥控普通运动模式、 3. 超声波避障模式、

4:PID 跟随模式、 5: 遥控角度闭环

然后主函数显示当前处于的模式

然后判断当前模式执行不同代码

方法：一个功能一个功能的添加代码，添加好一个调试测试一下，然后再添加下一个

下面这个就是我们主函数的代码。

int main(void)
{
HAL_Init();

SystemClock_Config();

MX_GPIO_Init();
MX_USART1_UART_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM4_Init();
MX_ADC2_Init();

   OLED_Init(); //初始化OLED
   OLED_Clear();

   Motor_init();

   /*pwm波生成*/
   HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);//开启定时器2
   HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);//开启定时器4

   /*实际上可不开，因为速度没那么快，计数器压根不会溢出*/
   HAL_TIM_Base_Start_IT(&htim2); //开启定时器2 中断
   HAL_TIM_Base_Start_IT(&htim4); //开启定时器4 中断

   HAL_TIM_Base_Start_IT(&htim1); //开启定时器1 中断,计时2ms

   __HAL_UART_ENABLE_IT(&huart1,UART_IT_RXNE);//开启串口接收中断
   HAL_UART_Receive_IT(&huart3,&g_ucUsart3ReceiveData,1); //串口三接收数据


   /*初始化mpu6050*/
   HAL_Delay(500);//延时0.5秒 6050上电稳定后初始化
   MPU_Init(); //初始化MPU6050
   while(MPU_Init()!=0);
   while(mpu_dmp_init()!=0);

   /*pid初始化*/
   PID_init();


while (1)
{
   sprintf((char *)OledString," g_ucMode:%d",g_ucMode);//显示g_ucMode 当前模式
   OLED_ShowString(0,6,OledString,12);   //显示在OLED上

   sprintf((char *)Usart3String," g_ucMode:%d",g_ucMode);//蓝牙APP显示
   HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小

   if(g_ucMode == 0)//0LED显示功能
   {
       sprintf((char*)OledString, "V1:%.2fV2:%.2f", Motor1Speed,Motor2Speed);//显示速度
       OLED_ShowString(0,0,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数，

       sprintf((char*)OledString, "Mileage:%.2f", Mileage);//显示里程
       OLED_ShowString(0,1,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数，

       sprintf((char*)OledString, "U:%.2fV", adcGetBatteryVoltage());//显示电池电压
       OLED_ShowString(0,2,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数，

       sprintf((char *)OledString,"HC_SR04:%.2fcm\r\n",HC_SR04_Read());//显示超声波数据
       OLED_ShowString(0,3,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数，

       sprintf((char *)OledString,"p:%.2f r:%.2f \r\n",pitch,roll);//显示6050数据俯仰角横滚角
       OLED_ShowString(0,4,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数，

       sprintf((char *)OledString,"y:%.2f \r\n",yaw);//显示6050数据航向角
       OLED_ShowString(0,5,OledString,12);//这个是oled驱动里面的，是显示位置的一个函数，

   //蓝牙APP显示
       sprintf((char*)Usart3String, "V1:%.2fV2:%.2f", Motor1Speed,Motor2Speed);//显示速度
       HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小
       //阻塞方式发送可以保证数据发送完毕，中断发送不一定可以保证数据已经发送完毕才启动下一次发送
       sprintf((char*)Usart3String, "Mileage:%.2f", Mileage);//显示里程
       HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小

       sprintf((char*)Usart3String, "U:%.2fV", adcGetBatteryVoltage());//显示电池电压
       HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小

       sprintf((char *)Usart3String,"HC_SR04:%.2fcm\r\n",HC_SR04_Read());//显示超声波数据
       HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小

       sprintf((char *)Usart3String,"p:%.2f r:%.2f \r\n",pitch,roll);//显示6050数据俯仰角横滚角
       HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小

       sprintf((char *)Usart3String,"y:%.2f \r\n",yaw);//显示6050数据航向角
       HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),50);//阻塞式发送通过串口三输出字符 strlen:计算字符串大小

       //获得6050数据
       while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0){} //这个可以解决经常读不出数据的问题

       //在显示模式电机停转设置小车速度为0
       motorPidSetSpeed(0,0);
   }


   if(g_ucMode == 1)///**** 红外PID循迹功能******************/
   {
                       g_ucaHW_Read[0] = READ_HW_OUT_1;//读取红外对管状态、这样相比于写在if里面更高效
                       g_ucaHW_Read[1] = READ_HW_OUT_2;
                       g_ucaHW_Read[2] = READ_HW_OUT_3;
                       g_ucaHW_Read[3] = READ_HW_OUT_4;

                       if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )
                       {
                           g_cThisState = 0;//前进
                       }
                       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )//使用else if更加合理高效
                       {
                           g_cThisState = -1;//应该右转
                       }
                       else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0 )
                       {
                           g_cThisState = -2;//快速右转
                       }
                       else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 1&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 0)
                       {
                           g_cThisState = -3;//快速右转
                       }
                       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 0 )
                       {
                           g_cThisState = 1;//应该左转
                       }
                       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3] == 1 )
                       {
                           g_cThisState = 2;//快速左转
                       }
                       else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0&&g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3] == 1)
                       {
                           g_cThisState = 3;//快速左转
                       }
                       g_fHW_PID_Out = PID_realize(&pidHW_Tracking,g_cThisState);//PID计算输出目标速度这个速度，会和基础速度加减
                       g_fHW_PID_Out1 = 3 + g_fHW_PID_Out;//电机1速度=基础速度+循迹PID输出速度
                       g_fHW_PID_Out2 = 3 - g_fHW_PID_Out;//电机1速度=基础速度-循迹PID输出速度
                       if(g_fHW_PID_Out1 >5) g_fHW_PID_Out1 =5;//进行限幅限幅速度在0-5之间
                       if(g_fHW_PID_Out1 <0) g_fHW_PID_Out1 =0;
                       if(g_fHW_PID_Out2 >5) g_fHW_PID_Out2 =5;//进行限幅限幅速度在0-5之间
                       if(g_fHW_PID_Out2 <0) g_fHW_PID_Out2 =0;
                       if(g_cThisState != g_cLastState)//如何这次状态不等于上次状态、就进行改变目标速度和控制电机、在定时器中依旧定时控制电机
                       {
                           motorPidSetSpeed(g_fHW_PID_Out1,g_fHW_PID_Out2);//通过计算的速度控制电机
                       }
                       g_cLastState = g_cThisState;//保存上次红外对管状态
   }


   if(g_ucMode == 2)//***************遥控模式***********************//
   {
       //遥控模式的控制在串口三的中断里面
   }


   if(g_ucMode == 3)   //******超声波避障模式*********************//
   {
       //避障逻辑
       if(HC_SR04_Read() > 25)//前方无障碍物
       {
           motorPidSetSpeed(1,1);//前运动
           HAL_Delay(100);
       }
       else
       {   //前方有障碍物
           motorPidSetSpeed(-1,1);//右边运动原地
           HAL_Delay(500);
           if(HC_SR04_Read() > 25)//右边无障碍物
           {
               motorPidSetSpeed(1,1);//前运动
               HAL_Delay(100);
           }
           else
           {//右边有障碍物
               motorPidSetSpeed(1,-1);//左边运动原地
               HAL_Delay(1000);
               if(HC_SR04_Read() >25)//左边无障碍物
               {
                   motorPidSetSpeed(1,1);//前运动
                   HAL_Delay(100);
               }
               else
               {
                   motorPidSetSpeed(-1,-1);//后运动
                   HAL_Delay(1000);
                   motorPidSetSpeed(-1,1);//右边运动
                   HAL_Delay(50);
               }
           }
       }
   }


   if(g_ucMode == 4)   //**********PID跟随功能***********//
   {
       g_fHC_SR04_Read=HC_SR04_Read();//读取前方障碍物距离

       if(g_fHC_SR04_Read < 60)//如果前60cm 有东西就启动跟随
       {
           g_fFollow_PID_Out = PID_realize(&pidFollow,g_fHC_SR04_Read);//PID计算输出目标速度这个速度，会和基础速度加减
           if(g_fFollow_PID_Out > 6) g_fFollow_PID_Out = 6;//对输出速度限幅
           if(g_fFollow_PID_Out < -6) g_fFollow_PID_Out = -6;
           motorPidSetSpeed(g_fFollow_PID_Out,g_fFollow_PID_Out);//速度作用与电机上
       }
       else motorPidSetSpeed(0,0);//如果前面60cm 没有东西就停止
       HAL_Delay(10);//读取超声波传感器不能过快
   }


   if(g_ucMode == 5)//*************MPU6050航向角 PID转向控制*****************//
   {

       sprintf((char *)Usart3String,"pitch:%.2f roll:%.2f yaw:%.2f\r\n",pitch,roll,yaw);//显示6050数据俯仰角横滚角航向角
       HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const char *)Usart3String),0xFFFF);//通过串口三输出字符 strlen:计算字符串大小

   //mpu_dmp_get_data(&pitch,&roll,&yaw);//返回值:0,DMP成功解出欧拉角
       while(mpu_dmp_get_data(&pitch,&roll,&yaw)!=0){} //这个可以解决经常读不出数据的问题


       g_fMPU6050YawMovePidOut = PID_realize(&pidMPU6050YawMovement,yaw);//PID计算输出目标速度这个速度，会和基础速度加减

       g_fMPU6050YawMovePidOut1 = 1.5 + g_fMPU6050YawMovePidOut;//基础速度加减PID输出速度
       g_fMPU6050YawMovePidOut2 = 1.5 - g_fMPU6050YawMovePidOut;
       if(g_fMPU6050YawMovePidOut1 >3.5) g_fMPU6050YawMovePidOut1 =3.5;//进行限幅
       if(g_fMPU6050YawMovePidOut1 <0) g_fMPU6050YawMovePidOut1 =0;
       if(g_fMPU6050YawMovePidOut2 >3.5) g_fMPU6050YawMovePidOut2 =3.5;//进行限幅
       if(g_fMPU6050YawMovePidOut2 <0) g_fMPU6050YawMovePidOut2 =0;
       motorPidSetSpeed(g_fMPU6050YawMovePidOut1,g_fMPU6050YawMovePidOut2);//将最后计算的目标速度通过motorPidSetSpeed控制电机

   }

/* USER CODE END WHILE */

/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}

我们添加一个通过蓝牙APP按钮切换模式代码

if ( g_ucUsart3ReceiveData == 'J' ) // 改变模式

{

        if ( g_ucMode == 5 ) g_ucMode = 1 ; //g_ucMode 模式是 0 1 2 3 4 5

        else

        {

                g_ucMode += 1 ;

        }

}

if ( g_ucUsart3ReceiveData == 'K' ) g_ucMode = 0 ; // 设置为显示模式