从0实现一辆平衡小车

LW836

已于 2023-10-26 17:28:10 修改

阅读量253

点赞数 1

分类专栏： STM32项目 STM32 文章标签：单片机嵌入式硬件

于 2023-10-26 16:09:37 首次发布

本文链接：https://blog.csdn.net/weixin_61909518/article/details/134055874

版权

STM32 同时被 2 个专栏收录

14 篇文章

订阅专栏

STM32项目

1 篇文章

订阅专栏

平衡小车电路框架

硬件部分:STM32,MPU6050陀螺仪,TB6612FNG电机驱动,电池12V,霍尔编码器直流电机,以及支架,面包板,杜邦线.

引脚资源分配

STM引脚资源分配
IO编号	资源说明	外设	备注
A12	普通IO	MPU6050的INT引脚	检测MPU6050是否产生数据就绪中断
B8、B9	普通IO	MPU6050	软件模拟IIC的SCL与SDA
A8、B15、B14	PWMA、IO	左轮电机	电机控制信号：PWM大小、电机方向
A11、B12、B13	PWMB、IO	右轮电机	电机控制信号：PWM大小、电机方向
A0、A1	TIM2_CH1、CH2	左轮编码器	编码器采集
B6、B7	TIM4_CH1、CH2	右轮编码器	编码器采集
其他任意	普通IO	OLED显示屏	软件模拟IIC的

1 电机驱动代码

在此平衡小车项目中,使用的是带霍尔编码器的直流电机以及TB6612FNG电机驱动器件.

TB6612FNG具有大电流的MOSFET-H桥结构,双通道带你路输出,可同时驱动2个电机, 相比 L298N 的热耗性和外围二极管续流电路，它无需外加散热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机,对于 PWM 信号输入频率范围，高达 100 kHz 的频率更是足以满足大部分需求.

电机驱动使用TIM1, 初始化为 PWM 输出，CH1，CH4 输出双路 10KHZ 的 PWM 控制电机

TB6612.c

#include "./TB6612/TB6612.h"  

/**************************************************************************
函数功能：TB6612初始化函数
入口参数：定时器3计数上限 定时器3预分频系数
返回  值：无
**************************************************************************/
void TB6612_Init(int arr, int psc)
{
	GPIO_InitTypeDef GPIO_InitStructure; //定义一个引脚初始化的结构体
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStrue; //定义一个定时中断的结构体	
	TIM_OCInitTypeDef TIM_OCInitTypeStrue; //定义一个PWM输出的结构体
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能GPIOB时钟，GPIOB挂载在APB2时钟下，在STM32中使用IO口前都要使能对应时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); //使能通用定时器1时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
  
	//TB6612控制方向引脚
	GPIO_InitStructure.GPIO_Pin=BIN1_PIN|BIN2_PIN|AIN1_PIN|AIN2_PIN; 
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP; //引脚输入输出模式为推挽输出模式
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; //引脚输出速度为50MHZ
	GPIO_Init(BIN1_PORT, &GPIO_InitStructure); //根据上面设置好的GPIO_InitStructure参数，初始化引脚
	
	GPIO_ResetBits(BIN1_PORT, BIN1_PIN|BIN2_PIN|AIN1_PIN|AIN2_PIN); //初始化设置引脚低电平

	//TB6612PWM输出引脚
	GPIO_InitStructure.GPIO_Pin=PWMB_PIN|PWMA_PIN;//引脚PA8,PA11
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP; //复用推挽输出模式，定时器功能为引脚复用功能
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; //定义该引脚输出速度为50MHZ
  GPIO_Init(PWMAB_PORT, &GPIO_InitStructure); //初始化引脚GPIOB1
	
	TIM_TimeBaseInitStrue.TIM_Period=arr; //计数模式为向上计数时，定时器从0开始计数，计数超过到arr时触发定时中断服务函数
	TIM_TimeBaseInitStrue.TIM_Prescaler=psc; //预分频系数，决定每一个计数的时长
	TIM_TimeBaseInitStrue.TIM_CounterMode=TIM_CounterMode_Up; //计数模式：向上计数
	TIM_TimeBaseInitStrue.TIM_ClockDivision=TIM_CKD_DIV1; //一般不使用，默认TIM_CKD_DIV1
