【电赛】STM32-PID直流减速电机小车【寻迹+避障+跟随】【更新ing】_电磁车pid避障

一.需求分析

1.主控：STM32C8T6（没什么好说的哈哈）

2.电机：JAG25-370电机

【问】为什么要用直流减速电机？？

PID控制器需要依靠精确的反馈信号来调整其输出，确保电机按照预定的速度和位置运行。

直流减速电机具有编码器，所以具有很高的可靠性。

3.电机驱动模块：A4950

A4950通过简单的PWM控制电机速度和转矩，采用了高效的H桥驱动方式，可以有效地控制电机的正反转和制动。H桥驱动可以减少能量损失，提高整体效率。

4.超声波测距模块：HC-SR04

5.寻迹：TCRT5000红外反射传感器 * 4

6.蓝牙：HC-05蓝牙串口透传模块

7.姿态控制：MPU6050（经典）

8.电源检测：ADC

9.OLED

二.硬件原理图

主控：

电源输入：

 电机驱动和电机接口：

超声波模块接口：

红外对管接口：

蓝牙：

MPU6050:

 ADC电源安全检测：

OLED：

三.实现功能的逐个实验

1.通过外部中断控制按键点灯

PB4 - KEY1

PA12 - KEY2

首先GPIO初始化一下，然后设置外部中断：

PB4 - KEY1 - 设置为下拉（3.3V，把他拉下来） - 上升沿（没按下之前是0，按下才变1，所以是检测上升沿）

PA12 - KEY2 - 设置为上拉 - 下降沿（按下之后是0，没按下之前是1，所以检测下降沿）

STM32CubeIDE中配置外部中断。

 生成项目后：找到GPIO中断的回调函数

在我们自己的gpio.c中重新写一下：

 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin == KEY1_Pin) 
	{
		HAL_GPIO_TogglePin(LED_GPIO_Port,LED_Pin);
	}
	if(GPIO_Pin == KEY2_Pin) 
	{
		HAL_GPIO_TogglePin(LED_GPIO_Port,LED_Pin);
	}
}

这是测试代码。

2.OLED初始化

引脚首先要配置好：

PA15    OLED_SCL

PB12    OLED_SDA

都是上拉输出模式（IIC）

OLED的代码上优信电子官网就可以下载了，我们只要会移植，会用即可。

#include "oled.h"
#include "stdlib.h"
#include "oledfont.h"  	 
 
//OLED的显存
//存放格式如下.
//[0]0 1 2 3 ... 127	
//[1]0 1 2 3 ... 127	
//[2]0 1 2 3 ... 127	
//[3]0 1 2 3 ... 127	
//[4]0 1 2 3 ... 127	
//[5]0 1 2 3 ... 127	
//[6]0 1 2 3 ... 127	
//[7]0 1 2 3 ... 127 			   
/**********************************************
//IIC Start
**********************************************/
/**********************************************
//IIC Start
**********************************************/
void IIC_Start()
{
 
	OLED_SCLK_Set() ;
	OLED_SDIN_Set();
	OLED_SDIN_Clr();
	OLED_SCLK_Clr();
}
 
/**********************************************
//IIC Stop
**********************************************/
void IIC_Stop()
{
OLED_SCLK_Set() ;
//	OLED_SCLK_Clr();
	OLED_SDIN_Clr();
	OLED_SDIN_Set();
	
}
 
void IIC_Wait_Ack()
{
 
	//GPIOB->CRH &= 0XFFF0FFFF;	//设置PB12为上拉输入模式
	//GPIOB->CRH |= 0x00080000;
//	OLED_SDA = 1;
//	delay_us(1);
	//OLED_SCL = 1;
	//delay_us(50000);
/*	while(1)
	{
		if(!OLED_SDA)				//判断是否接收到OLED 应答信号
		{
			//GPIOB->CRH &= 0XFFF0FFFF;	//设置PB12为通用推免输出模式
			//GPIOB->CRH |= 0x00030000;
			return;
		}
	}
*/
	OLED_SCLK_Set() ;
	OLED_SCLK_Clr();
}
/**********************************************
// IIC Write byte
**********************************************/
 
