传统平衡小车学习

博主教学视频

 

 

代码部分

第一部分

--------------------------------------------------------------------------------------------------------------------------

1、编码器函数

Encoder.c

GPIO初始化，定时器初始化，设置编码器模式，清除标志位，中断配置，开定时器

#include "encoder.h"
//初始化GPIO口：A0/A1为TIM2；B6/B7为TIM4
void Encoder_TIM2_Init(void)
{
	//1、GPIO口开启
	GPIO_InitTypeDef GPIO_InitStruct;//定义GPIO口初始化结构体
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;//定义定时器初始化结构体
	TIM_ICInitTypeDef TIM_ICInitStruct;//定义结构体
	//2、开启时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//GPIO开启时钟
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);//开启定时器时钟
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1，浮空输入
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;//TIM2的口
	//这里不用速度因为编码器式读取它的速度数据并不是向外输出
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	//TIM_InternalClockConfig(TIM2);	
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器##
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;//定时器的时钟分割（不分）
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;//定时器的计数模式（向上计数）
	TIM_TimeBaseInitStruct.TIM_Period=65535;//定时器的重装载值
	TIM_TimeBaseInitStruct.TIM_Prescaler=0;//定时器的分频系数（不分）
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);	
	//4、输入捕获
	TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式，（定时器，模式三，上升沿，上升沿）
	TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获##
	TIM_ICInitStruct.TIM_ICFilter=10;//滤波器
	TIM_ICInit(TIM2,&TIM_ICInitStruct);
	//5、其他函数，清除本身定时器的标志位
	TIM_ClearFlag(TIM2,TIM_FLAG_Update);//（计数，计数溢出之后标志位会拉高，要把这个标志位清除一下）
	TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
	//因为编码器从一开始就要捕获通道的计数值，刚开始一定要为零，如果不是零基础就会错了，所以要清零定时器计数值
	TIM_SetCounter(TIM2,0);//清零定时器计数值（设置为0，即清零）
	//6、开启计时器
	TIM_Cmd(TIM2,ENABLE);//开启定时器
}
void Encoder_TIM4_Init(void)
{
	//1、GPIO口开启
	GPIO_InitTypeDef GPIO_InitStruct;//定义GPIO口初始化结构体
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;//定义定时器初始化结构体
	TIM_ICInitTypeDef TIM_ICInitStruct;//定义结构体
	//2、开启时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//GPIO开启时钟
	//3、配置编码器
	//开启定时器（tim.c）
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启定时器时钟
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1，浮空输入
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_6 |GPIO_Pin_7;//TIM2的口
	//这里不用速度因为编码器式读取它的速度数据并不是向外输出
	GPIO_Init(GPIOB,&GPIO_InitStruct);
	
	
	
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器##
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;//定时器的时钟分割（不分）
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;//定时器的计数模式（向上计数）
	TIM_TimeBaseInitStruct.TIM_Period=65535;//定时器的重装载值
	TIM_TimeBaseInitStruct.TIM_Prescaler=0;//定时器的分频系数（不分）
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
	
	//4、输入捕获
	TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式，（定时器，模式三，上升沿，上升沿）
	TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获##
	TIM_ICInitStruct.TIM_ICFilter=10;//滤波器
	TIM_ICInit(TIM4,&TIM_ICInitStruct);

	//5、其他函数，清除本身定时器的标志位
	TIM_ClearFlag(TIM4,TIM_FLAG_Update);//（计数，计数溢出之后标志位会拉高，要把这个标志位清除一下）
	TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
	//因为编码器从一开始就要捕获通道的计数值，刚开始一定要为零，如果不是零基础就会错了，所以要清零定时器计数值
	TIM_SetCounter(TIM4,0);//清零定时器计数值（设置为0，即清零）
	//6、开启计时器
	TIM_Cmd(TIM4,ENABLE);//开启定时器
}
//读取速度
//编码器读取函数入口参数为定时器
//编码器读的实际上是编码电机用的位移量，而不是速度，现在写的是读取速度

int Read_Speed(int TIMx)//传定时器，并且执行操作：1.采集编码器的计数值并保存;2.将定时器的计数值清零
{
	int value_1;//把读取的计数器的值给一个变量让他保存后，定时器清零
	switch(TIMx)
	{
		case 2:{value_1 = (short)TIM_GetCounter(TIM2); TIM_SetCounter(TIM2,0);};break;//记得转类型，TIM_GetCounter()是短整型int short
		case 4:{value_1 = (short)TIM_GetCounter(TIM4); TIM_SetCounter(TIM4,0);};break;
		default:value_1=0;
	}
	return value_1;
}

//中断函数，中断之后要找一个地址给他寻过去（寻址）
void TIM2_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM2,TIM_IT_Update)!=0)
	{
		TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
	}
}
	
void TIM4_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM4,TIM_IT_Update)!=0)//标志位不等于0，说明他已经进入了中断，这时候把它清零
	{
		TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
	}
}