//	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStrue); //根据TIM_TimeBaseInitStrue的参数初始化定时器TIM3
	TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStrue);
  
  
	TIM_OCInitTypeStrue.TIM_OCMode=TIM_OCMode_PWM1; //PWM模式1，当定时器计数小于TIM_Pulse时，定时器对应IO输出有效电平
	TIM_OCInitTypeStrue.TIM_OCPolarity=TIM_OCNPolarity_High; //输出有效电平为高电平
	TIM_OCInitTypeStrue.TIM_OutputState=TIM_OutputState_Enable; //使能PWM输出
	TIM_OCInitTypeStrue.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
//	TIM_OC4Init(TIM3, &TIM_OCInitTypeStrue); //根TIM_OCInitTypeStrue参数初始化定时器3通道4
  TIM_OC4Init(TIM1, &TIM_OCInitTypeStrue);
  TIM_OC1Init(TIM1, &TIM_OCInitTypeStrue); 
  TIM_CtrlPWMOutputs(TIM1,ENABLE);	//MOE 主输出使能

	TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Disable); //CH4预装载使能 使能后改变TIM_Pulse(即PWM)的值立刻生效，不使能则下个周期生效
	TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Disable);
	TIM_ARRPreloadConfig(TIM1, ENABLE); //TIM3预装载使能
	
	TIM_Cmd(TIM1, ENABLE); //使能定时器TIM3
}

/**************************************************************************
函数功能：设置TIM3通道4PWM值
入口参数：PWM值
返回  值：无
**************************************************************************/
void SetPWM(int PWM_L, int PWL_R)
{
  if(PWM_L>0)	          BIN1=1,			BIN2=0; //前进 
	else           			  BIN1=0,			BIN2=1; //后退
	PWMB=ABS(PWM_L);
  if(PWL_R>0)			      AIN2=0,			AIN1=1;	//前进
	else 	        			  AIN2=1,			AIN1=0; //后退
	PWMA=ABS(PWL_R);
}


static int ABS(int n)
{ 		   
	  int temp;
		if(n<0)  temp=-n;  
	  else temp=n;
	  return temp;
}

TB6612.h

#ifndef __TB6612_H 
#define __TB6612_H 
#include "sys.h"


#define BIN1_PIN  GPIO_Pin_13
#define BIN1_PORT GPIOB
#define BIN2_PIN  GPIO_Pin_12
#define BIN2_PORT GPIOB
#define AIN1_PIN  GPIO_Pin_14
#define AIN1_PORT GPIOB
#define AIN2_PIN  GPIO_Pin_15
#define AIN2_PORT GPIOB

#define PWMA_PIN  GPIO_Pin_11
#define PWMB_PIN  GPIO_Pin_8
#define PWMAB_PORT  GPIOA
#define PWMB   TIM1->CCR1  //PA8
#define PWMA   TIM1->CCR4  //PA11


#define BIN2   PBout(12)
#define BIN1   PBout(13)
#define AIN2   PBout(15)
#define AIN1   PBout(14)


void TB6612_Init(int arr, int psc);
void SetPWM(int PWM_L, int PWL_R);


#endif

2 编码器代码

电机集成霍尔传感器，能够读出脉冲计数，从而得到电机速度。在程序中，我们使用 M 法测速，即在一定时间周期内，测量编码器输出的脉冲个数M1来计算转速，用个数除以时间就可以得到编码器输出脉冲的频率。频率F1 = M1/Tc。因存在测量时间内首尾变个脉冲问题，可能会有两个脉冲的误差，但在高速下可以忽略不计。

假设电机转动一圈可以产生 Z 个脉冲，其中 Z=4*编码器线数*电机减速比，这里 4 代表四倍频，同时采集 A 和 B 相的上升沿和下降沿，用频率F1除以一圈脉冲个数就可以得到单位时间内的转速(单位：转/分钟)：

$N = \frac{f1}{Z}*60=60*\frac{M1}{Z*Tc} (r/min)$

编码器使用TIM2,TIM4,初始化为正交编码器模式，硬件采集编码器 4, 2 的数据

MotorEncoder.c

#include "./MotorEncoder/MotorEncoder.h" 

//外部变量 extern说明改变量已在其它文件定义
extern int   Encoder, CurrentPosition; //当前速度、当前位置
extern int   TargetVelocity, TargetCircle, CurrentPosition, Encoder,PWM; //目标速度、编码器读数、PWM控制变量
extern float Velocity_Kp,  Velocity_Ki,  Velocity_Kd; //相关速度PID参数
extern float Position_Kp, Position_Ki, Position_Kd; //相关位置PID参数
extern int   MotorRun;  //允许电机控制标志位