void Write_IIC_Byte(unsigned char IIC_Byte)
{
	unsigned char i;
	unsigned char m,da;
	da=IIC_Byte;
	OLED_SCLK_Clr();
	for(i=0;i<8;i++)		
	{
			m=da;
		//	OLED_SCLK_Clr();
		m=m&0x80;
		if(m==0x80)
		{OLED_SDIN_Set();}
		else OLED_SDIN_Clr();
			da=da<<1;
		OLED_SCLK_Set();
		OLED_SCLK_Clr();
		}
 
 
}
/**********************************************
// IIC Write Command
**********************************************/
void Write_IIC_Command(unsigned char IIC_Command)
{
   IIC_Start();
   Write_IIC_Byte(0x78);            //Slave address,SA0=0
	IIC_Wait_Ack();	
   Write_IIC_Byte(0x00);			//write command
	IIC_Wait_Ack();	
   Write_IIC_Byte(IIC_Command); 
	IIC_Wait_Ack();	
   IIC_Stop();
}
/**********************************************
// IIC Write Data
**********************************************/
void Write_IIC_Data(unsigned char IIC_Data)
{
   IIC_Start();
   Write_IIC_Byte(0x78);			//D/C#=0; R/W#=0
	IIC_Wait_Ack();	
   Write_IIC_Byte(0x40);			//write data
	IIC_Wait_Ack();	
   Write_IIC_Byte(IIC_Data);
	IIC_Wait_Ack();	
   IIC_Stop();
}
void OLED_WR_Byte(unsigned dat,unsigned cmd)
{
	if(cmd)
			{
 
   Write_IIC_Data(dat);
   
		}
	else {
   Write_IIC_Command(dat);
		
	}
 
 
}
 
 
/********************************************
// fill_Picture
********************************************/
void fill_picture(unsigned char fill_Data)
{
	unsigned char m,n;
	for(m=0;m<8;m++)
	{
		OLED_WR_Byte(0xb0+m,0);		//page0-page1
		OLED_WR_Byte(0x00,0);		//low column start address
		OLED_WR_Byte(0x10,0);		//high column start address
		for(n=0;n<128;n++)
			{
				OLED_WR_Byte(fill_Data,1);
			}
	}
}
 
 
/***********************Delay****************************************/
void Delay_50ms(unsigned int Del_50ms)
{
	unsigned int m;
	for(;Del_50ms>0;Del_50ms--)
		for(m=6245;m>0;m--);
}
 
void Delay_1ms(unsigned int Del_1ms)
{
	unsigned char j;
	while(Del_1ms--)
	{	
		for(j=0;j<123;j++);
	}
}
 
