RM-串级PID

思路

内环：（速度环）

外环：（角度环）

串级：内环加外环

程序：（只介绍yaw角的串级方法，俯仰角同理）

先看freertos.c
只用到两个线程：

第一个线程的作用是吧电流值发给电机让他转起来
第二个线程是主程序
（至于为什么can要单独一个线程发送后面会解释）

第二个线程里面是这样的：
此处基本就是步兵的全部代码了，包含底盘解算，发射机构，遥控器f发来的信息，各种裁判系统闭环，但是云台不在这里，所以这里不是本文的重点。、

云台其实是放在中断内的，这样做可以控制pid控制频率

可以看到我配置的是2ms触发一次中断，也就是每隔2毫秒就运算一遍上文中的串级代码，而且算完的数据会迅速到第一个线程直接通过can发送到电机立马执行

这一段if是通过鼠标控制的云台

①：设置云台电机内环pid参数以及内环速度环

motor_pid[4].f_pid_reset(&motor_pid[4],in_XP,in_XI,in_XD,PID_Speed);//这里重新设置pid参数，而且in_XP这种被我设为全局变量，debug时候可以快速调参

②：taget_Yaw作为目标角度，注意这里是用累加的方法，把鼠标输出的速度值mouseX累加形成一个位置量taget_Yaw

taget_Yaw=taget_Yaw-mouseX*mouseX_speed;//taget_Yaw就是目标yaw角，yaw参数就是当前yaw角

③：限制累加值在0-360度之间

if     (taget_Yaw>max_angle)taget_Yaw=taget_Yaw-max_angle; //鼠标x轴移动  限值
else if (taget_Yaw<0)taget_Yaw=max_angle+taget_Yaw; //鼠标x轴移动  限值

④：这里三句话的作用是外环pid，但是我们需要增加一个跟内环的权重比（权重比在串级pid中非常重要！）

Xnum：一个标志位，用来计数用的
n：具体权重比，程序中我给的是3（具体效果可以再仔细调整，不一定是最优参数）
Xerr：这个变量是外环pid的输出值，意义是一个速度量，作为内环pid的目标值输入
Yaw：陀螺仪输出的yaw角（频率为100hz，10ms发送一次数据）

	Xnum++;
	if(Xnum>n){Xnum=0;	Xerr=Xyaw_to_tageyaw(Yaw,taget_Yaw);}
		ch[4]=Xerr;

程序运行思路：每隔2ms进入一次中断—每次进入中断Xnum都会记录进入的次数，当产生了3次中断，也就是过了2ms*3=6ms之后，运行一次Xyaw_to_tageyaw函数，也就是外环运行一次，这里权重比为3.

这里小提一下Xyaw_to_tageyaw（）函数的内容：

float Xyaw_to_tageyaw(float now,float target)
{
	 static int err=0;
	float out_put;
	 err=target-now;                //计算偏差
	 
		if( err>(max_angle/2)){  err= err-max_angle;}//解决360度问题
	else if( err<-(max_angle/2)){ err= err+max_angle;}
	
 out_put=Xp/100*err-Xd/100*(moto_chassis[4].speed_rpm);   //PI控制器
	if(out_put>max_output)out_put=max_output;
	else if(out_put<-max_output)out_put=-max_output;
	 return out_put;                         //输出
	
}

其中if语句是用来判断限位的

注意点：

一：

重点在于pid输出那一句：
out_put=Xp/100err-Xd/100(moto_chassis[4].speed_rpm); //PI控制器
在这里如果直接采用xp那么他的数值会非常小，在1-1.5之间非常难调整，所以我会用缩小100倍的方法进行调试，效果会很好
二：
注意到微分运算xd的可能会有疑问，为什么会用系数xd乘以电机速度值，网上pid中的公式不是xd*（err-lasterr）吗？
其实这个问题弄明白非常关键！
如果真正去推倒过公式的会发现（err-lasterr）/6ms就等于一个速度，路程/时间=速度，而且经过推到你也可以发现，这个值换成电机速度值 完全可以达到消除震荡的作用，速度快的时候，微分项就大，输出就减小（这一块建议去弄明白，想办法推到一下pid的公式或者找找视频也好，事实上我测试过，微分项直接给电机速度值效果会非常好，因为err-lasterr永远等于0或者1，测试过就知道了）

⑤：最后一步，将外环输出的值作为内环速度目标值输入内环：

内环pid计算：

内环最后输出的output作为云台电机的电流值被第一个线程的can发送到电机，闭环结束