/**************************************************************************
Function: Initialize TIM2 to encoder interface mode
Input   : none
Output  : none
函数功能：把TIM2初始化为编码器接口模式
入口参数：无
返回  值：无
**************************************************************************/
void Encoder_Init_TIM2(void)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;  
  TIM_ICInitTypeDef TIM_ICInitStructure;  
  GPIO_InitTypeDef GPIO_InitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器4的时钟
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PB端口时钟
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;	//端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
  GPIO_Init(GPIOA, &GPIO_InitStructure);					      //根据设定参数初始化GPIOA
  
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
  TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 
  TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //设定计数器自动重装值
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上计数  
  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
  TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising,         
  TIM_ICPolarity_Rising);//使用编码器模式3
  TIM_ICStructInit(&TIM_ICInitStructure);
  TIM_ICInitStructure.TIM_ICFilter = 10;	//滤波10
  TIM_ICInit(TIM2, &TIM_ICInitStructure);
  TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
  //Reset counter
  TIM_SetCounter(TIM2,0);
  TIM_Cmd(TIM2, ENABLE); 
}


/**************************************************************************
函数功能：编码器初始化函数
入口参数：无
返回  值：无
**************************************************************************/
void Encoder_Init_Tim4(void)
{
	GPIO_InitTypeDef GPIO_InitStructure; //定义一个引脚初始化的结构体  
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;//定义一个定时器初始化的结构体
  TIM_ICInitTypeDef TIM_ICInitStructure; //定义一个定时器编码器模式初始化的结构体
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //使能TIM4时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能CPIOB时钟
 
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;	//TIM4_CH1、TIM4_CH2
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
	GPIO_Init(GPIOB, &GPIO_InitStructure);	//根据GPIO_InitStructure的参数初始化GPIOB

	TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //设定计数器自动重装值
	TIM_TimeBaseStructure.TIM_Prescaler = 0; // 预分频器 
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //选择时钟分频：不分频
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct的参数初始化定时器TIM4
	
  TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); //使用编码器模式3：CH1、CH2同时计数，为四分频
  TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入
	TIM_ICInitStructure.TIM_ICFilter = 10;  //设置滤波器长度
  TIM_ICInit(TIM4, &TIM_ICInitStructure); //根TIM_ICInitStructure参数初始化定时器TIM4编码器模式
	
	TIM_ClearFlag(TIM4, TIM_FLAG_Update);//清除TIM的更新标志位
  TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //更新中断使能
  TIM_SetCounter(TIM4,0); //初始化清空编码器数值
	
	TIM_Cmd(TIM4, ENABLE); //使能定时器4
}



/**************************************************************************
Function: Read encoder count per unit time
Input   : TIMX：Timer
Output  : none
函数功能：单位时间读取编码器计数
入口参数：TIMX：定时器
返回  值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)
{
   int Encoder_TIM;    
   switch(TIMX)
	 {
	   case 2:  Encoder_TIM= (short)TIM2 -> CNT;  TIM2 -> CNT=0;break;	
		 case 4:  Encoder_TIM= (short)TIM4 -> CNT;  TIM4 -> CNT=0;break;	
		 default: Encoder_TIM=0;
	 }
		return  Encoder_TIM;
}

/**************************************************************************
函数功能：TIM4中断服务函数
入口参数：无
返回  值：无
**************************************************************************/
void TIM4_IRQHandler(void)
{ 		    		  			    
	if(TIM4->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM4->SR&=~(1<<0);//清除中断标志位 	    
}




/**************************************************************************
函数功能：TIM2中断服务函数
入口参数：无
返回  值：无
**************************************************************************/
void TIM2_IRQHandler()
{
  

  if(TIM2->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM2->SR&=~(1<<0);//清除中断标志位 	  

}

MotorEncoder.h

#ifndef __MOTORENCODER_H 
#define __MOTORENCODER_H 
#include "sys.h"
#include "./TB6612/TB6612.h"

#define ENCODER_TIM_PERIOD (u16)(65535)   //不可大于65535 因为F103的定时器是16位的。



void Encoder_Init_Tim4(void); 
void Encoder_Init_TIM2(void);
int Read_Encoder(u8 TIMX);

#endif

3 MPU6050中断代码

EXTI.c

#include "./EXTI/EXTI.h"
/**************************************************************************
Function: External interrupt initialization
Input   : none
Output  : none
函数功能：外部中断初始化
入口参数：无
返回  值：无 
**************************************************************************/
void MPU6050_EXTI_Init(void)
{  
		GPIO_InitTypeDef GPIO_InitStructure;
		EXTI_InitTypeDef EXTI_InitStructure;
		NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);   //外部中断，需要使能AFIO时钟
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能GPIO端口时钟
		GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;	          //端口配置
  
    GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;         //上拉输入
		GPIO_Init(GPIOA, &GPIO_InitStructure);					      //根据设定参数初始化GPIO
  	GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource12);
  	EXTI_InitStructure.EXTI_Line=EXTI_Line12;
  	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;	
  	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿触发
  	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
  	EXTI_Init(&EXTI_InitStructure);	 	//根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
		NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;			//使能按键所在的外部中断通道
  	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;	//抢占优先级2， 
  	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01;					//子优先级1
  	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;								//使能外部中断通道
  	NVIC_Init(&NVIC_InitStructure); 
}

