思路
内环:(速度环)
外环:(角度环)
串级:内环加外环
程序:(只介绍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发送到电机,闭环结束