//坐标设置
 
	void OLED_Set_Pos(unsigned char x, unsigned char y) 
{ 	OLED_WR_Byte(0xb0+y,OLED_CMD);
	OLED_WR_Byte(((x&0xf0)>>4)|0x10,OLED_CMD);
	OLED_WR_Byte((x&0x0f),OLED_CMD); 
}   	  
//开启OLED显示    
void OLED_Display_On(void)
{
	OLED_WR_Byte(0X8D,OLED_CMD);  //SET DCDC命令
	OLED_WR_Byte(0X14,OLED_CMD);  //DCDC ON
	OLED_WR_Byte(0XAF,OLED_CMD);  //DISPLAY ON
}
//关闭OLED显示     
void OLED_Display_Off(void)
{
	OLED_WR_Byte(0X8D,OLED_CMD);  //SET DCDC命令
	OLED_WR_Byte(0X10,OLED_CMD);  //DCDC OFF
	OLED_WR_Byte(0XAE,OLED_CMD);  //DISPLAY OFF
}		   			 
//清屏函数,清完屏,整个屏幕是黑色的!和没点亮一样!!!	  
void OLED_Clear(void)  
{  
	uint8_t i,n;		    
	for(i=0;i<8;i++)  
	{  
		OLED_WR_Byte (0xb0+i,OLED_CMD);    //设置页地址（0~7）
		OLED_WR_Byte (0x00,OLED_CMD);      //设置显示位置—列低地址
		OLED_WR_Byte (0x10,OLED_CMD);      //设置显示位置—列高地址   
		for(n=0;n<128;n++)OLED_WR_Byte(0,OLED_DATA); 
	} //更新显示
}
void OLED_On(void)  
{  
	uint8_t i,n;		    
	for(i=0;i<8;i++)  
	{  
		OLED_WR_Byte (0xb0+i,OLED_CMD);    //设置页地址（0~7）
		OLED_WR_Byte (0x00,OLED_CMD);      //设置显示位置—列低地址
		OLED_WR_Byte (0x10,OLED_CMD);      //设置显示位置—列高地址   
		for(n=0;n<128;n++)OLED_WR_Byte(1,OLED_DATA); 
	} //更新显示
}
//在指定位置显示一个字符,包括部分字符
//x:0~127
//y:0~63
//mode:0,反白显示;1,正常显示				 
//size:选择字体 16/12 
void OLED_ShowChar(uint8_t x,uint8_t y,uint8_t chr,uint8_t Char_Size)
{      	
	unsigned char c=0,i=0;	
		c=chr-' ';//得到偏移后的值			
		if(x>Max_Column-1){x=0;y=y+2;}
		if(Char_Size ==16)
			{
			OLED_Set_Pos(x,y);	
			for(i=0;i<8;i++)
			OLED_WR_Byte(F8X16[c*16+i],OLED_DATA);
			OLED_Set_Pos(x,y+1);
			for(i=0;i<8;i++)
			OLED_WR_Byte(F8X16[c*16+i+8],OLED_DATA);
			}
			else {	
				OLED_Set_Pos(x,y);
				for(i=0;i<6;i++)
				OLED_WR_Byte(F6x8[c][i],OLED_DATA);
				
			}
}
//m^n函数
uint32_t oled_pow(uint8_t m,uint8_t n)
{
	uint32_t result=1;	 
	while(n--)result*=m;    
	return result;
}				  
//显示2个数字
//x,y :起点坐标	 
//len :数字的位数
//size:字体大小
//mode:模式	0,填充模式;1,叠加模式
//num:数值(0~4294967295);	 		  
void OLED_ShowNum(uint8_t x,uint8_t y,uint32_t num,uint8_t len,uint8_t size2)
{         	
	uint8_t t,temp;
	uint8_t enshow=0;						   
	for(t=0;t<len;t++)
	{
		temp=(num/oled_pow(10,len-t-1))%10;
		if(enshow==0&&t<(len-1))
		{
			if(temp==0)
			{
				OLED_ShowChar(x+(size2/2)*t,y,' ',size2);
				continue;
			}else enshow=1; 
		 	 
		}
	 	OLED_ShowChar(x+(size2/2)*t,y,temp+'0',size2); 
	}
} 
//显示一个字符号串
void OLED_ShowString(uint8_t x,uint8_t y,uint8_t *chr,uint8_t Char_Size)
{
	unsigned char j=0;
	while (chr[j]!='\0')
	{		OLED_ShowChar(x,y,chr[j],Char_Size);
			x+=8;
		if(x>120){x=0;y+=2;}
			j++;
	}
}
//显示汉字
void OLED_ShowCHinese(uint8_t x,uint8_t y,uint8_t no)
{      			    
	uint8_t t,adder=0;
	OLED_Set_Pos(x,y);	
    for(t=0;t<16;t++)
		{
				OLED_WR_Byte(Hzk[2*no][t],OLED_DATA);
				adder+=1;
     }	
		OLED_Set_Pos(x,y+1);	
    for(t=0;t<16;t++)
			{	
				OLED_WR_Byte(Hzk[2*no+1][t],OLED_DATA);
				adder+=1;
      }					
}
/***********功能描述：显示显示BMP图片128×64起始点坐标(x,y),x的范围0～127，y为页的范围0～7*****************/
void OLED_DrawBMP(unsigned char x0, unsigned char y0,unsigned char x1, unsigned char y1,unsigned char BMP[])
{ 	
 unsigned int j=0;
 unsigned char x,y;
  
  if(y1%8==0) y=y1/8;      
  else y=y1/8+1;
	for(y=y0;y<y1;y++)
	{
		OLED_Set_Pos(x0,y);
    for(x=x0;x<x1;x++)
	    {      
	    	OLED_WR_Byte(BMP[j++],OLED_DATA);	    	
	    }
	}
} 
 