EXTI.h

#ifndef __EXTI_H
#define __EXIT_H	 
#include "sys.h"
#define INT PAin(12)   //PA12连接到MPU6050的中断引脚
void MPU6050_EXTI_Init(void);	//外部中断初始化		 					    
#endif

4 OLED以及MPU6050代码

OLED代码可参考之前历程OLED-IIC软硬件

MPU6050网上教程多,能读到数据即可

读取数据函数如下

//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
//返回值:0,正常
//    其他,失败
u8 MPU6050_DMP_Get_Data(float *pitch,float *roll,float *yaw)//（5MS  200HZ调用一下，给 DEFAULT_MPU_HZ 频率保持一致，或者int中断引脚读取）
{                              //  （此处在主函数中一直调用，在中断函数里更新 频率未变）
  unsigned char more;
  unsigned long sensor_timestamp; 
	long quat[4]; 
	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;	 
	/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
	 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
	**/
	/*if (sensors & INV_XYZ_GYRO )
	send_packet(PACKET_TYPE_GYRO, gyro);
	if (sensors & INV_XYZ_ACCEL)
	send_packet(PACKET_TYPE_ACCEL, accel); */
	/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
	 * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 
	**/
	if(sensors&INV_WXYZ_QUAT) 
	{
		q0 = quat[0] / q30;	//q30格式转换为浮点数
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30; 
		//计算得到俯仰角/横滚角/航向角
		*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
		*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
		*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
    
    Gyro_Balance=gyro[0];              //更新平衡角速度,前倾为正，后倾为负
		Gyro_Turn=gyro[2];                 //更新转向角速度
		Acceleration_Z=accel[2];           //更新Z轴加速度计
	}else return 2;
	return 0;
}

5 中断服务控制函数和PID函数

Control.c




#include "./Control/Control.h"
#include "./EXTI/EXTI.h"

extern float Velocity_Kp,  Velocity_Ki,  Velocity_Kd; //相关速度PID参数
extern float Velocity_Left,Velocity_Right;	//车轮速度(mm/s)
float Pitch,Roll,Yaw;
/**************************************************************************
Function: Control function
Input   : none
Output  : none
函数功能：所有的控制代码都在这里面
         5ms外部中断由MPU6050的INT引脚触发 (inv_mpu.c)
         严格保证采样和数据处理的时间同步	
入口参数：无
返回  值：无				 
**************************************************************************/
int EXTI15_10_IRQHandler(void) 
{   
    int Balance_Pwm,Velocity_Pwm,Turn_Pwm,Motor_Left,Motor_Right;		  					//平衡环PWM变量，速度环PWM变量，转向环PWM变
    int Speed_Left,Speed_Right;

  if(EXTI_GetITStatus(EXTI_Line12) != 0)
  {
    if(INT==0)		
      {   
        EXTI->PR=1<<12;

        MPU6050_DMP_Get_Data(&Pitch,&Roll,&Yaw);
        Speed_Left = -Read_Encoder(4);
        Speed_Right = Read_Encoder(2);
        Get_Velocity_Form_Encoder(Speed_Left,Speed_Right);//编码器读数转速度（mm/s）
        Balance_Pwm=Balance(Pitch,Gyro_Balance);    //平衡PID控制 Gyro_Balance平衡角速度极性：前倾为正，后倾为负
        Velocity_Pwm=Velocity(Speed_Left,Speed_Right);  //速度环PID控制	记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
        Turn_Pwm=Turn(Gyro_Turn);														//转向环PID控制     
        

        Motor_Left=Balance_Pwm+Velocity_Pwm+Turn_Pwm;       //计算左轮电机最终PWM
        Motor_Right=Balance_Pwm+Velocity_Pwm-Turn_Pwm;      //计算右轮电机最终PWM

        Motor_Left=PWM_Limit(Motor_Left,8000,-8000);
        Motor_Right=PWM_Limit(Motor_Right,8000,-8000);			//PWM限幅  
        
        SetPWM(Motor_Left,Motor_Right);


      }  
	   
    }
} 

