STM32两轮自平衡小车——PID

代码

#include "control.h"

float Med_Angle = -1.4;		//机械中值（需调）
float 
	Vertical_Kp = -348,			//直立环Kp（需调）			
	Vertical_Kd = -0.9;			//直立环Kd（需调）
float 
	Velocity_Kp = 0.35,			//速度环Kp（需调）
	Velocity_Ki = 0.00175;		//速度环Ki（需调）

float 
	Turn_Kp=1;					//转向环Kp（需调）

int Vertical_out,Velocity_out,Turn_out;			//直立环&速度环&转向环输出变量

void EXTI9_5_IRQHandler(void)
{
	if(EXTI_GetITStatus(EXTI_Line5)!=0)				//一级判定
	{
		int PWM_out;
		if(PBin(5)==0)								//二级判定
		{
			EXTI_ClearITPendingBit(EXTI_Line5);		//清除中断标志位
			/*步骤1.采集编码器数据&MPU6050角度信息*/
			Encoder_Left = -Read_Speed(2);
			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.将数据带入直立环&速度环&转向环，计算出控制输出量
			Vertical_out = Vertical(Med_Angle,Pitch,gyroy);
			Velocity_out = Velocity(Encoder_Left,Encoder_Right);
			Turn_out = Turn(gyroz);
			PWM_out = Vertical_out - Vertical_Kp*Velocity_out;
/*****************************************************/			
			//3.将控制输出量加载至电机
			MOTO1 = PWM_out-Turn_out;				//电机1
			MOTO2 = PWM_out+Turn_out;				//电机2
			Limit(&MOTO1,&MOTO2);					//PWM限幅
			Load(MOTO1,MOTO2);						//加载至电机
		}
	}
}

/****************
直立环PD:Kp*Ex+Kd*Ex_D
入口参数：Med真实角度，Angle期望角度，gyro_Y真实角速度
出口参数：直立环输出
****************/
int Vertical(float Med,float Angle,float gyro_Y)
{
	int PWM_out;
	
	PWM_out = Vertical_Kp*(Angle-Med) + Vertical_Kd*(gyro_Y-0);
	return PWM_out;
}

/****************
速度环PI:Kp*Ek+Ki*Ek_s
****************/
int Velocity(int encoder_left,int encoder_right)
{
	static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
	float a = 0.7;
	
	//1.计算速度偏差
	Encoder_Err = (encoder_left+encoder_right)-0;	//这里左右电机之和没有除以2是为了避免舍去误差
	//2.对速度误差进行低通滤波
	//low_out=(1-a)*Ek+a*low_out_last;
	EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last;
	EnC_Err_Lowout_last = EnC_Err_Lowout;
	//3.对速度误差积分，积分出位移
	Encoder_S += EnC_Err_Lowout;
	//4.积分限幅
	Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
	//5.速度环控制积分计算
	PWM_out = Velocity_Kp*EnC_Err_Lowout + Velocity_Ki*Encoder_S;
	
	return PWM_out;
}

/****************
转向环：Kp*gyro_Z
****************/ 
int Turn(int gyro_Z)
{
	int PWM_out;
	
	PWM_out = Turn_Kp*gyro_Z;
	
	return PWM_out;
}

调参

直立环
屏蔽速度环&转向环

1.机械中值
小车本身尽可能不倒下的Pitch值。

2.Kp极性（正负）
极性正确：小车呈直立趋势。
极性错误：小车加速倒下。

3.Kp大小
数值从小到大尝试，直到小车出现大幅低频抖动。

4.Kd极性（正负）
屏蔽直立环的Kp
极性正确：小车轮胎与小车转动方向相同。
极性错误：小车轮胎与小车转动方向相反。

5.Kd大小
加入直立环Kp
数值从小到大尝试，直到小车出现高频振荡。

Kp与Kd最终数值=0.6✖振荡数值。
速度环
Ki=1/200Kp*
1.Kp极性
屏蔽直立环&转向环
极性正确：旋转小车其中一个轮子，两轮同向转动，直至转速最大。
极性错误：旋转小车其中一个轮子，另一轮子反向转动。

2.Kp大小
加入直立环
数值从小到大尝试，直到小车走直线性能优秀，回位效果良好。

转向环
屏蔽直立环&速度环