2021年电赛国赛E题 智能送药小车 代码部分原理详解

之前在校电赛做过送药小车，效果还不错。当时刚刚学STM32，用的还是标准库，写的代码也是非常粗糙，现在准备回过头来用HAL库对整体的代码进行移植，并在一些小细节上进行逻辑的优化和重构，在两年来的技术积累中，PID的使用也逐渐熟练，尤其是在送药小车的代码中更是大量运用PID算法，下面我会讲解一些我做的送药小车的一些基本配置代码和一些关键的PID算法代码。

首先要明白做送药小车需要用到什么外设，一般小车，能做三轮的最好都做成是三轮的，所以结构搭建材料选用方面我选择的是一个三轮小车车模、两个霍尔编码器电机、一个万向轮。电子元件材料方面选择STM32103C8T6最小系统板作为主控板，TB6612FNG作为电子驱动模块，OLED显示一些数据（可用可不用），两个K210模块分别用作巡线和数字识别功能（也可以用一块同时作巡线和数字识别，但是那样的话屏幕帧数很低，巡线效果会比较差）。

整体代码采用STM32CubeMX生成在Keil工程里，也就是HAL库。

其中，使用TIM1的CH1和CH4两路通道发出PWM波驱动左右电机，并且使用TIM2和TIM3的编码器模式分别读取左右电机传回的脉冲数进行闭环控制，最后用TIM4的定时器中断功能进行PID的调速以及后续功能的实现。

TIM的预分频系数配置成1-1，重装载值配置成7200-1，发出频率为10kHZ的PWM波（这个频率是最适合驱动市面上大多数的编码器电机的）

同时，TIM2和TIM3都配置成Encoder Mode也就是编码器模式，计数沿选择Encoder Mode TI1 and TI2，也就是上下沿都计数，这样就是实现了四倍频。

最后TIM4的定时器中断模式，NVIC里面开启TIM4的全局中断功能，预分频系数配置成720-1，重装载值配置成1000-1，这样就实现了TIM4每10ms中断一次。

然后开启串口1的中断接收模式，波特率配置成115200，串口2也配置成相同的模式。

  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);  //使能定时器1通道1，输出PWM波给左电机
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);  //使能定时器1通道4，输出PWM波给右电机
  HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);  //使能定时器2编码器模式
  HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);  //使能定时器3编码器模式
  HAL_TIM_Base_Start_IT(&htim4);  //使能定时器4中断，10ms一次
  HAL_UART_Receive_IT(&huart1,(uint8_t*)&Rx1_Buff,1);  //串口1接收到一个字符就中断一次
  HAL_UART_Receive_IT(&huart2,(uint8_t*)&Rx2_Buff,1);  //串口2接收到一个字符就中断一次

在主函数的/* USER CODE BEGIN 2 */和/* USER CODE END 2 */之间加入上述代码，使能所有外设。

下面来看整个工程的核心代码，定时器4的中断回调函数中的内容：

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)  //定时器4每10ms发生一次中断
{
	if(htim->Instance==TIM4)
	{		
		if(time_count++%5==0)
			key_scan();
		if(pid_flag==1)
		{
			static int16_t Current_Speed_L; 
			static int16_t Current_Speed_R;
			static int16_t Output_L;
			static int16_t Output_R;
			Current_Speed_L=Get_Speed(2);  //读取左电机编码器数值
			Current_Speed_R=Get_Speed(3);   //读取右电机编码器数值
			Output_L=Increment_Pid(Current_Speed_L,Speed_Set_L);
			Output_R=Increment_Pid(Current_Speed_R,Speed_Set_R);
			Set_Speed_L(Output_L);  //将PID运算的结果输出给左电机
			Set_Speed_R(Output_R);  //将PID运算的结果输出给左电机
		}
		if(pid_flag==2)
		{	
			static int16_t Current_Speed_L;  
			static int16_t Current_Speed_R;
			static int16_t Output_L;
			static int16_t Output_R;
			Current_Speed_L=Get_Speed(2);  //读取左电机编码器数值
			Current_Speed_R=Get_Speed(3);  //读取右电机编码器数值
			Left_Sum+=Current_Speed_L;   //将左边电机的脉冲数累加
			Right_Sum+=Current_Speed_R;  //将右边电机的脉冲数累加
			Output_L=Position_Pid(Left_Sum,Position_Set_L);
			Output_R=Position_Pid(Right_Sum,Position_Set_R);
			Set_Speed_L(Output_L);  //将PID运算的结果输出给左电机
			Set_Speed_R(Output_R);
		}
		else
		{
			Set_Speed_L(0);  //将PID运算的结果输出给左电机
			Set_Speed_R(0);
		}
	}
}

 这里的pid_flag是pid模式标志位，是为了判断要用速度环还是位置环，方便进行控制。其中Increment_PID和Position_PID分别是自己写的增量式PID和位置式PID的函数

int16_t Position_Pid(int16_t Current_Position,int16_t Target_Position)  //直立环PID控制
{
	static int16_t Error;
	static int16_t Output;
	static int16_t Last_Error;
	static int16_t Error_Sum;
	
	Error=Target_Position-Current_Position;
	Error_Sum+=Error;
	if(Error_Sum>5000)
		Error_Sum=5000;
	if(Error_Sum<-5000)
		Error_Sum=-5000;
	Output=Position_Kp*Error+Position_Ki*Error_Sum+Position_Kd*(Error-Last_Error);  
	Last_Error=Error;
	return Output;
}

int16_t Increment_Pid(int16_t Current_Speed,int16_t Target_Speed)  //左电机速度环增量式PI控制
{
	static int16_t Error;
	static int16_t Output;
	static int16_t Last_Error;
	Error=Target_Speed-Current_Speed;
	Output+=Speed_Kp*Error+Speed_Ki*(Error-Last_Error);
	Last_Error=Error;
	return Output;
}

增量式PID主要用在电机转速控制中，优点是不需要积分部件，也就是不需要对前面时刻的误差进行累加，只需要知道上次的误差就可以了。

下面我会更新视觉识别红线并传回误差的部分，并且有视觉模块与单片机串口通信的部分，未完待续。