/**************************************************************************
Function: Encoder reading is converted to speed (mm/s)
Input   : none
Output  : none
函数功能：编码器读数转换为速度（mm/s）
入口参数：无
返回  值：无
**************************************************************************/
void Get_Velocity_Form_Encoder(int encoder_left,int encoder_right)
{ 	
	float Rotation_Speed_L,Rotation_Speed_R;						//电机转速  转速=编码器读数（5ms每次）*读取频率/倍频数/减速比/编码器精度
	Rotation_Speed_L = encoder_left*Control_Frequency/EncoderMultiples/Reduction_Ratio/Encoder_precision;
	Velocity_Left = Rotation_Speed_L*PI*Diameter_67;		//求出编码器速度=转速*周长
	Rotation_Speed_R = encoder_right*Control_Frequency/EncoderMultiples/Reduction_Ratio/Encoder_precision;
	Velocity_Right = Rotation_Speed_R*PI*Diameter_67;		//求出编码器速度=转速*周长
}

/**************************************************************************
Function: Vertical PD control
Input   : Angle:angle；Gyro：angular velocity
Output  : balance：Vertical control PWM
函数功能：直立PD控制		
入口参数：Angle:角度；Gyro：角速度
返回  值：balance：直立控制PWM
**************************************************************************/	
int Balance(float Angle,float Gyro)
{  
   float Angle_bias,Gyro_bias;
	 int balance;
	 Angle_bias=0-Angle;                       				//求出平衡的角度中值 和机械相关
	 Gyro_bias=0-Gyro; 
	 balance=Balance_Kp/100*Angle_bias+Gyro_bias*Balance_Kd/100; //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 
	 return balance;

}







int Velocity(int encoder_left,int encoder_right)
{  
    static float velocity,Encoder_Least,Encoder_bias,Movement;
	  static float Encoder_Integral,Target_Velocity;

		
   //================速度PI控制器=====================//	
		Encoder_Least =0-(encoder_left+encoder_right);                    //获取最新速度偏差=目标速度（此处为零）-测量速度（左右编码器之和） 
		Encoder_bias *= 0.84;		                                          //一阶低通滤波器       
		Encoder_bias += Encoder_Least*0.16;	                              //一阶低通滤波器，减缓速度变化 
		Encoder_Integral +=Encoder_bias;                                  //积分出位移 积分时间：10ms
		if(Encoder_Integral>10000)  	Encoder_Integral=10000;             //积分限幅
		if(Encoder_Integral<-10000)	  Encoder_Integral=-10000;            //积分限幅	
		velocity=Encoder_bias*Velocity_Kp/100+Encoder_Integral*Velocity_Ki/100;     //速度控制	

	  return velocity;


}


/**************************************************************************
Function: Turn control
Input   : Z-axis angular velocity
Output  : Turn control PWM
函数功能：转向控制 
入口参数：Z轴陀螺仪
返回  值：转向控制PWM
**************************************************************************/
int Turn(float gyro)
{
	 static float Turn_Target,turn,Turn_Amplitude=54;
	 float Kp=Turn_Kp,Kd;			//修改转向速度，请修改Turn_Amplitude即可

  //===================转向PD控制器=================//
	 turn=Turn_Target*Kp/100-gyro*Kd/100;//结合Z轴陀螺仪进行PD控制
	 return turn;								 				 //转向环PWM右转为正，左转为负
}


int PWM_Limit(int IN,int max,int min)
{
	int OUT = IN;
	if(OUT>max) OUT = max;
	if(OUT<min) OUT = min;
	return OUT;
}

Control.h

#ifndef  __CONTROL_H__
#define  __CONTROL_H__

#include "sys.h"
#define PI 3.14159265							//PI圆周率
#define Control_Frequency  200.0	//编码器读取频率
#define Diameter_67  67.0 				//轮子直径67mm 
#define EncoderMultiples   4.0 		//编码器倍频数
#define Encoder_precision  13.0 	//编码器精度 13线
#define Reduction_Ratio  30.0			//减速比30
#define Perimeter  210.4867 			//周长，单位mm

