串级PID算法

单环PID和串级PID的区别（框图表示）

串级PID的优势：增加稳定性，增强抗干扰性（其实不是太理解。。。）

代码实现

定义4个PID的结构体，分别用于x方向内环、外环以及y方向内环、外环（当初做板球系统xy方向用的都是同一个PID结构体变量，最后居然能调出来，惊了。。。PID真是玄学）
（一）结构体变量

//定义PID结构体
typedef struct
{
	float ek;
	float ek1;
	float esum;
	float kp;
	float ki;
	float kd;
} PID_StructureDef;

//需要开x，y两个方向的pid结构体
PID_StructureDef PID_shell_Structure_x={0,0,0,0,0,0};	//x外环pid结构体
PID_StructureDef PID_core_Structure_x={0,0,0,0,0,0};	//x内环pid结构体

PID_StructureDef PID_shell_Structure_y={0,0,0,0,0,0};	//y外环pid结构体
PID_StructureDef PID_core_Structure_y={0,0,0,0,0,0};	//y内环pid结构体

（二）PID结构体变量的参数设置函数

//设置PID结构体的初始化参数
void Set_PID_para(float kp,float ki,float kd,PID_StructureDef *PID_Structure)
{
	PID_Structure->kp=kp;
	PID_Structure->ki=ki;
	PID_Structure->kd=kd;
}

（三）PID输出计算函数，输入的是外环的误差，返回的是内环的输出

//PID计算输出
int PID_Calculate(float error,float gyro,PID_StructureDef *PID_shell_Structure,PID_StructureDef *PID_core_Structure)
{
	float shell_output,core_output;
	
	PID_shell_Structure->ek=error;
	//积分限幅
	if(PID_shell_Structure->esum>300)
		PID_shell_Structure->esum=300;
	else if(PID_shell_Structure->esum<-300)
		PID_shell_Structure->esum=-300;
	else
		PID_shell_Structure->esum+=error;
	shell_output=(PID_shell_Structure->ek)*(PID_shell_Structure->kp)+(PID_shell_Structure->esum)*(PID_shell_Structure->ki);
	
	//外环输出，作为内环输入 用陀螺仪当前的角速度作为实际值
	PID_core_Structure->ek=shell_output-gyro;
	//内环积分限幅
	if(PID_core_Structure->esum>500)
		PID_core_Structure->esum=500;
	else if(PID_core_Structure->esum<-500)
		PID_core_Structure->esum=-500;
	else
		PID_core_Structure->esum+=PID_core_Structure->ek;
	core_output=(PID_core_Structure->ek)*(PID_core_Structure->kp)+(PID_core_Structure->esum)*(PID_core_Structure->ki)+(PID_core_Structure->ek-PID_core_Structure->ek1)*(PID_core_Structure->kd);
	PID_core_Structure->ek1=PID_core_Structure->ek;
	
	return (int)core_output;
}

（四）使用的时候，输入角度误差，以及陀螺仪当前的角速度

//计算当前姿态的坐标x和y,并实现控制
void Calculate_Control(void)
{
	float temp_pitch,temp_roll,pid_x,pid_y;
	//获取陀螺仪的三个角速度
	MPU_Get_Gyroscope(&GRY_x,&GRY_y,&GRY_z);
	
	//计算误差值 以及PID xy坐标换算到角度，利用串级pid控制
	temp_pitch=atan2(Height,Goal_X)-pitch;
	pid_x=PID_Calculate(temp_pitch,GRY_x,&PID_shell_Structure_x,&PID_core_Structure_x);
	if(pid_x)
	{
		motor_1=0;
		motor_3=pid_x;
	}
	else
	{
		motor_3=0;
		motor_1=-pid_x;
	}
	
	temp_roll=atan2(Height,Goal_Y)-roll;
	pid_y=PID_Calculate(temp_roll,GRY_y,&PID_shell_Structure_y,&PID_core_Structure_y);
	if(pid_y)
	{
		motor_4=0;
		motor_2=pid_y;
	}
	else
	{
		motor_2=0;
		motor_4=-pid_y;
	}
}

注：代码虽然过了编译，但由于硬件出了问题，代码能否实现功能还不知道。。。

串级PID调参技巧

Reference:https://blog.csdn.net/nemol1990/article/details/45131603