//初始化SSD1306					    
void OLED_Init(void)
{ 	
 
 	 
 
 	 
// 	GPIO_InitTypeDef  GPIO_InitStructure;
// 	
 
//	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);	 //使能A端口时钟
//	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5|GPIO_Pin_7;	 
// 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 		 //推挽输出
//	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//速度50MHz
// 	GPIO_Init(GPIOA, &GPIO_InitStructure);	  //初始化GPIOD3,6
// 	GPIO_SetBits(GPIOA,GPIO_Pin_5|GPIO_Pin_7);	
 
 
    HAL_Delay(800);
    OLED_WR_Byte(0xAE,OLED_CMD);//--display off
	OLED_WR_Byte(0x00,OLED_CMD);//---set low column address
	OLED_WR_Byte(0x10,OLED_CMD);//---set high column address
	OLED_WR_Byte(0x40,OLED_CMD);//--set start line address  
	OLED_WR_Byte(0xB0,OLED_CMD);//--set page address
	OLED_WR_Byte(0x81,OLED_CMD); // contract control
	OLED_WR_Byte(0xFF,OLED_CMD);//--128   
	OLED_WR_Byte(0xA1,OLED_CMD);//set segment remap 
	OLED_WR_Byte(0xA6,OLED_CMD);//--normal / reverse
	OLED_WR_Byte(0xA8,OLED_CMD);//--set multiplex ratio(1 to 64)
	OLED_WR_Byte(0x3F,OLED_CMD);//--1/32 duty
	OLED_WR_Byte(0xC8,OLED_CMD);//Com scan direction
	OLED_WR_Byte(0xD3,OLED_CMD);//-set display offset
	OLED_WR_Byte(0x00,OLED_CMD);//
	
	OLED_WR_Byte(0xD5,OLED_CMD);//set osc division
	OLED_WR_Byte(0x80,OLED_CMD);//
	
	OLED_WR_Byte(0xD8,OLED_CMD);//set area color mode off
	OLED_WR_Byte(0x05,OLED_CMD);//
	
	OLED_WR_Byte(0xD9,OLED_CMD);//Set Pre-Charge Period
	OLED_WR_Byte(0xF1,OLED_CMD);//
	
	OLED_WR_Byte(0xDA,OLED_CMD);//set com pin configuartion
	OLED_WR_Byte(0x12,OLED_CMD);//
	
	OLED_WR_Byte(0xDB,OLED_CMD);//set Vcomh
	OLED_WR_Byte(0x30,OLED_CMD);//
	
	OLED_WR_Byte(0x8D,OLED_CMD);//set charge pump enable
	OLED_WR_Byte(0x14,OLED_CMD);//
	
	OLED_WR_Byte(0xAF,OLED_CMD);//--turn on oled panel
}  

SCL和SDA宏定义要改一下；

库函数要改成hal库的函数；

最后进行测试：

OLED_Init(); //初始化OLED
OLED_Clear();
 
OLED_ShowCHinese(0,0,0);//中
OLED_ShowCHinese(18,0,1);//景
OLED_ShowCHinese(36,0,2);//园
OLED_ShowCHinese(54,0,3);//电
OLED_ShowCHinese(72,0,4);//子
OLED_ShowCHinese(90,0,5);//科
OLED_ShowCHinese(108,0,6);//技

3.串口初始化

硬件接线：

因为用到了CH340，USB转串口模块：

PA9 TX     --------     CH340的TX

PA10 RX   ---------     CH340的RX

然后我们要实现printf函数：

打开魔术棒 -  Target  -  Use MicroLIB打勾

然后重定向fputc:

/**
* @brief 重定向printf (重定向fputc)，
使用时候记得勾选上魔法棒->Target->UseMicro LIB
可能需要在C文件加typedef struct __FILE FILE;
包含这个文件#include "stdio.h"
* @param
* @return
*/
int fputc(int ch,FILE *stream)
{
  HAL_UART_Transmit(&huart1,( uint8_t *)&ch,1,0xFFFF);
  return ch;
}

在usart.c添加 typedef struct __FILE FILE;

测试一下printf即可。

4.PWM控制电机

硬件接线 ：

AIN2 - PWMA - PA11

BIN2 - PWMB - PA8

【问】两个PWM的通道对应的引脚是如何选择的？

查阅STM32数据手册可以发现：

PA8和PA11刚好对应TIM1的通道1和通道4。

所以我们在IDE中进行设置一下复用功能重映射：

设置PWM输出 ，设置占空比 

生成代码后打开定时器1的通道1和4 

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

