stm32平衡车

一.所需材料

二.PID算法（简单说明）

直立环

速度环

串级PID

三.使用到的外设

1.定时器输出比较-PWM

2.定时器编码器模式

3.编码器读取速度

4.电机驱动函数

5.外部中断

四、小车

调试

一.所需材料

1.陀螺仪MPU6050--读取三轴的加速度和角速度，然而我们平衡车需要的不是加速度和角速度，而是需要实时的角度，即我们还需要进行姿态角转换，可以直接移植正点原子的DMP函数

2.TB6612电机驱动，l298N也可，我用的是TB6612，点击查看接线

3.OLED，用来显示姿态角，可以直接移植

4.12v锂电池和DCDC降压模块给电机和单片机供电

5.编码电机，点击查看接线

6.c8t6单片机

二.PID算法（简单说明）

P（比例）算法：Kp*误差。

可以看出，误差越大，P的输出就越大，所以P的作用是减小误差，但是只有P算法会发生超调现象，即在不受外力下可以看作简谐运动，一直震荡

I（积分）算法：Ki*误差的积分。

可以看出，只要存在误差（一般叫做稳态误差，比如小车在平衡位置偏左或右一点点，但是又不会倒下等等的情况下产生的角度误差）就会一直累加，累加累加的总误差就变的很大，滚雪球一样，越来越大，所以I可以消除稳态误差

D（微分）：Kd*误差的微分。

可以看出，两次误差之差反映的是系统的反应速度，响应越快，误差之差越大，D输出很大，反之很小，所以D算法会抑制过冲，消弱系统的反应速度。例如简谐运动，以最下点为期望点，小球从空中开始速度很小，后面越来越快，即可以理解为误差微分越来越大，且为负的，所以D输出就越来越大，力的方向与小球运动方向相反，若没有D算法，小球就会继续简谐运动，若此时有了D算法，小球的运动就会收到阻碍而令小球到达的高度降低，长此以往，小球就会静止在最低点

也可以这样看，在水中挥拳，挥的速度越大，受到的阻尼越大

直立环

PWM_OUT=Kp*误差+Kd*误差的微分

误差=真实角度-期望角度

误差的微分=角速度，姿态角转换得到

作用：让小车保持短时间的平衡，但是推一下就可能倒下

