PID角度控制代码

假定初态角度已知，并可以实时测量角速度（一阶量），控制量为角加速度（二阶量），控制目标为角度（原变量）为0#includestdio.h#includemath.h#defineT0.01#definepi3.1416#defineAIM0#defineKP0.1#defineKI0.1#defineKD0.0048typedefstructState系统状态，包括原变量，一阶量，二阶量。此处为真实状态，区别于下面控制器中计算得到的状态，并且除了角加速度外其他量不能传递给外界{doubletheta;doubleomega;doublealpha;}s,ps;voidinit_system(psp,doubletheta0)系统初始化，初始角加速度，角速度为零，初始角度为theta0{p-theta=theta0;p-omega=0;p-alpha=0;}doublesensor(sstate)模拟传感器环节，从系统中得到角速度并返回，这里省略测量环节{doublefeedback;feedback=state.omega;returnfeedback;}voidstate_calculate(psp,doublefeedback)此处的一组状态量是根据传感器返回的角加速度进行积分微分得到角度和角加速度，并不直接与系统真实状态发生信息传递，模拟真实情况下控制器中记录的状态，并不一定与真实状态相同{p-alpha=(feedback-p-omega)T;p-theta+=p-omegaT+p-alphaTT2;p-omega=feedback;}doublepid_calculate(sstate)控制器解算了当前系统状态后利用pid算法计算修正量{doublekp=KP,ki=KI,kd=KD;doubledifferential;staticdoublelastpoint=0,integration;doublefix_angle,fix;doubleerror;error=AIM-state.theta;differential=(error-lastpoint)T;integration+=error;lastpoint=error;fix_angle=kperror+kiintegration+kddifferential;fix=2fix_angleTT-2state.omega-state.alpha;returnfix;}voidadjust_system(psp,doublefix)系统根据控制器给出的修正值进行修正，调整状态。此处改变的是真实状态{p-alpha+=fix;p-theta+=p-omegaT+p-alphaTT2;p-omega+=p-alphaT;}voiddisturb_system(psp,intn)模拟外力对系统的干扰，在特定时间内对角加速度产生干扰{if(n100&&n200)p-alpha+=1000;}voidrun_system(psp,intn)p所指向的系统运行，n为运行时间（10ms为单位）{staticssimd_state;运行过程中存储在控制器中模拟的系统状态doublefeedback,fix;运行过程中传递于各模块之间的中间变量if(n==1)simd_state=p;将系统真实的初始状态赋给控制器中的模拟状态，此后两者之间不会直接发生信息的交换printf(%.2fs%7.4f%5.2f,(float)n100,p-theta,simd_state.theta);printf(%.2fs%7.4f,(float)n100,p-alpha);printf(%.2fs%7.4f,(float)n100,p-theta);if(n%5==0)printf(n);elseprintf();feedback=sensor(p);传感器直接从系统真实状态读出角速度作为反馈值state_calculate(&simd_state,feedback);根据传感器的反馈值，系统计算当期系统的状态赋给当前的模拟状态fix=pid_calculate(simd_state);根据解算的模拟状态给出修正值disturb_system(p,n);外力对系统进行干扰adjust_system(p,fix);根据控制器给出的修正值系统的真实状态改变}voidmain(){intn,time=3;系统运行时间,time单位是s，n单位是10mssactual_state;真实的系统init_system(&actual_state,-pi12);系统初始化for(n=1;n=time100;n++)系统运行run_system(&actual_state,n);}PID参数调试结果kpkikd收敛时间（0.01rad）smax0.010.0050.0053.9-0.39520.020.0050.0052.0-0.30160.030.0050.0051.4-0.23050.040.0050.0051.0-0.17750.10.0050.0050.6-0.07410.3360.0050.005发散0.10.050.0050.4-0.30160.10.10.0050.3-0.04300.10.20.0051.80.11910.10.20.08发散0.10.20.05发散0.10.10.0040.4-0.37900.10.10.0040.4-0.37900.10.10.00520.29-0.10.10.00510.28-0.10.10.00500.28-0.10.10.00490.28-0.10.10.00480.27-0.10.10.00470.28-