int Velocity(int encoder_left,int encoder_right);
void Get_Velocity_Form_Encoder(int encoder_left,int encoder_right);
int Balance(float Angle,float Gyro);
int Turn(float gyro);
int PWM_Limit(int IN,int max,int min);


#endif  /*__CONTROL_H__*/

以上代码中的sys.h,是MPU6050 DMP移植后其中的一个库,此项目中我只用来包含需要头文件和传递全局变量,按自己需要进行头文件包含和函数传递.

这里利用的是陀螺仪采集完数据会产生中断，(这里设置陀螺仪采集数据的周期为5ms (MPU6050 inv_mpu.c 初始化函数里面设置)，然后就触发单片机中断，进入中断完成相应的控制。

这里也可以不用陀螺仪中断，直接采用定时器定时10ms,然后进入定时器中断函数完成相应的控制。

进入中断后先清除中断标志位，再读取陀螺仪和编码器的数据，将数据再压入PID控制器进行计算，计算完成后将相应的值加载到电机上就可以了，进入中断调整一次小车的姿态，一直不断的调整，这样就大概完成了平衡小车的制作。

PID极性和数据调节

平衡小车的PID函数由三部分构成，分别为直立环，速度环，转向环三部分构成，直立环就是让小车角度趋近0，速度环就是让电机速度趋近0，转向环就是让电机保持角速度为零。

直立环调节: 先确定 Balance_Kp 的极性。若小车倾斜时也向倾斜的方向加速，那么极性就是对的，反之，即为错的, Balance_Kd同理; 然后首先调节 Balance_Kp，每次增大 Balance_Kp 直至小车出现低频抖动，保留此时的值。增大 Balance_Kd 直至出现高频抖动，此时记录直立环的参数值，乘以 0.6 作为直立环最终参数。

速度环调节:首先也是确认极性, 先把直立环和转向环屏蔽，单独调试速度环。当转动一只轮子时，另外一只轮子速度相同方向转动，那么速度环极性就是对的; 把上述所得的直立环最终参数保留，即乘以 0.6 的参数。然后开始调节速度环。每次增大速度环的Velocity_Kp值，其中Velocity_Ki值也要设定为Velocity_Kp 的 1/200，观察小车是否能立在原地。若增大到小车出现抖动，而且用手推动小车会发生大幅回摆，说明此参数不可取，应减少到适合的数值

转向环调节: 屏蔽直立环和转向环，单独调试转向环。当我们用手摁着小车在地上旋转时，若出现电机对抗我们的旋转的现象，那就说明极性是对的，若很容易旋转，说明转向环帮助了我们旋转，这个时候极性是错的。取 Kd 参数为 0.6，用手摁着小车在地上旋转，可以发现，此时电机出现了对抗我们的旋转的现象，说明此时极性是对的，Kd 参数取正; 保留调试好的直立环和速度环，开始调节转向环 Kd 参数的大小。逐渐加大转向环 Kd 参数，令小车前进，观察小车走直线的效果。若小车出现了高频抖动的现象，说明此参数不可取，取前面的相对理想的结果

6 main 函数

#include "stm32f10x.h"	
#include "./OLED/OLED.h"
#include "./LED/bsp_led.h"
#include "Delay.h"
#include "inv_mpu.h"
#include "./TB6612/TB6612.h"
#include "./MotorEncoder/MotorEncoder.h" 
#include "bsp_usart.h"
#include "./Control/Control.h"
#include "sys.h"
#include "./EXTI/EXTI.h"



int   MotorRun;  //允许电机控制标志位
float Velocity_Left,Velocity_Right;	//车轮速度(mm/s)
Balance_Kp=39000,Balance_Kd=108,Velocity_Kp=151000,Velocity_Ki=755,Turn_Kp=0,Turn_Kd=0;//PID参数（放大100倍）
extern float Gyro_Balance,Gyro_Turn,Acceleration_Z;
extern float Pitch,Roll,Yaw;

int main(void)
{


  delay_init();
  LED_GPIO_Config();
  MPU6050_DMP_Init();
  TB6612_Init(7199, 0);        //电机驱动外设初始化 使用定时器1
  OLED_Init();
  delay_ms(2000);              //延迟等待初始化完成
  	
  MotorRun = 0;
  Encoder_Init_Tim4(); 
  Encoder_Init_TIM2();
  delay_ms(2000);
  MPU6050_EXTI_Init();	//外部中断初始化  
  LED_G(1);
  while(1)
   {
    }

}