PID算法Word格式文档下载.docx
《PID算法Word格式文档下载.docx》由会员分享，可在线阅读，更多相关《PID算法Word格式文档下载.docx（13页珍藏版）》请在冰点文库上搜索。
*pvand*spareintegerpointers.
voidpid_init（struct_pid*warm,intprocess_point,intset_point）
struct_pid*pid;
pid=warm;
pid->
pv=process_point;
sp=set_point;
}
//－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－pid_tuneDESCRIPTIONSetstheproportionalgain（p_gain）,integralgain（i_gain）,
derivitivegain（d_gain）,andthedeadband（dead_band）ofapidcontrolstructure_pid.
设定PID参数－－－－P,I,D,死区
voidpid_tune（struct_pid*pid,floatp_gain,floati_gain,floatd_gain,intdead_band）
pgain=p_gain;
igain=i_gain;
dgain=d_gain;
deadband=dead_band;
integral=integral_val;
last_error=0;
pid_setintegDESCRIPTIONSetanewvaluefortheintegraltermofthepidequation.
Thisisusefulforsettingtheinitialoutputofthepidcontrolleratstartup.
设定输出初始值
voidpid_setinteg（struct_pid*pid,floatnew_integ）
integral=new_integ;
last_error=0;
pid_bumplessDESCRIPTIONBumplesstransferalgorithim.
Whensuddenlychangingsetpoints,orwhenrestartingthePIDequationafteranextendedpause,
thederivativeoftheequationcancauseabumpinthecontrolleroutput.Thisfunctionwillhelpsmoothoutthatbump.
Theprocessvaluein*pvshouldbetheupdatedjustbeforethisfunctionisused.
pid_bumpless实现无扰切换
当突然改变设定值时，或重新启动后，将引起扰动输出。
这个函数将能实现平顺扰动，在调用该函数之前需要先更新PV值
voidpid_bumpless（struct_pid*pid）
last_error=（pid->
sp）-（pid->
pv）;
//设定值与反馈值偏差
pid_calcDESCRIPTION
PerformsPIDcalculations
forthe_pidstructure*a.
Thisfunctionusesthepositionalformofthepidequation,andincorporatesanintegralwinduppreventionalgorithim.
Rectangularintegrationisused,sothisfunctionmustberepeatedonaconsistenttimebasisforaccuratecontrol.
RETURNVALUEThenewoutputvalueforthepidloop.USAGE#include"
control.h"
本函数使用位置式PID计算方式，并且采取了积分饱和限制运算
PID计算
floatpid_calc（struct_pid*pid）
interr;
floatpterm,dterm,result,ferror;
//计算偏差
err=（pid->
sp）-（pid->
//判断是否大于死区
if（abs（err）>
pid->
deadband）
ferror=（float）err;
//dointegertofloatconversiononlyonce数据类型转换
//比例项
pterm=pid->
pgain*ferror;
if（pterm>
100||pterm<
-100）
integral=0.0;
else
//积分项
integral+=pid->
igain*ferror;
//输出为0－－100%
//如果计算结果大于100，则等于100
if（pid->
integral>
100.0）
integral=100.0;
//如果计算结果小于0.0，则等于0
elseif（pid->
integral<
0.0）
//微分项
dterm=（（float）（err-pid->
last_error））*pid->
dgain;
result=pterm+pid->
integral+dterm;
result=pid->
integral;
//在死区范围内，保持现有输出
//保存上次偏差
last_error=err;
//输出PID值（0-100）
return（result）;
voidmain（void）
floatdisplay_value;
intcount=0;
pid=&
warm;
//printf（"
EnterthevaluesofProcesspoint,Setpoint,Pgain,Igain,Dgainn"
）;
//scanf（"
%d%d%f%f%f"
&
process_point,&
set_point,&
p_gain,&
i_gain,&
d_gain）;
//初始化参数
process_point=30;
set_point=40;
p_gain=（float）（5.2）;
i_gain=（float）（0.77）;
d_gain=（float）（0.18）;
dead_band=2;
integral_val=（float）（0.01）;
printf（"
ThevaluesofProcesspoint,Setpoint,Pgain,Igain,Dgainn"
%6d%6d%4f%4f%4fn"
process_point,set_point,p_gain,i_gain,d_gain）;
EnterthevaluesofProcesspointn"
while（count<
=20）
scanf（"
%d"
&
process_point）;
//设定PV,SP值
pid_init（&
warm,process_point,set_point）;
//初始化PID参数值
pid_tune（&
warm,p_gain,i_gain,d_gain,dead_band）;
//初始化PID输出值
pid_setinteg（&
warm,0.0）;
//pid_setinteg（&
warm,30.0）;
//Getinputvalueforprocesspoint
pid_bumpless（&
warm）;
//howtodisplayoutput
display_value=pid_calc（&
%fn"
display_value）;
//printf（"
n%f%f%f%f"
warm.pv,warm.sp,warm.igain,warm.dgain）;
count++;
飞控代码
intmain（void）
////////////////////////////////////////
//变量定义
//10轴传感器初始化结构体
structTen_Axis_Sensors_Init_sTen_Axis_Init_Structure;
//当前角度及期望角度
floatcurrent_attitude[2],current_heading,
desired_attitude[2]={0.f},desired_heading={0.f},desired_thro=1000.f;
//PID参数
floata_p_gain=4.08f,a_i_gain=0.f,a_d_gain=238.f,
h_p_gain=2.f,h_i_gain=0.f,h_d_gain=235.f;
//PID过程变量
floatlast_attitude[2]={0.f},last_heading=0.f,
attitude_int_value[2]={0.f},heading_int_value=0.f,
error;
//PID调整量
floatattitude_trim[2],heading_trim;
//控制量
uint16_tthro[4];
//状态变量
uint8_toffline=0,sbus_rtn,n;
//函数声明
voidSysTick_delay（__IOuint32_tnTime）;
//配置所有外设
Horizon_Configuration（）;
//Application
//给电调一个持续的低油门信号
SysTick_delay（4000）;
//初始化S.Bus缓冲区
sbus_set_buffer_en
（1）;
//初始化10轴传感器
Ten_Axis_Init_Structure.MPU_SMPRT=200;
Ten_Axis_Init_Structure.MPU_LPF_DEF=MPU_LPF_GYRO_BW_98HZ;
Ten_Axis_Init_Structure.MPU_GYRO_FSR_DEF=MPU_GYRO_FSR_2000DPS;
Ten_Axis_Init_Structure.MPU_ACCEL_FSR_DEF=MPU_ACCEL_FSR_2G;
Ten_Axis_Init_Structure.MPU_GYRO_CALIB_EN=1;
Ten_Axis_Init_Structure.MPU_ACCEL_CALIB_EN=1;
ten_axis_sensors_init（&
Ten_Axis_Init_Structure）;
//开始接收上位机指令
usart_receive（UPPER_USART,2,G_upper_cmd）;
while
（1）
{
while（!
G_ten_axis_data_calculable）;
ten_axis_data_norm（）;
////////////////////
//解姿态
if（G_Ten_Axis_Data_Structure.mag_available）
MadgwickAHRSupdate（G_Ten_Axis_Data_Structure.gyro[0]*0.01745329f,G_Ten_Axis_Data_Structure.gyro[1]*0.01745329f,G_Ten_Axis_Data_Structure.gyro[2]*0.01745329f,
G_Ten_Axis_Data_Structure.accel[0],G_Ten_Axis_Data_Structure.accel[1],G_Ten_Axis_Data_Structure.accel[2],
G_Ten_Axis_Data_Structure.mag[0],G_Ten_Axis_Data_Structure.mag[1],G_Ten_Axis_Data_Structure.mag[2]）;
}
else
MadgwickAHRSupdateIMU（G_Ten_Axis_Data_Structure.gyro[0]*0.01745329f,G_Ten_Axis_Data_Structure.gyro[1]*0.01745329f,
G_Ten_Axis_Data_Structure.gyro[2]*0.01745329f,G_Ten_Axis_Data_Structure.accel[0],
G_Ten_Axis_Data_Structure.accel[1],G_Ten_Axis_Data_Structure.accel[2]）;
current_attitude[0]=-atan2f（2.f*（q0*q1+q2*q3）,1.f-2.f*（q1*q1+q2*q2））*57.29578f;
current_attitude[1]=-asinf（2.f*（q0*q2-q3*q1））*57.29578f;
current_heading=atan2f（2.f*（q0*q3+q1*q2）,1.f-2.f*（q2*q2+q3*q3））*57.29578f;
if（current_heading<
0.f）
current_heading=-current_heading;
current_heading=360-current_heading;
//读遥控器信号
sbus_rtn=sbus_frame_analy（）;
if（!
sbus_rtn&
&
!
（G_sbus_flag&
0xC））
offline=0;
desired_thro=（float）（1684-G_sbus_channel[2]）/1320.f*（float）（MAX_THRO_US-1000）+1000.f;
desired_attitude[0]=-（float）（G_sbus_channel[0]-1024）/1024.f*MAX_ATTITUDE_DEG_F;
desired_attitude[1]=（float）（G_sbus_channel[1]-1024）/1024.f*MAX_ATTITUDE_DEG_F;
//desire_angle[2]=tobecontinue...
elseif（sbus_rtn!
=1）
offline=1;
//PID控制
offline）
//姿态控制
for（n=0;
n<
2;
n++）
error=desired_attitude[n]-current_attitude[n];
attitude_int_value[n]+=error*a_i_gain;
if（attitude_int_value[n]>
ATTI_INT_LIMIT_US_F）
attitude_int_value[n]=ATTI_INT_LIMIT_US_F;
elseif（attitude_int_value[n]<
-ATTI_INT_LIMIT_US_F）
attitude_int_value[n]=-ATTI_INT_LIMIT_US_F;
attitude_trim[n]=（error*a_p_gain）+attitude_int_value[n]-（（current_attitude[n]-last_attitude[n]）*a_d_gain）;
last_attitude[n]=current_attitude[n];
//航向控制
error=relative_error（desired_heading,current_heading）;
heading_int_value+=error*h_i_gain;
if（heading_int_value>
HEAD_INT_LIMI_US_F）
heading_int_value=HEAD_INT_LIMI_US_F;
elseif（heading_int_value<
-HEAD_INT_LIMI_US_F）
heading_int_value=-HEAD_INT_LIMI_US_F;
heading_trim=（error*h_p_gain）+heading_int_value-（relative_error（current_heading,last_heading）*h_d_gain）;
last_heading=current_heading;
//tobecontinue...
//限制最大控制量
if（attitude_trim[n]>
MAX_ATTI_TRIM_US_F）
attitude_trim[n]=MAX_ATTI_TRIM_US_F;
elseif（attitude_trim[n]<
-MAX_ATTI_TRIM_US_F）
attitude_trim[n]=-MAX_ATTI_TRIM_US_F;
if（heading_trim>
MAX_HEAD_TRIM_US_F）
heading_trim=MAX_HEAD_TRIM_US_F;
elseif（heading_trim<
-MAX_HEAD_TRIM_US_F）
heading_trim=-MAX_HEAD_TRIM_US_F;
//计算最终油门量（期望油门、姿态控制、航向控制的总和）
thro[0]=desired_thro+attitude_trim[0]+heading_trim;
thro[1]=desired_thro-attitude_trim[0]+heading_trim;
thro[2]=desired_thro+attitude_trim[1]-heading_trim;
thro[3]=desired_thro-attitude_trim[1]-heading_trim;
//限制油门范围
4;
if（thro[n]>
MAX_THRO_US）
thro[n]=MAX_THRO_US;
elseif（thro[n]<
1000）
thro[n]=1000;
//更新油门量
TIM_SetCompare1（TIM3,thro[0]）;
TIM_SetCompare2（TIM3,thro[1]）;
TIM
升级会员 