|
想找四轴的串级pid程序,没有找到,就照着资料写了一个,希望各位前辈帮我指正一下错误,就是很初级的一个程序。程序目的是:翻滚角的串级pid控制,也希望各位前辈能分享一些串级pid或者四轴姿态控制的pid能让我学习一下,谢谢啦。
/***************************************************************************************************
***_tar:遥控器目标角度
roll/float/pitch:当前角度值
***_speed:当前角速度值
***************************************************************************************************/
void pid_control(float roll_tar,float pitch_tar,float yaw_tar,float roll,float pitch,float yaw,float roll_speed,float pitch_speed,float yaw_speed)
{
///////////////////////////////roll角度外环·////////////////////////////////
roll_pid.err = roll_tar - roll ; //偏差
roll_pid.P_out = KP_ROLL * roll_pid.err ;//p项
roll_pid.D_out = KD_ROLL * angle_speed_data[0] ;//D
roll_pid.I_out = KI_ROLL * roll_pid.err + roll_I_last ;//I
roll_pid.I_out=CH_limit(roll_pid.I_out,ROLL_I_min,ROLL_I_max);//限幅
roll_I_last=roll_pid.I_out;
roll_pid.out = roll_pid.P_out - roll_pid.D_out + roll_pid.I_out ;
///////////////////////////////roll角速度内环·/////////////////////////////////
roll_speed_pid.err=roll_pid.out-roll_speed//
roll_speed_pid.P_out=KP_ROLL_SPEED*roll_speed_pid.err;
roll_speed_pid.D_out = KD_ROLL_SPEED * (roll_speed_pid.err-roll_speed_pid.last_err);
roll_speed_pid.last_err=roll_speed_pid.err;
roll_speed_pid.I_out = KI_ROLL_SPEED * roll_pid_speed.err + roll_speed_I_last ;
roll_speed_pid.I_out=CH_limit(roll_speed_pid.I_out,ROLL_speed_I_min,ROLL_speed_I_max);
roll_speed_I_last=roll_speed_pid.I_out;
}
|
阿莫论坛20周年了!感谢大家的支持与爱护!!
如果天空是黑暗的,那就摸黑生存;
如果发出声音是危险的,那就保持沉默;
如果自觉无力发光,那就蜷伏于牆角。
但是,不要习惯了黑暗就为黑暗辩护;
也不要为自己的苟且而得意;
不要嘲讽那些比自己更勇敢的人。
我们可以卑微如尘土,但不可扭曲如蛆虫。
|