【仿真测试PWM】

然后设置时钟频率，板子外部晶振8Mhz

开启仿真

仿真成功。

5.电机驱动初始化

硬件连接：

AIN1和BIN1我们通过引脚电平控制高低：

AIN1低电平 - 转速为正方向 - 此时AIN2的PWM占空比数值对应着转速

AIN1高电平 - 转速为负方向 - 此时AIN2的 （1 - PWM占空比数值）对应着转速

【一个朴实无华的电机转速控制函数】

 
#define  AIN1_SET    HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET)
#define  AIN1_RESET  HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET)
 
#define  BIN1_SET    HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET)
#define  BIN1_RESET  HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET)
 
 
void Motor_Set(int Motor1,int Motor2)
{
	//1.先根据正负设置方向GPIO 高低电平
	if(Motor1 <0) BIN1_SET;
	else  BIN1_RESET;
	
	if(Motor2 <0) AIN1_SET;
	else AIN1_RESET;
	
	//2.然后设置占空比  
	if(Motor1 <0)
	{
		if(Motor1 <-99) Motor1 =-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (100+Motor1));
	}
	else 
	{
		if(Motor1 >99) Motor1 = 99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,Motor1);
	}
 
	if(Motor2<0)
	{
		if(Motor2 <-99) Motor2=-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, (100+Motor2));
	}
	else
	{
		if(Motor2 >99) Motor2 =99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, Motor2);
	}
}

这个函数就是根据我们传进去的Motor数值设置占空比 

__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (100+Motor1));

6.编码器测速

参考这一篇：

STM32定时器（4.JGA25-370霍尔编码器测速）https://blog.csdn.net/Xiaoxuexxxxx/article/details/137937083

【使用中断定时测速】

设置定时器1，时钟为内部时钟，开启自动更新中断

 NVIC开启中断

代码中开启中断：

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

 定时中断函数中添加测速函数：

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;
		}
	}

7.观察PID控制曲线的准备工作：匿名上位机的移植

【大端模式】

数据的低位存储在地址的高位，数据的高位存储在地址的低位。

【小端模式】

数据的低位存储在地址的低位，数据的高位存储在地址的高位。

STM32数据是小端存储 

匿名上位机通信协议：

新建一个NiMing.c

#include "niming.h"
#include "main.h"
#include "usart.h"
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);//这里是串口发送函数
}

#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))

8.PID

#include "pid.h"
//定义一个结构体类型变量
tPid pidMotor1Speed;
//给结构体类型变量赋初值
void PID_init()
{
pidMotor1Speed.actual_val=0.0;
pidMotor1Speed.target_val=0.00;
pidMotor1Speed.err=0.0;
pidMotor1Speed.err_last=0.0;
pidMotor1Speed.err_sum=0.0;
pidMotor1Speed.Kp=0;
pidMotor1Speed.Ki=0;
pidMotor1Speed.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;
}

PID的精髓就是这句代码：

PID输出 = Kp*当前误差 + Ki*误差累计值 + Kd*(当前误差-上次误差)

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

PID本身可以理解成一个封装好的函数，输入的两个参数，一个是理想值（自己设置），一个是实际值，比如速度，我设置一个理想值，那么当前速度如何输入进去？

用到上一节的编码器测速：实时检测当前小车速度，然后输入进PID，PID输出一个速度实时调整小车的速度，使其尽可能稳定在设置的理想值速度。

9.使用cjson：PID调参：确定Kp、Ki、Kd

首先软件开启USART1，主函数开启接收中断：

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

在USART1的串口中断回调函数中：

uint8_t Usart1_ReadBuf[256]; //串口1 缓冲数组
uint8_t Usart1_ReadCount = 0; //串口1 接收字节计数
 
 
void USART1_IRQHandler(void)
{
  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);
  }
  HAL_UART_IRQHandler(&huart1);
}
 

再写一个函数判断是否接收完一帧数据：

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库加进去：

#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);

然后通过串口1在匿名上位机发送json格式的数据：

{“p”: 11，“i”：1，“d”：1，“a”：-4 }

PID调参方法：

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

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

3. 一般系统不使用 D 观察PID波形调参。

10.PID整定

那么多久采样一次速度呢？同时PID整定也需要放在采样周期的中断里执行才更加合理对吧？