对于编码器函数 

参数为（定时器，模式，极性，极性）

这里讲极性，但是点进去定义会发现成了上升沿或者下降沿：

原因：在stm32f10x_tim.c,(1294行)，“/*set the TI1 and the TI2 Polarities*/”,

去中文手册找查找CCER,找到通用计时器

找到CC1,编码器为输入

第二个参数：

TI1：在T1的所有边沿计数

TI2：在T2的所有边沿计数

TI12：在T1和T2的所有边沿计数

Encoder.h

#ifndef  _ENCODER_H
#define  _ENCODER_H
#include "sys.h" 
void TIM2_IRQHandler(void);
void TIM4_IRQHandler(void);
int Read_Speed(int TIMx);
void Encoder_TIM2_Init(void);
void Encoder_TIM4_Init(void);
#endif

----------------------------------------------------------------------------------------------------------------------------

2、PWM控制

PWM.c

GPIO初始化，定时器初始化，输出比较模式初始化，MOE主输出使能，通道预装载使能，使能定时器在ARR寄存器上的预装载寄存器

#include "pwm.h"
//定时器1-PA8\PA11
void PWM_Init_TIM1(u16 Psc,u16 Per)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
	//GPIO空初始化
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//初始化GPIO--PA8、PA11为复用推挽输出
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStruct);
	//初始化定时器
	TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period=Per;//设置变量
	TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;//设置参数
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);//高级定时器1
	
	//输入捕获比较
	TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1;//初始化输出比较模式:PWM1模式
	TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;//输出比较极限：计数超过CCR之后，状态为高
	TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;//输出比较状态
	TIM_OCInitStruct.TIM_Pulse=0;//初始脉冲设置为0即可
	TIM_OC1Init(TIM1,&TIM_OCInitStruct);//1通道
	TIM_OC4Init(TIM1,&TIM_OCInitStruct);//4通道
	
	TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器专属函数--MOE主输出使能（必然要开，不然输出不了PWM波形）
	
	TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);//ENABLE//OC1预装载寄存器使能
	TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);//ENABLE//OC4预装载寄存器使能
	TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1在ARR上预装载寄存器使能
	
	TIM_Cmd(TIM1,ENABLE);//开定时器。
}

PWM.h

#ifndef  _PWM_H
#define  _PWM_H

#include "sys.h" 



void PWM_Init_TIM1(u16 Psc,u16 Per);
#endif

------------------------------------------------------------------------------------------------------------------------------

3、MPU6050

Exit.c

GPIO初始化、EXTI外部中断初始化

#include "exti.h"


void MPU6050_EXTI_Init(void)
{
	EXTI_InitTypeDef EXTI_InitStruct;//stm32f10x_exit.h
	GPIO_InitTypeDef GPIO_InitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//开启时钟，因为用的是第二功能，所以要开复用时钟AFIO
	//MPU中断引脚-PB5
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU;//GPIO_Mode_AF_PP//推挽输出
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5;//PB5配置为上拉输入
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStruct);	
	
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//外部中断和GPIO映射
	
	EXTI_InitStruct.EXTI_Line=EXTI_Line5;//中断线；5（EXTI_Init->assert_param()中的IS_EXTI_LINE->#define 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);
}

Exit.h

#ifndef  _EXTI_H
#define  _EXTI_H

#include "sys.h" 

void MPU6050_EXTI_Init(void);
#endif

4、motor控制

Motor.c

#include "motor.h"

//电机初始化函数
void Motor_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启时钟
	//电机1：PB12\PB13   电机2：PB14/PB15
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//初始化GPIO--PB12、PB13、PB14、PB15为推挽输出
	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);	
}
//电机是10kHz，如果分频是1，重装载值是7199，+1=7200，stm32f1就是70兆/7=100us，而100us对应的刚好是10kHZ，刚刚好对应电机频率
//
//int PWM_MAX=7200,PWM_MIN= -7200;//放主函数了main
//限幅函数
void Limit(int *motoA,int *motoB)
{
	if(*motoA>PWM_MAX)*motoA=PWM_MAX;//限制在最小和最大区间
	if(*motoA<PWM_MIN)*motoA=PWM_MIN;
	
	if(*motoB>PWM_MAX)*motoB=PWM_MAX;
	if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}

//绝对值函数（返回正值）
int GFP_abs(int p)
{
	int q;
	q=p>0?p:(-p);//>0返回p，<0返回-p
	return q;
}

//赋值函数
//将PWM写进电机
//入口参数：PID运算完成后的最终PWM值
void Load(int moto1,int moto2)//假设moto1=-200：负号表示反转200个脉冲
{
	//1.研究正负号，对应正反转（极性，给逻辑电平）
	if(moto1>0)	
		Ain1=1,Ain2=0;//左电机正转
	else 		
		Ain1=0,Ain2=1;//左电机反转
	//2.研究PWM值（不需要符号，所以要设置一个绝对值函数来取正）
	TIM_SetCompare1(TIM1,GFP_abs(moto1));
	
	//右电机同理
	if(moto2>0)	
		Bin1=1,Bin2=0;
	else 		
		Bin1=0,Bin2=1;	
	TIM_SetCompare4(TIM1,GFP_abs(moto2));
}

