自制【平衡小车】程序篇——基于STM32核心_stm32平衡小车pid算法程序

目录

一：程序整体构成

二：外设程序

1.角度获取

2.速度获取

3.电机控制

4.人机交互

三：PID控制器

四：下载链接

一：程序整体构成

        平衡小车站立的前提，一是对当前倾角进行测量并反馈，二是对当前速度进行反馈，前者利用MPU6050进行采集并通过I2C与主控进行信息传递，后者通过主控定时器的编码器模式进行采集，最终经过PID运算产生结果进行输出，在当前项目当中是将最终结果装载到定时器当中，经过TB6612转换为电压（即转速）来控制电机，所以整个项目可以划为6050-主控传递角度信息，编码器-主控传递速度信息，主控-6612电机控制，人机交互四个部分。

二：外设程序

1.角度获取

        博主采用6050作为角度传感器装置，网上有现成的代码可供参考，在此不做过多解释说明。6050六轴传感使用时需要注意有一个方向的速度偏移量过大，采集数据不适合作为实际数据作为传入参数进行计算，建议找到最稳定的测量方向作为小车的前后方向。

2.速度获取

        利用STM32定时器资源自带的编码器模式，对电机速度进行读取。下方代码是F103VCT6的配置，读取函数当中将其转换为short型，即将计数区间从0-65535变为-32768~32767之间，这样一来我们就可以从其正负来判断小车的驶向，并且要注意在前面初始化函数当中，重装载值必须为65535才能使无输入时读取为0。

 
void Encoder_Init_TIM4(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
	TIM_ICInitTypeDef TIM_ICInitStructure;  
	GPIO_InitTypeDef GPIO_InitStructure;
 
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); 
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB0,GPIOB1
    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB,&GPIO_InitStructure); 
 
 	/*TIM2 定时器配置*/ 
	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 分频不分频
	TIM_TimeBaseStructure.TIM_Period = 65535;  //重载初值
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //选择时钟分频：不分频
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数  
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
 
	 /*编码器配置*/ 
	TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	TIM_ICStructInit(&TIM_ICInitStructure);
	TIM_ICInitStructure.TIM_ICFilter = 10;
	TIM_ICInit(TIM4, &TIM_ICInitStructure);
	TIM_ClearFlag(TIM4, TIM_FLAG_Update); //清除TIM的更新标志位
	TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
	//Reset counter
	TIM_SetCounter(TIM4,0);
	TIM_Cmd(TIM4, ENABLE); 
}
 
//读计数值
int Read_Encoder(u8 TIMX)
{
	int Encoder_TIM;    
	switch(TIMX)
	{
		case 3:  Encoder_TIM= (short)TIM3 -> CNT;  
		         TIM3 -> CNT=0;
		         break;
		case 4:  Encoder_TIM= (short)TIM4 -> CNT;  
		         TIM4 -> CNT=0;
		         break;	
		default:  Encoder_TIM=0;
	}
	return Encoder_TIM;
}
 

3.电机控制

        博主采用的电机驱动为TB6612电机驱动，理论最大输入12V，两路可控输出，分别由两路PWM输入来控制，由四个输入口控制输出极性。但是理论终究是理论，当前常见的现成模块上的钽电容耐压值限制的实际输入电压大小，一般只有钽电容耐压达到了25V才能直接加上12V输入，如果自己制作，钽电容选用额定电压=2*输入电压即可。

        所以电机控制部分需要定时器输出PWM，4个I/O口作为方向控制部分。

4.人机交互

        屏幕和蓝牙等人机交互部分可以自由选择，人机交互部分主要是方便寻找机械中值或后期开发升级，根据不同的要求进行选择。博主选择蓝牙模块进行上位机读取数据、测试波形等操作。

三：PID控制器

        PID算法是平衡小车当中常见的控制算法，由角度环构成内环，由速度环构成外环，对平衡小车来说内环的建立实现其直立的效果，这个效果在无速度环参数添加时通常持续时间不长，如果能达到5S左右的直立状态就说明机械结构比较好。速度环加上后抗干扰性能会有提升，主要体现在长时间直立及位移后可以返回原点，对于PID函数，个人建议自己写，一是方便整合理解参数，另一方面在出错时利于修改。在整合程序时需要选择使用串级控制还是并级控制，是选用位置式PID又或是增量式PID。博主选用的是位置式PID公式+串级控制。

四：下载链接

F407ZGT6核心:https://download.csdn.net/download/qq_59194050/68349863

F103VCT6核心:https://download.csdn.net/download/qq_59194050/68353602