/*
直立环
PWM_OUT = Kp*角度偏差+Kd*角度偏差的微分
角度偏差求微分就是对角度偏差求导，即为角速度
gyro_Y：俯仰角的角速度
*/int Vertical(float Expect_Angle,float Angle,float gyro_Y)
{int PWM_out1=0;PWM_out1 = Vertical_Kp*(Expect_Angle-Angle)+Vertical_Kd*(gyro_Y-0);return PWM_out1;
}
/*

速度环

PWM_OUT=Kp*误差+Ki*误差的积分

误差：期望速度-真实速度

误差的积分=误差累加

作用：消除稳态误差，加强系统的反应速度

极性问题：

速度环在平衡一类的基本上都是用正反馈，即积分误差，放大误差，先注释掉直立环，则极性正确的现象就是转动一下轮子，立刻就会加速到最大，因为我们速度理论值是0，我们转了一下，产生了误差，速度环不断放大误差，使速度环输出越大，就会让轮子不断加速到最大速度，若是负反馈，我们很难转动轮子

int Velocity(int Target,int left,int right)
{	static int Encoder_S,EnC_Err_Lowout_last,PWM_out3,Encoder_Err,EnC_Err_Lowout;float a = 0.7;Encoder_Err = left+right-Target;//速度偏差//对速度偏差进行一阶低通滤波EnC_Err_Lowout = a*EnC_Err_Lowout_last+(1-a)*Encoder_Err;EnC_Err_Lowout_last=EnC_Err_Lowout;//积分Encoder_S+=EnC_Err_Lowout;//限幅Encoder_S=I_Limit(Encoder_S);PWM_out3 = Velocity_Kp*EnC_Err_Lowout + Velocity_Ki*Encoder_S;return PWM_out3;	
}

int I_Limit(int Encoder_S)
{if(Encoder_S>10000)Encoder_S=10000;else if(Encoder_S<-10000)Encoder_S=-10000;else Encoder_S=Encoder_S;return Encoder_S;
}

速度环必须滤波，因为速度会突变，突变的速度会对系统产生很大影响

滤波：编码器测到的数据（速度）是存在一些突变的，这些突变会导致系统的不稳定，所以我们要滤除这些突变，这就是滤波，类似于模电里面的“滤除电路中的高频信号”

一阶低通滤波：把权重给到上次滤波后的速度，另一个这次的速度为（1-a）倍，0.5<a<1

积分限幅：误差的积分在一直累加，若没有限幅，积分会越来越大，举个例子：小车倒下了，我们为了保护电机而关掉了电机，但是单片机的电源并没有关掉，误差一直在累加，很大很大了，当我们把小车放好并打开电机后，由于I的作用根本无法平衡。所以要限幅。

串级PID

串级，顾名思义，就是一个环的输出作为另一个环的输入，例如速度环的输出作为直立环的输入，或者直立环的输出作为速度环的输入

速度环输入：1.给定速度。2.速度反馈。

输出：角度值（直立环的期望速度输入）

直立环输入：1.给定角度（速度环输出）。2.角度反馈

输出：PWM（直接控制小车）

Vertical_out=Kp1*(Angle-Expect_Angle)+Kd* gyro_y

直立环输出=Kp1*（真实角度-期望角度+机械中值）+Kd*角度偏差的微分

Velocity_out =Kp2*(Encoder_ real- Encoder_ expect)+Ki*(Encoder_ real- Encoder_ expect)的积分

速度环输出=Kp2*（反馈编码器值-期望编码器值）+Ki*编码器偏差的积分

合并推导：Expect_Angle = Velocity_out

Vertical_out = Kp1*{ Angle-[ Kp2*(Encoder_ real- Encoder_ expect)+Ki*Σ(Encoder_ real- Encoder_ expect) ]}+Kd*gyro_y

=Kp1*真实角度+ Kd*角度偏差的微分-Kp1* [Kp2*编码器偏差- Ki *编码器偏差的积分]

三.使用到的外设

1.定时器输出比较-PWM

void PWM_Init_TIM1(uint16_t Psc,uint16_t Per)
{//开时钟,AFIORCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);GPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;//复用推挽GPIO_InitStruct.GPIO_Pin  = GPIO_Pin_8 | GPIO_Pin_11;GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;GPIO_Init(GPIOA,&GPIO_InitStruct);TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//不分频----滤波器的采样频率，可以由内部时钟直接提供，//也可以由内部时钟加一个时钟分频而来，//分频系数就是由TIM_ClockDivision决定TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数TIM_TimeBaseInitStruct.TIM_Period = Per;//ARR的值TIM_TimeBaseInitStruct.TIM_Prescaler = Psc;//PSC的值//TIM_TimeBaseInitStruct.TIM_RepetitionCounter  重复计数器，这里不需要TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);TIM_OCInitTypeDef  TIM_OCInitStruct;TIM_OCStructInit(&TIM_OCInitStruct);//初始化输出比较结构体TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;//输出比较模式 TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; //极性，有效电平为高电平TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;//输出使能TIM_OCInitStruct.TIM_Pulse = 0;//CCRTIM_OC1Init(TIM1,&TIM_OCInitStruct);TIM_OC4Init(TIM1,&TIM_OCInitStruct);TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器特有的，必须使能//使能ARR影子寄存器TIM_ARRPreloadConfig(TIM1,ENABLE);//使能输出比较预装载值寄存器TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);TIM_Cmd(TIM1,ENABLE);
}

2.定时器编码器模式

//配置编码器
//编码器1-PA0/PA1-TIM2
//编码器2-PB6/PB7-TIM4
void Encoder_TIM2_Init()
{//开启GPIO时钟RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启定时器时钟RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);//初始化GPIOGPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;GPIO_InitStruct.GPIO_Pin  = GPIO_Pin_0 | GPIO_Pin_1;GPIO_Init(GPIOA,&GPIO_InitStruct);//初始化定时器	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//滤波分频系数TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数TIM_TimeBaseInitStruct.TIM_Period = 65535;//ARR的值TIM_TimeBaseInitStruct.TIM_Prescaler = 0;//PSC的值//TIM_TimeBaseInitStruct.TIM_RepetitionCounter  重复计数器，这里不需要TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);//编码器特有的配置函数,配置模式TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//初始化输入捕获TIM_ICInitTypeDef TIM_ICInitStruct;TIM_ICStructInit(&TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下TIM_ICInitStruct.TIM_ICFilter = 10;// 滤波器，范围是 0x0 and 0xFTIM_ICInit(TIM2,&TIM_ICInitStruct);//清除定时器溢出中断标志位TIM_ClearFlag(TIM2,TIM_FLAG_Update);//配置溢出中断TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//设置计数器初始值为0TIM_SetCounter(TIM2,0);//开启定时器TIM_Cmd(TIM2,ENABLE);
}void Encoder_TIM4_Init()
{//开启GPIO时钟RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启定时器时钟RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//初始化GPIOGPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;GPIO_InitStruct.GPIO_Pin  = GPIO_Pin_6 | GPIO_Pin_7;GPIO_Init(GPIOB,&GPIO_InitStruct);//初始化定时器	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//外部时钟滤波分频系数TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数TIM_TimeBaseInitStruct.TIM_Period = 65535;//ARR的值TIM_TimeBaseInitStruct.TIM_Prescaler = 0;//PSC的值//TIM_TimeBaseInitStruct.TIM_RepetitionCounter  重复计数器，这里不需要TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);//编码器特有的配置函数TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置IC输入TIM_ICInitTypeDef TIM_ICInitStruct;TIM_ICStructInit(&TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置，所以这里先初始化一下TIM_ICInitStruct.TIM_ICFilter = 10;// 滤波器TIM_ICInit(TIM4,&TIM_ICInitStruct);//清除定时器溢出中断标志位TIM_ClearFlag(TIM4,TIM_FLAG_Update);//配置溢出中断TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);//设置计数器初始值为0TIM_SetCounter(TIM4,0);//开启定时器TIM_Cmd(TIM4,ENABLE);
}

3.编码器读取速度

/*******************
编码器速度读取函数*入口参数：定时器2/4
*******************/
int Read_Speed(int TIMx)
{int value_1;switch(TIMx){//int ? shortcase 2:value_1=(short)TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0);break; //先读取编码器的计数值，然后清零计数值，目的是直接获得速度，而不用计数值相减来算速度case 4:value_1=(short)TIM_GetCounter(TIM4);TIM_SetCounter(TIM4,0);break;default :value_1 = 0;}//这里TIM_GetCounter()函数的返回值是uint16_t ,而我们的返回值是有符号的int类型，所以这里强制类型转化一下return value_1;
}

4.电机驱动函数


//电机初始化函数
void Motor_Init()
{RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);GPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;//复用推挽GPIO_InitStruct.GPIO_Pin  = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15 ;GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;GPIO_Init(GPIOB,&GPIO_InitStruct);
}//限幅函数
void Limit(int *motorA ,int *motorB)
{if(*motorA > PWM_MAX)*motorA=PWM_MAX;if(*motorA < PWM_MIN)*motorA=PWM_MIN;if(*motorB > PWM_MAX)*motorB=PWM_MAX;if(*motorB < PWM_MIN)*motorB=PWM_MIN;
}//绝对值函数
int abs(int p)
{int q;q = p>0?p:(-p);return q;
}//电机驱动函数
//入口参数：PID运算完成后最终PWM值
void Load(int moto1,int moto2)
{//判断正反转if(moto1>0)Ain1=1,Ain2=0;else 			 Ain1=0,Ain2=1;//输入PWM的绝对值TIM_SetCompare1(TIM1,abs(moto1));if(moto2>0)Bin1=1,Bin2=0;else 			 Bin1=0,Bin2=1;TIM_SetCompare4(TIM1,abs(moto2));
}