//后期
char PWM_Zero=0,stop=0;
void Stop(float *Med_Jiaodu,float *Jiaodu)
{
	if(GFP_abs(*Jiaodu-*Med_Jiaodu)>60)
	{
		Load(PWM_Zero,PWM_Zero);
		stop=1;
	}
}

Motor.h

#ifndef  _MOTOR_H
#define  _MOTOR_H

#include "sys.h" 

//宏定义几个位带函数
#define Ain1  PBout(14)
#define Ain2  PBout(15)

#define Bin1  PBout(13)/*【4】*/
#define Bin2  PBout(12)



void Stop(float *Med_Jiaodu,float *Jiaodu);
void Motor_Init(void);/*【5】*/
void Limit(int *motoA,int *motoB);
int GFP_abs(int p);
void Load(int moto1,int moto2);
#endif

------------------------------------------------------------------------------------------------------------------------------

------------------------------------------------------------------------------------------------------------------------------

第二部分

（链接可能会失效，在我的主页有这篇）

PID原理

（之前在知乎看到的一篇文章做了笔记）

工程中P必存在，在P的基础上又有PD、PI、PID控制器。

1、三项作用：

比例项P：提高响应速度，减少稳态误差（Kp太小了，Kp太大时会出现震荡） 

积分项I：消除稳态误差

微分项D：减少震荡以及超调

稳态误差（Kp太小了，距离目标值有一定的距离）

震荡（Kp太大了，在目标值震荡）

2、各项作用解释：

比例项：提高响应速度->上式可见，I和D原本的式子就含有Kp，所以比例项稍微提高一点，这三项都会有一个提升，输出就会比较大的变化，大幅增大，提高了响应速度。稳态误差就是Kp太小导致的，增大Kp即可减少。

积分项：只要有偏差，就会积分，直至偏差变为0。消除稳态误差

微分项：减少震荡和超调，可以理解为"刹车"（单摆中的阻尼），给式子加了一个负数，负数越大，减少的震荡越明显。

3、PID函数（位置式PID）

位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。 

e(k)：本次偏差--------------------------------------

e(n)：e(k)以及以前的偏差的累积和，其中k=1，2，......,k------------------------

e(k-1)：上一次误差---------------------------------

（PWM相当于u（k），代表输出） ------------Pwm

Bias偏差，Pwm输出，Integral_bias偏差积分，Last_Bias上一次偏差

​
int Position_PID(int Encoder,int Target)
{
    static float Bias,Pwm,Integral_bias,Last_Bias;
    Bias=Encoder-Target; //计算偏差
    Integral_bias+=Bias;//求出偏差的积分（和）
    //位置式PID控制器
    Pwm=Position_KP*Bias + Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);
    Last_Bias=Bias;//保存上一次偏差
    return Pwm;//输出
}

​

串级控制系统(两个PID串起来)

 

1、直立环

(1)输入：1、给定角度（速度环输出），2、角度反馈

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

//直立PD控制器输出=Kp1*(真实角度-期望角度)+Kd*角度偏差的微分

//其中角度偏差=真实角度-期待角度

Vertical_out=Kp1*(real_Angle-expect_Angle)+Kd*D(real_Angle-expect_Angle)

2、速度环

(1)输入：1、给定速度，2、速度反馈

(2)输出：角度值（直立环的期望角度输入）

//速度PI控制器输出=Kp2*(反馈编码器值-期望编码器值)+Ki*编码器偏差的积分

//其中编码器偏差=反馈编码器值-期望编码器值

Velocity_out=Kp2*(Encoder_real-Encoder_expect)+Ki*S(Encoder_real-Encoder_expert) 

（备注：速度环输出=直立环的期待角度、Kp1:直立环Kp、Kp2:速度环Kp）

显而易见，红色两项就为直立环式子，只不过角度偏差改成了真实角度，蓝色两项就为速度环式子，只不过多乘了Kp1。并不是真正的直立环和速度环。

编程就按照该式子编写即可。 

直立环PD（内环）

目的让小车角度趋向于0

小车往哪边倒，车轮就往哪边开，保持车平衡（手持棒）

P是必有的，这里用PD比例微分，不用到积分I

大小与角度成正比、方向与角速度成正比的回复力--------P

大小与角速度成正比、方向与回复力成反比的阻尼力-----D

比例微分控制：PD(out)=Kp*Angle+Kd*(Angle-Angle_last)

即a=b1*+b2*

速度环PI（外环）

目的：让电机速度趋近于0

转向环（无，用了z轴的角速度的系数做了转向环的输出）

平衡小车的控制理论和控制过程的编程

第三部分

PID的调参