四轮两驱小车（四）：STM32驱动5路灰度传感器PID循迹

目录

前言：

小车效果展示：

5路数字灰度传感器：

巡线思路：

加入PID调节的代码：

前言：

        之前买了一批5路灰度传感器，想用这传感器进行循迹，无奈网上和官方的资料提供的还是比较少，这里还是做一下当初的学习记录。

小车效果展示：

STM32RCT6主控，5路灰度寻迹，超声波HC_SR04中断式测距，蓝牙模块HC_08通信，AS4950电机驱动芯片，减缓了MPU6050零漂问题，PID丝滑_哔哩哔哩_bilibili

5路数字灰度传感器：

         这是某宝上买的5路灰度传感器，价格稍微有点贵，50多块一个。当然，一分钱一分货，这个模块可以用小螺丝刀来调节传感器上面的旋钮，通过这个旋钮来调节灵敏度，这个灵敏度调节好了的话可以识别黑白循迹、红白循迹。所以还是比较值得的。

        对于这款5路数字灰度传感器来说。识别到黑线，传回来的数字量就是0，识别到白色，传回来的数字量就是1。

巡线思路：

        博主想着用这5路的中间三路来巡线，用最左和最右端的传感器用来识别十字或者丁字路口，巡线时，加入PID算法，遇到十字或者丁字路口就用最左和最右的传感器来识别，识别到了之后，我们就可以搭配MPU6050进行转90°弯了。

加入PID调节的代码：

        其中 sensor_bias 是根据中间三路传感器和黑线的相对位置来估计出的误差（如果你要问我怎么得来的，其实这个数据大差不差就行，它只是为PID服务的一个变量罢了，甚至你可以把62.5改成50，最后只要调好PID三个参数，达到的效果是一样的），这里的decide类似于状态机的信号，我令decide为6的时候，也就是小车跑出了黑线，小车停止。

#include "sensor.h" #include "stm32f10x.h" #include "move.h" #include "motor.h" #include "FSM.h"  //STEER4 		--> PA11  --> R2  红线 //STEER3 		--> PC9 	--> R1  橘线 //			 		--> PB4   --> M0  黄线 //STEER1 		--> PA6   --> L1  绿线 //ENCODE1_A --> PB5	  --> L2  棕线  float Kp_sensor = 8.134, Ki_sensor = 0.021, Kd_sensor = 2.36;//pid弯道参数参数  float sensor_bias = 0; float sensor_bias_last = 0; float P = 0, I = 0, D = 0, PID_value = 0;  //pid直道参数  int decide;  unsigned char move_flag; extern unsigned char FSM_state; void sensor_Init() { 	GPIO_InitTypeDef GPIO_InitStructure; 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC,ENABLE);//开启C时钟   PC9 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; 	//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//这句话其实可以不用，在使用输入功能时，不需要配置频率 	GPIO_Init(GPIOC, &GPIO_InitStructure); 	 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启A时钟  PA11  PA6 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11|GPIO_Pin_6; 	GPIO_Init(GPIOA, &GPIO_InitStructure); 	 	GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE);  	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启B时钟  PB4  PB5 	 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4|GPIO_Pin_5; 	GPIO_Init(GPIOB, &GPIO_InitStructure); }  unsigned char times; extern unsigned char FSM_hc08; unsigned char back_flag; unsigned char one_time; unsigned char one_flag; unsigned char channel_num;//channel的存在导致发送2号通道的时候只会进入一次 void sensor_read() { 	if((L2 == 1)&&(L1 == 1)&&(M0 == 1)&&(R1 == 0))// 1 1 1 0  	{ 		if(one_time == 0) 		{ 			if(FSM_hc08 == Channel_1) 			{ 				FSM_state = Turn_lift_state;//跳出循环，将PID分别清零。 				one_time++; 				P = 0;I = 0; D = 0; 			} 			else if(FSM_hc08 == Channel_2) 			{ 				if(one_flag == 0) 				{ 					channel_num++; 					one_flag = 1; 				} 				if(channel_num == 2) 				{	 					FSM_state = stay2_state;//跳出循环，将PID分别清零。 					one_time++; 					P = 0;I = 0; D = 0; 				} 			} 		} 	} 	else if((L1 == 0)&&(M0 == 1)&&(R1 == 0))// 0 1 0  	{ 		sensor_bias = 0;decide = 3;one_flag = 0;//积分项清零 	} 	else if((L1 == 1)&&(M0 == 1)&&(R1 == 0))// 1 1 0  	{ 		sensor_bias = -62.5;decide = 2; 	} 	else if((L1 == 0)&&(M0 == 1)&&(R1 == 1))// 0 1 1  	{ 		sensor_bias = 62.5;decide = 2; 	} 	else if((L1 == 1)&&(M0 == 0)&&(R1 == 0))// 1 0 0  	{ 		sensor_bias = -125;decide = 4; 	} 	else if((L1 == 0)&&(M0 == 0)&&(R1 == 1))// 0 0 1  	{ 		sensor_bias = 62.5;decide = 4; 	} 	else if((L1 == 0)&&(M0 == 0)&&(R1 == 0))// 0 0 0  	{ 		decide = 6; 	} 	else if((L1 == 1)&&(M0 == 1)&&(R1 == 1))// 1 1 1  	{ 		decide = 6;FSM_state = Judge_state;		//如果读取到了整条黑线，那么就进入下一状态 		if(back_flag == 1) 		{ 			FSM_state = Back_state; 			back_flag = 0; 		}			//第一次识别到全黑线为小车停止线。第二次识别到，代表小车即将回归循迹 	}  } void Sensor_pid() { 	if(decide<=5) 	{ 		P = sensor_bias; 		I = I + sensor_bias; 		D = sensor_bias-sensor_bias_last; 		PID_value = Kp_sensor*P + Ki_sensor*I + Kd_sensor*D; 		sensor_bias_last = sensor_bias; 		//对积分值设置一个限制，防止积分值超标 		 		if(I >=3500)I = 3500; 		if(I <= -3500)I = -3500; 		PWM_value_R = 2099 - (int)PID_value; 		PWM_value_L = 2099 + (int)PID_value;//当线在左，左轮要慢，右轮要快，左轮要加，右轮要减，但这里的偏差是负值 		Motor3_forward(PWM_value_R); 		Motor4_forward(PWM_value_L); 		//4 --> 右电机 		//3 --> 左电机	 	} 	else{ 		Move_stop(); 	} } 

        对应的头文件部分

#ifndef __SENSOR_H #define __SENSOR_H  //STEER4 --> PA11 --> R2  绿线 //STEER3 --> PC9 	--> R1  黄线 //		B  --> PB6  --> M0  橘线 //STEER1 --> PA6  --> L1  红线 // 		A  --> PB7	--> L2  白线 // 灰度传感器，当传感器识别到黑线的时候，输出为1，其余时刻输出为0 // 所以在这里我们要使用下拉输入 #define L2 GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_5) #define L1 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6) #define M0 GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4) #define R1 GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_9) #define R2 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_11)  void sensor_Init(void); void sensor_read(void); void Sensor_pid(void);  #endif