我们选择的是 20ms 一次采样，代码放在编码器测速的中断里面。

if(TimerCount %10 ==0)//每20ms一次
{
  Motor_Set(PID_realize(&pidMotor1Speed,Motor1Speed),0);
  TimerCount=0;
}

11.双电机的PID控制

两个电机，两个速度，两个不同的PID需要单独调参，我们封装一个函数：

typedef struct 
{
	float target_val;//目标值
	float actual_val;//实际值
	float err;//当前偏差
	float err_last;//上次偏差
	float err_sum;//误差累计值
	float Kp,Ki,Kd;//比例，积分，微分系数
	
} tPid;

//定义一个结构体类型变量
tPid pidMotor1Speed;
tPid pidMotor2Speed;
//给结构体类型变量赋初值
void PID_init()
{
 
	pidMotor1Speed.actual_val=0.0;
	pidMotor1Speed.target_val=3.00;
	pidMotor1Speed.err=0.0;
	pidMotor1Speed.err_last=0.0;
	pidMotor1Speed.err_sum=0.0;
	pidMotor1Speed.Kp=15;
	pidMotor1Speed.Ki=5;
	pidMotor1Speed.Kd=0;
	
	pidMotor2Speed.actual_val=0.0;
	pidMotor2Speed.target_val=3.00;
	pidMotor2Speed.err=0.0;
	pidMotor2Speed.err_last=0.0;
	pidMotor2Speed.err_sum=0.0;
	pidMotor2Speed.Kp=15;
	pidMotor2Speed.Ki=5;
	pidMotor2Speed.Kd=0;
}
 
//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;
}

12.小车前后左右停

封装一个函数：

/*******************
* @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,M
otor2Speed));
}

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

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

motorPidSetSpeed(1,1);       // 前进

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

motorPidSetSpeed(0,0);       // 停止

motorPidSetSpeed(-1,1);      // 右原地旋转

motorPidSetSpeed(1,-1);      // 左原地旋转

加速减速函数：

//向前加速函数
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);//设置到电机
}

13.ADC采集电源电压

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;
}

#define READ_HW_OUT_1 HAL_GPIO_ReadPin(HW_OUT_1_GPIO_Port,HW_OUT_1_Pin) 
#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);//快速左转
}

加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 )
{
// printf("应该前进\r\n");//注释掉更加高效，减少无必要程序执行
  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更加合理高效
{
// printf("应该右转\r\n");
  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 )
{
// printf("快速右转\r\n");
  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)
{
// printf("快速右转\r\n");
  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 )
{
// printf("应该左转\r\n");
  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 )
{
// printf("快速左转\r\n");
  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)
{
// printf("快速左转\r\n");
  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;
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);//通过计算的速度控制电机
}

15.通过串口控制小车

同样首先通过软件初始化串口3。

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

找到串口3的中断处理函数，根据自己喜好进行编写：

extern uint8_t g_ucUsart3ReceiveData; //保存串口三接收的数据
 
//串口接收回调函数
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);//继续进行中断接收
}
}

16.HC-SR04超声波避障

参考这篇：

超声波测距实验https://blog.csdn.net/Xiaoxuexxxxx/article/details/137917873?spm=1001.2014.3001.5501避障的逻辑代码：

//避障逻辑
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);
}
}
}

17.超声波跟随

如果不加PID，那么如下代码就是跟随的朴实无华的逻辑：

//超声波跟随
if(HC_SR04_Read() > 25)
{
  motorForward();//前进
  HAL_Delay(100);
}
if(HC_SR04_Read() < 20)
{
  motorBackward();//后退
  HAL_Delay(100);
}

显然不行，那么让我们加上PID

首先再定义一个用于跟随的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跟随逻辑：

输入检测的小车和前方物体的距离，实时输出两个电机的速度，实现稳定跟随。

//**********PID跟随功能***********//
g_fHC_SR04_Read=HC_SR04_Read();//读取前方障碍物距离
 
//如果前60cm 有东西就启动跟随
if(g_fHC_SR04_Read < 60){ 
//PID计算输出目标速度 这个速度，会和基础速度加减
g_fFollow_PID_Out = PID_realize(&pidFollow,g_fHC_SR04_Read);
 
//对输出速度限幅
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);//读取超声波传感器不能过快