//电机为10KHZ
//72000000/7200 = 10 000 HZ
#define PWM_MAX		 7200	
#define PWM_MIN		-7200extern int MOTO1,MOTO2;
#define Ain1	PBout(14)
#define Ain2	PBout(15)#define Bin1	PBout(13)
#define Bin2	PBout(12)

5.外部中断

dmp读取是用的中断读取，10ms读取一次，所以我们要配置MPU6050的外部中断

void MPU6050_EXTI_Init(void)
{//开启时钟//因为是复用功能，所以开启AFIORCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//配置GPIOGPIO_InitTypeDef GPIO_InitStruct;GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPU;//GPIO_Mode_AF_PP;复用推挽GPIO_InitStruct.GPIO_Pin  = GPIO_Pin_5;GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;GPIO_Init(GPIOB,&GPIO_InitStruct);	//映射GPIO和外部中断GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//配置EXTI结构体EXTI_InitTypeDef  EXTI_InitStruct;EXTI_InitStruct.EXTI_Line = EXTI_Line5;//中断线EXTI_InitStruct.EXTI_LineCmd = ENABLE;//使能中断线EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;//中断模式EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿触发EXTI_Init(&EXTI_InitStruct);}void NVIC_Config()
{NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//外部中断NVIC_InitTypeDef  NVIC_InitStruct;NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn;NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;NVIC_Init(&NVIC_InitStruct);//串口NVIC_InitStruct.NVIC_IRQChannel = USART1_IRQn;NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority =0;NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1;NVIC_Init(&NVIC_InitStruct);
}void EXTI9_5_IRQHandler(void)
写中断服务函数，里面做的是PID算法
void EXTI9_5_IRQHandler(void)
{		if(EXTI_GetITStatus(EXTI_Line5) == SET)//MPU6050外部中断是PB5{if(PBin(5) == 0)//引脚定义为外部上拉，而外部中断触发模式是下降沿，这里就是再进行下降沿的检测{EXTI_ClearITPendingBit(EXTI_Line5);//1.采集编码器数据和MPU6050的角度信息Encoder_Left  = -Read_Speed(2);//因为电机是相对安装的，即左右电机相差180度，为了让编码器输出极性一致，Encoder_Right = Read_Speed(4);//所以需要取反，或者调转一下极性也可以mpu_dmp_get_data(&Pitch,&Roll,&Yaw);		MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);MPU_Get_Accelerometer(&aacx,&aacy,&aacz);//2.将数据压入闭环控制中，计算输出控制量			Velocity_out = Velocity(Target_Speed,Encoder_Left,Encoder_Right);Vertical_out = Vertical(Velocity_out+Med_Angle,Pitch,gyroy);Turn_out = Turn(gyroz);	PWM_out = Vertical_out;//3.把控制量加载到电机上MOTO1 = PWM_out-Turn_out;//这里一加一减是因为两个电机是反向安装的，实际上就是同向加减了MOTO2 = PWM_out+Turn_out;Limit(&MOTO1,&MOTO2);Load(MOTO1,MOTO2);}}
}

四、小车

本人不会打板，所以选择了做一回杜邦线战神！

调了直立环和速度环，但是不理想，不能很好的回位

调试

串级的PID调试时应该断掉两个环的连接

机械中值：把平衡小车放在地面上，从前向后以及从后向前绕电机轴旋转平衡小车，两次的向另一边倒下的角度的中值，就是机械中值。

直立环

Kp极性：

极性错误：小车往哪边倒，车轮就往反方向开，会使得小车加速倒下。

极性正确：小车往哪边倒，车轮就往哪边开，以保证小车有直立的趋势。

Kp大小：

Kp一直增加，直到出现大幅低频震荡。

Kd极性：

极性错误：拿起小车绕电机轴旋转，车轮反向转动，无跟随。

极性正确：拿起小车绕电机轴旋转，车轮同向转动，有跟随。

Kd大小：

Kd一直增加，直到出现高频震荡。

直立环调试完毕后，对所有确立的参数乘以0.6作为最终参数。

原因：之前得到的参数都是Kp、Kd最大值，根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。

结果：乘以0.6后，小车的抖动消失，但同时直立效果也变差。待下面加入速度环就能得到更好的性能。

在调试速度环参数极性时：需要去掉（注释掉）直立环运算

在调试速度环参数大小时：再次引入（取消注释）直立环运算

Kp&Ki：

线性关系、Ki=(1/200)*Kp、仅调Kp即可。

Kp&Ki极性：

极性错误：手动转动其中一个车轮，另一车轮会以同样速度反向旋转——典型负反馈。

极性正确：手动转动其中一个车轮，两个车伦会同向加速，直至电机最大速度——典型正反馈。

Kp&Ki大小：

增加Kp&Ki，直至：小车保持平衡的同时，速度接近于零。且回位效果较好。